Kalman filter prediction in case of missing measurement and only positions are known

Question

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?

Answer 1

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

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

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

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.

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