How do I combine data from two sensors using an extended Kalman filter where one of the sensors is more reliable than the other?

Question

I've two sensors A and B. I'm trying to combine their sensor data using Extended Kalman Filter for a productive data analysis. Since the data types are non-linear, I'm using the Extended Kalman filter. In my case, the data from sensor A is always reliable however the reliability of data from sensor B varies on time. For example, at t=0, it's reliability is high and the reliability decreases gradually (uniformly) till t=T. In this situation, how can I combine the sensor data for a better-optimized result? Or how can I assign a dynamic weight (t=0 to t=T) for data from the sensor B ?

Update: I have to use the data from sensor B in the prediction phase of the EKF.

Answer 1

As I've already mentioned in my comments you can express the reliability of your sensors through the input variance. The variance of Sensor A will be constant whereas the variance of Sensor B increases over the time.

The filter does not care how many sensors are in the system. It just gets measurements on its input as if there is only one sensor. The important thing is the communicated variance.

Here is a short matlab example of a filter with two acceleration sensors. The state space consists of the velocity and acceleration. Sensor A is active at even and Sensor B at odd calls.

function [] = main()

    dt = 0.01;
    t=(0:dt:70)';

    acc_ref = sin(0.3*t) + 0.5*sin(0.04*t);

    accA_std = 0.05; % standard deviation for Sensor A
    accB_std = 0.1 + 0.01*t; % standard deviation for Sensor B

    accA = acc_ref + randn(size(t)).*accA_std;
    accB = acc_ref + randn(size(t)).*accB_std;

    n = numel(t);

    % state matrix (velocity, acceleration)
    X = zeros(2,1);

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

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

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

    % observation matrix 
    H = [0 1];

    % measurement noise 
    R = 0; % will be set depending on sensor A or B

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

    % fusion
    for i = 1:n

        % use sensor A at even and B at odd calls
        if (mod(i, 2))
            y = accA(i);
            R = accA_std^2; %set varaince of Sensor A as measurement noise
        else
            y = accB(i);
            R = accB_std(i)^2; %set varaince of Sensor B as measurement noise
        end

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

        X_arr(i, :) = X;
    end  

    figure;
    plot(t, acc_ref, 'LineWidth', 2);
    hold on;
    plot(t, accA, 'LineWidth', 1);
    plot(t, accB, 'LineWidth', 1);
    plot(t, X_arr(:, 2), 'LineWidth', 2);
    hold off;
    grid on;
    legend('Ground Truth', 'SensorA', 'SensorB', 'Estimation');
end

function [X] = init_kalman(X, y)
    X(1) = 0;
    X(2) = y;
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 result looks like this:

If your system proceeds both sensors in each call you need to execute the update phase twice:

for i = 1:n
    y1 = accA(i);
    R1 = accA_std^2; %set varaince of Sensor A as measurement noise

    y2 = accB(i);
    R2 = accB_std(i)^2; %set varaince of Sensor B as measurement noise

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

        [X, P] = update(X, P, y1, R1, H); %Update for Sensor A
        [X, P] = update(X, P, y2, R2, H); %Update for Sensor B
    end

    X_arr(i, :) = X;
end  

The result will be a little bit better, because the filter gets more information from the measurements: