基于卡尔曼滤波的多传感器融合

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

其结果是：

​

​