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

Question

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

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

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  

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