使用卡尔曼滤波器的多传感器融合

Question

我需要使用卡尔曼滤波器来融合多传感器位置以进行高斯测量（例如，4个位置作为滤波器的输入，1个位置作为输出）。有可能帮我一些例子或教程，因为我发现的所有例子都与位置的估计有关？

Answer 1

选项1

加权平均

在这种情况下，您不需要实现真正的卡尔曼滤波器。您可以使用信号方差来计算权重，然后计算输入的加权平均值。权重可以作为方差的倒数找到。

因此，如果您有两个信号S1和S2，其变量为V1和V2，则融合结果为

在下一个图中可以看到融合示例。

我模拟了两个信号。第二信号的方差随时间变化。只要它小于第一信号的方差，融合结果就接近第二信号。当第二信号的方差太高时不是这种情况。

方案2

具有多个更新步骤的卡尔曼滤波器

经典卡尔曼滤波器在循环中使用prediction和update步骤：

prediction 
update
prediction 
update
...

在您的情况下，您有4个独立的测量值，因此您可以在单独的update步骤中相继使用这些读数：

prediction 
update 1
update 2
update 3
update 4
prediction 
update 1
...

一个非常好的观点是这些更新的顺序无关紧要！您可以使用更新1,2,3,4或3,2,4,1。在这两种情况下，您都应该获得相同的融合输出。

与第一个选项相比，您有以下优点：

• 你有一个方差传播
• 您有系统噪声矩阵Q，因此您可以控制融合输出的平滑度

这是我的matlab代码：

function [] = main()
    % time step
    dt = 0.01;

    t=(0:dt:2)';
    n = numel(t);

    %ground truth
    signal = sin(t)+t; 

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

    % covariance matrix
    P = zeros(2,2);

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

    % system noise
    Q = [0.04 0;
         0 1];

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

    % observation matrix 
    H = [1 0];

    % variance of signal 1 
    s1_var = 0.08*ones(size(t)); 
    s1 = generate_signal(signal, s1_var);

    % variance of signal 2 
    s2_var = 0.01*(cos(8*t)+10*t);
    s2 = generate_signal(signal, s2_var);

    % variance of signal 3 
    s3_var = 0.02*(sin(2*t)+2);
    s3 = generate_signal(signal, s3_var);

    % variance of signal 4 
    s4_var = 0.06*ones(size(t)); 
    s4 = generate_signal(signal, s4_var);

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

            [X, P] = update(X, P, s1(i, 1), s1(i, 2), H);
            [X, P] = update(X, P, s2(i, 1), s2(i, 2), H);
            [X, P] = update(X, P, s3(i, 1), s3(i, 2), H);
            [X, P] = update(X, P, s4(i, 1), s4(i, 2), H);
        end

        X_arr(i, :) = X;
    end

    plot(t, signal, 'LineWidth', 4);
    hold on;
    plot(t, s1(:, 1), '--', 'LineWidth', 1);
    plot(t, s2(:, 1), '--', 'LineWidth', 1);
    plot(t, s3(:, 1), '--', 'LineWidth', 1);
    plot(t, s4(:, 1), '--', 'LineWidth', 1);
    plot(t, X_arr(:, 1), 'LineWidth', 4);
    hold off;
    grid on;
    legend('Ground Truth', 'Sensor Input 1', 'Sensor Input 2', 'Sensor Input 3', 'Sensor Input 4', 'Fused Output');
end

function [s] = generate_signal(signal, var)
    noise = randn(size(signal)).*sqrt(var);

    s(:, 1) = signal + noise;
    s(:, 2) = var; 
end

function [X, P] = init_kalman(X, y)
    X(1,1) = y;
    X(2,1) = 0;

    P = [100 0;
         0   300];
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

这是结果：