3

I am trying to implement Kalman filter. I only know the positions. The measurements are missing at some time steps. This is how I define my matrices:

Process noise matrix

Q = np.diag([0.001, 0.001])

Measurement noise matrix

R = np.diag([10, 10])

Covariance matrix

P = np.diag([0.001, 0.001])

Observation matirx

H = np.array([[1.0, 0.0], [0.0, 1.0]])

Transition matrix

F = np.array([[1, 0], [0, 1]])

state

x = np.array([pos[0], [pos[1]])

I dont know if it is right. For instance, if I see target at t=0 and dont see at t = 1, how will I predict its position. I dont know the velocity. Are these matrix defintion correct?

Lalu
  • 137
  • 9

1 Answers1

3

You need to expand your model and add states for the velocity (and if you want for the acceleration). The filter will estimate the new states based on the position and use them to predict position even if you don't have position measurements.

Your matrices would look something like this:

Process noise matrix

Q = np.diag([0.001, 0.001, 0.1, 0.1, 0.1, 0.1]) #enter correct numbers for vel and acc

Measurement noise matrix stays the same

Covariance matrix

P = np.diag([0.001, 0.001, 0.1, 0.1, 0.1, 0.1]) #enter correct numbers for vel and acc

Observation matrix

enter image description here

H = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0, 0.0, 0.0]])

Transition matrix

enter image description here

F = np.array([[1, 0, dt,  0, 0.5*dt**2,         0], 
              [0, 1,  0, dt,         0, 0.5*dt**2], 
              [0, 0,  1,  0,        dt,         0],
              [0, 0,  0,  1,         0,        dt],
              [0, 0,  0,  0,         1,         0],
              [0, 0,  0,  0,         0,         1]])

State

enter image description here

Have a look at my old post with a very similar problem. In that case there was only a measurement for the acceleration and the filter estimated position and velocity as well.

Using PyKalman on Raw Acceleration Data to Calculate Position

In the following post one had to predict position as well. The model consisted only of two positions and two velocities. You can find the matrices in the python code there.

Kalman filter with varying timesteps

UPDATE

Here is my matlab example to show you the state estimation for velocity and acceleration only from the position measurements:

function [] = main()
    [t, accX, velX, posX, accY, velY, posY, t_sens, posX_sens, posY_sens, posX_var, posY_var] = generate_signals();

    n = numel(t_sens);

    % state matrix
    X = zeros(6,1);

    % covariance matrix
    P = diag([0.001, 0.001,10, 10, 2, 2]);

    % system noise
    Q = diag([50, 50, 5, 5, 3, 0.4]);

    dt = t_sens(2) - t_sens(1);

    % transition matrix
    F = [1, 0, dt,  0, 0.5*dt^2,        0; 
         0, 1,  0, dt,        0, 0.5*dt^2; 
         0, 0,  1,  0,       dt,        0;
         0, 0,  0,  1,        0,       dt;
         0, 0,  0,  0,        1,        0;
         0, 0,  0,  0,        0,        1]; 

    % observation matrix 
    H = [1 0 0 0 0 0;
         0 1 0 0 0 0];

    % measurement noise 
    R = diag([posX_var, posY_var]);

    % kalman filter output through the whole time
    X_arr = zeros(n, 6);

    % fusion
    for i = 1:n
        y = [posX_sens(i); posY_sens(i)];

        if (i == 1)
            [X] = init_kalman(X, y); % initialize the state using the 1st sensor
        else
            if (i >= 40 && i <= 58) % missing measurements between 40 ans 58 sec
                [X, P] = prediction(X, P, Q, F);
            else
                [X, P] = prediction(X, P, Q, F);
                [X, P] = update(X, P, y, R, H);
            end
        end

        X_arr(i, :) = X;
    end  

    figure;
    subplot(3,1,1);
    plot(t, posX, 'LineWidth', 2);
    hold on;
    plot(t_sens, posX_sens, '.', 'MarkerSize', 18);
    plot(t_sens, X_arr(:, 1), 'k.', 'MarkerSize', 14);
    hold off;
    grid on;
    title('PositionX');
    legend('Ground Truth', 'Sensor', 'Estimation');

    subplot(3,1,2);
    plot(t, velX, 'LineWidth', 2);
    hold on;
    plot(t_sens, X_arr(:, 3), 'k.', 'MarkerSize', 14);
    hold off; 
    grid on;
    title('VelocityX');
    legend('Ground Truth', 'Estimation');

    subplot(3,1,3);
    plot(t, accX, 'LineWidth', 2);
    hold on;
    plot(t_sens, X_arr(:, 5), 'k.', 'MarkerSize', 14);
    hold off;
    grid on;
    title('AccX');
    legend('Ground Truth', 'Estimation');


    figure;
    subplot(3,1,1);
    plot(t, posY, 'LineWidth', 2);
    hold on;
    plot(t_sens, posY_sens, '.', 'MarkerSize', 18);
    plot(t_sens, X_arr(:, 2), 'k.', 'MarkerSize', 14);
    hold off;
    grid on;
    title('PositionY');
    legend('Ground Truth', 'Sensor', 'Estimation');

    subplot(3,1,2);
    plot(t, velY, 'LineWidth', 2);
    hold on;
    plot(t_sens, X_arr(:, 4), 'k.', 'MarkerSize', 14);
    hold off; 
    grid on;
    title('VelocityY');
    legend('Ground Truth', 'Estimation');

    subplot(3,1,3);
    plot(t, accY, 'LineWidth', 2);
    hold on;
    plot(t_sens, X_arr(:, 6), 'k.', 'MarkerSize', 14);
    hold off;    
    grid on;
    title('AccY');
    legend('Ground Truth', 'Estimation');    

    figure;
    plot(posX, posY, 'LineWidth', 2);
    hold on;
    plot(posX_sens, posY_sens, '.', 'MarkerSize', 18);
    plot(X_arr(:, 1), X_arr(:, 2), 'k.', 'MarkerSize', 18);
    hold off;
    grid on;
    title('Trajectory');
    legend('Ground Truth', 'Sensor', 'Estimation');
    axis equal;

