如何使用扩展卡尔曼滤波器组合来自两个传感器的数据，其中一个传感器比另一个传感器更可靠？

Question

我有两个传感器A和B。我正在尝试使用扩展卡尔曼滤波器合并他们的传感器数据，以进行高效的数据分析。由于数据类型是非线性的，因此我使用扩展卡尔曼滤波器。就我而言，来自传感器A的数据始终可靠，但来自传感器B的数据的可靠性随时间变化。例如，在 时t=0，其可靠性很高，并且可靠性逐渐（均匀）下降，直到t=T。在这种情况下，如何结合传感器数据以获得更好的优化结果？或者如何为来自传感器Bt=0的数据分配动态权重？t=T

更新：我必须在 EKF 的预测阶段使用来自传感器B的数据。

Answer 1

正如我在评论中已经提到的，您可以通过输入方差来表达传感器的可靠性。传感器 A 的方差将保持不变，而传感器 B 的方差会随着时间的推移而增加。

过滤器不关心系统中有多少个传感器。它只是对其输入进行测量，就好像只有一个传感器一样。重要的是传达的差异。

这是一个带有两个加速度传感器的滤波器的简短 matlab 示例。状态空间由速度和加速度组成。传感器 A 在偶数呼叫时激活，传感器 B 在奇数呼叫时激活。

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

结果如下：

如果您的系统在每次调用中处理两个传感器，您需要执行更新阶段两次：

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  

结果会好一点，因为过滤器从测量中获得更多信息：