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

Question

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

kalman-filterfusion

7

我需要使用卡尔曼滤波器来融合多个传感器的高斯测量位置（例如将4个位置作为输入并输出1个位置）。你能否帮助我提供一些示例或教程，因为我找到的所有示例都与位置估计有关？

- khaoula mannay

https://dsp.stackexchange.com/questions/85434/kalman-filter-on-sensor-fusion - Thomas

1个回答

网页内容由stack overflow 提供, 点击上面的

可以查看英文原文，
原文链接

- Anton · Accepted Answer

选项1

加权平均

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

因此，如果您有两个具有方差V1和V2的信号S1和S2，则融合结果将是

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

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

选项2

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

经典的卡尔曼滤波器在循环中使用预测和更新步骤：

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

以下是结果: