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

Posted 2023-02-21

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

【英文标题】：How do I combine data from two sensors using an extended Kalman filter where one of the sensors is more reliable than the other?

【发布时间】：2019-07-29 07:29:19

【问题描述】：

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

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

【问题讨论】：

您需要一个函数来根据时间的变化计算传感器的可靠性。拥有一个，您可以为每个传感器设置测量协方差。看看***.com/questions/47599886/…。在这个实现中，有两个具有不同静态方差的传感器。您的情况几乎相同，但方差是时间的函数。

我不太明白您的更新声明。您的过滤器将只有一个测量值（对于传感器 A 和 B），并且测量值只有一个状态。过滤器甚至不知道有两个传感器，它唯一会看到的是一些测量值具有更高的方差。预测阶段根本不关心传感器。它仅取决于您的系统建模，最后取决于 F 和 Q 矩阵。

【参考方案1】：

正如我在 cmets 中已经提到的，您可以通过输入方差来表达传感器的可靠性。传感器 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  

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