end

function [t, accX, velX, posX, accY, velY, posY, t_sens, posX_sens, posY_sens, posX_var, posY_var] = generate_signals()
    dt = 0.01;
    t=(0:dt:70)';

    posX_var = 8; % m^2
    posY_var = 8; % m^2

    posX_noise = randn(size(t))*sqrt(posX_var);
    posY_noise = randn(size(t))*sqrt(posY_var);

    accX = sin(0.3*t) + 0.5*sin(0.04*t);
    velX = cumsum(accX)*dt;
    posX = cumsum(velX)*dt;

    accY = 0.1*sin(0.5*t)+0.03*t;
    velY = cumsum(accY)*dt;
    posY = cumsum(velY)*dt;

    t_sens = t(1:100:end);

    posX_sens = posX(1:100:end) + posX_noise(1:100:end);
    posY_sens = posY(1:100:end) + posY_noise(1:100:end);
end

function [X] = init_kalman(X, y)
    X(1) = y(1);
    X(2) = y(2);
end

function [X, P] = prediction(X, P, Q, F)
    X = F*X;
    P = F*P*F' + Q;
end

function [X, P] = update(X, P, y, R, H)
    Inn = y - H*X;
    S = H*P*H' + R;
    K = P*H'/S;

    X = X + K*Inn;
    P = P - K*H*P;
end

The simulated position signal disappears between 40s and 58s but the estimation keeps going by means of the estimated velocity and acceleration.

Position, Velocity and Acceleration signals

Trajectory XY

As you can see the position can be estimated even without sensor update

Anton
  • 4,544
  • 2
  • 25
  • 31
  • I dont know vel[0], vel[1], acc[0], acc[1]. – Lalu May 07 '19 at 08:10
  • The filter knows them for you. Those values will be estimated automatically. You just need to know the measurements for position X and Y – Anton May 07 '19 at 08:52
  • If you want I can post an example here. – Anton May 07 '19 at 08:56
  • Can you post your current python code? Otherwise it would be easier for me to load a matlab one (or a python code which uses pykalman library). – Anton May 07 '19 at 08:59
  • thank you for the code....is there any way to write zk = Hxk + vk in matlab. vk is the measurement noise error with zero mean and covariance given as R. – Lalu May 08 '19 at 08:56
  • Sorry, I did not understand the question. Can you reference the correspondence code line? Are we talking about update(X, P, y, R, H)? – Anton May 08 '19 at 09:15
  • In python, i wrote `x_cap = np.dot(F, x(-1))` for prediction, where `F` is the transition matrix and `x(-1)` the previous state at `t = 0` and `x_cap` is the predicted state at `t = 1`. In time `t = 1`, I dont see the measurement. If `F = np.array([[1, 0], [0, 1]])` and `x(-1) = np.array([pos[0], [pos[1]])`, then in the next step `x_cap = np.array([pos[0], [pos[1]]) = x(-1)`. – Lalu May 08 '19 at 09:35
  • You cannot get x_cap directly from the measurement. The update step looks like: x_cap = x_cap + P*H'/(H*P*H' + R)*(x - H*x_cap) – Anton May 08 '19 at 09:44
  • for example `x(-1) = [300, 20]` i.e the updated state at time `t = 0`, then what will be its `x_cap` i.e. the predicted state at time `t = 1` after using the matrices `(Q, R, F, H and P)` I have mentioned in the question? – Lalu May 08 '19 at 09:52
  • For the state prediction you just need the `F` matrix. `x_cap_new = F * x_cap_old`. If your F matrix is `F = np.array([[1, 0], [0, 1]])` then the state will stay the same. The point is, that your state `x_cap` is a 6x1 matrix including estimated states for `v_x`, `v_y`, `a_x`, `a_y` and your `F` is a 6x6 matrix. – Anton May 08 '19 at 10:40