Use a Kalman filter with two irregular observations of the same variable

Question

I understand how a Kalman filter works with single observations for the state variables. However I have two different sensors that observe the same position of an object.

1. How can I use a Kalman filter if I have more than one observation as input? I have six state variables $x, x', x'', y, y', y''$ (position, velocity, acceleration of the object in two dimensions) and four inputs $x_1, y_1, x_2, y_2$ (two obersvations of the position). What have $P,F,H,\text{etc}$ to look like?

2. Sometimes the object is obstructed for one sensor but not the other. So I only have one observation instead of two. How do I handle this case if the Kalman filter is configure to have two observations as input?

Answer 1

If you have several observations for the same signal you just need to update your model several times (once for each sensor). Your workflow would look like:

init
prediction
update (sensor 1)
update (sensor 2)
update (sensor N)
prediction
update (sensor 1)
update (sensor 2)
update (sensor N)
and so on

If some of the measurements are not available at the current call you just jump over and update only for available ones.

Doing so you don't need to change your model depending on the system configuration. The filter should not know there you take your measurements from and how many sensor you have. It just predicts for the time between the measurements and updates as soon one or several measurements are available.

Here is a short matlab example for your problem:

function [] = main(  )
    dt = 0.1;
    t = (0:dt:20)';

    %generating a reference trajectory
    x = 2*(sin(t)+t);
    y = 2*(7*(sin(t/4)-1) + t/2);  

    %reference velocity and acceleration for later plotting
    ref_vx = diff(x)/dt;
    ref_vy = diff(y)/dt;

    ref_ax = diff(ref_vx)/dt;
    ref_ay = diff(ref_vy)/dt;    

    n = numel(x);

    %variance of the position sensors
    sensor_1_var = 0.01; 
    sensor_2_var = 0.04;

    %noise calculation
    sensor_1_x_noise = randn(size(x))*sqrt(sensor_1_var);
    sensor_1_y_noise = randn(size(y))*sqrt(sensor_1_var);

    sensor_2_x_noise = randn(size(x))*sqrt(sensor_2_var);
    sensor_2_y_noise = randn(size(y))*sqrt(sensor_2_var);    

    %measurements sensor 1
    sensor_1_x = x + sensor_1_x_noise;
    sensor_1_y = y + sensor_1_y_noise;

    %measurements sensor 2
    sensor_2_x = x + sensor_2_x_noise;
    sensor_2_y = y + sensor_2_y_noise;    

    % state matrix (velocity, acceleration)
    X = zeros(6,1); %px, vx, ax, py, vy, ay

    % covariance matrix
    P = diag([0.01, 30, 30, 0.01, 30, 30]);

    % system noise
    Q = diag([0.1, 0.01, 1, 0.1, 0.01, 1]);

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

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

    % measurement noise 
    R = NaN; % will be set depending on sensor 1 or 2

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

    % fusion
    for i = 1:n

        y_1 = [sensor_1_x(i); sensor_1_y(i)]; %measurement from sensor 1
        y_2 = [sensor_2_x(i); sensor_2_y(i)]; %measurement from sensor 2

        if (i == 1)
            [X] = init_kalman(X, y_1); % initialize the state using the 1st sensor
        else
            [X, P] = prediction(X, P, Q, F);

            %here you can use both measurements or only the available one
            %the model stays the same
            [X, P] = update(X, P, y_1, eye(2)*sensor_1_var, H); % update using sensor 1
            [X, P] = update(X, P, y_2, eye(2)*sensor_2_var, H); % update using sensor 2
        end

        X_arr(i, :) = X;
    end  


    figure;

    hold on;
    plot(sensor_1_x, sensor_1_y, '.', 'MarkerSize', 10);
    plot(sensor_2_x, sensor_2_y, '.', 'MarkerSize', 10);
    plot(x, y, 'x-', 'LineWidth', 2);
    plot(X_arr(:, 1), X_arr(:, 4), 'o-', 'LineWidth', 2);
    hold off;
    title('Position');
    grid minor;
    axis equal;
    legend('Measurement Sensor 1', 'Measurement Sensor 2', 'Reference', 'Estimation');

    figure;
    subplot(2,1,1);
    plot(t(1:end-1), ref_vx);
    hold on;
    plot(t, X_arr(:, 2));
    hold off;
    title('vx');
    legend('ref', 'kf');
    grid minor;

    subplot(2,1,2);
    plot(t(1:end-1), ref_vy);
    hold on;
    plot(t, X_arr(:, 5));
    hold off;
    title('vy');
    legend('ref', 'kf');
    grid minor;

    figure;
    subplot(2,1,1);
    plot(t(1:end-2), ref_ax);
    hold on;
    plot(t, X_arr(:, 3));
    hold off;
    title('ax');
    legend('ref', 'kf');
    grid minor;

    subplot(2,1,2);
    plot(t(1:end-2), ref_ay);
    hold on;
    plot(t, X_arr(:, 6));
    hold off;
    title('ay');  
    legend('ref', 'kf');
    grid minor;
end

function [X] = init_kalman(X, y)
    X = zeros(6, 1);
    X(1) = y(1, 1);
    X(4) = y(2, 1);
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

Here are the results:

Velocity estimation:

Acceleration estimation: