卡爾曼濾波器預測在缺少測量且僅位置已知的情況下

Question

我正在嘗試實現卡爾曼濾波器。 我只知道職位。 某些時間步長缺少測量值。 這就是我定義矩陣的方式：

過程噪聲矩陣

Q = np.diag([0.001, 0.001] )

測量噪聲矩陣

R = np.diag([10, 10])

協方差矩陣

P = np.diag([0.001, 0.001])

觀察矩陣

H = np.array([[1.0, 0.0], [0.0, 1.0]])

轉移矩陣

F = np.array([[1, 0], [0, 1]])

狀態

x = np.array([pos[0], [pos[1]])

我不知道是否正確。 例如，如果我在t=0處看到目標而在t = 1處沒有看到，我將如何預測它的位置。 我不知道速度。 這些矩陣定義是否正確？

Answer 1

您需要擴展模型並添加速度狀態（如果需要加速度）。 即使您沒有位置測量值，過濾器也會根據位置估計新狀態並使用它們來預測位置。

你的矩陣看起來像這樣：

過程噪聲矩陣

Q = np.diag([0.001, 0.001, 0.1, 0.1, 0.1, 0.1]) #enter correct numbers for vel and acc

測量噪聲矩陣保持不變

協方差矩陣

P = np.diag([0.001, 0.001, 0.1, 0.1, 0.1, 0.1]) #enter correct numbers for vel and acc

觀察矩陣

H = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0, 0.0, 0.0]])

轉移矩陣

F = np.array([[1, 0, dt,  0, 0.5*dt**2,         0], 
              [0, 1,  0, dt,         0, 0.5*dt**2], 
              [0, 0,  1,  0,        dt,         0],
              [0, 0,  0,  1,         0,        dt],
              [0, 0,  0,  0,         1,         0],
              [0, 0,  0,  0,         0,         1]])

狀態

看看我的舊帖子，有一個非常相似的問題。 在那種情況下，只有加速度的測量值以及濾波器估計的位置和速度。

在原始加速度數據上使用 PyKalman 計算位置

在下面的帖子中，還必須預測位置。 該模型僅由兩個位置和兩個速度組成。 您可以在那里的 python 代碼中找到矩陣。

具有不同時間步長的卡爾曼濾波器

更新

這是我的 matlab 示例，僅通過位置測量向您展示速度和加速度的狀態估計：

function [] = main()
    [t, accX, velX, posX, accY, velY, posY, t_sens, posX_sens, posY_sens, posX_var, posY_var] = generate_signals();

    n = numel(t_sens);

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

    % covariance matrix
    P = diag([0.001, 0.001,10, 10, 2, 2]);

    % system noise
    Q = diag([50, 50, 5, 5, 3, 0.4]);

    dt = t_sens(2) - t_sens(1);

    % transition matrix
    F = [1, 0, dt,  0, 0.5*dt^2,        0; 
         0, 1,  0, dt,        0, 0.5*dt^2; 
         0, 0,  1,  0,       dt,        0;
         0, 0,  0,  1,        0,       dt;
         0, 0,  0,  0,        1,        0;
         0, 0,  0,  0,        0,        1]; 

    % observation matrix 
    H = [1 0 0 0 0 0;
         0 1 0 0 0 0];

    % measurement noise 
    R = diag([posX_var, posY_var]);

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

    % fusion
    for i = 1:n
        y = [posX_sens(i); posY_sens(i)];

        if (i == 1)
            [X] = init_kalman(X, y); % initialize the state using the 1st sensor
        else
            if (i >= 40 && i <= 58) % missing measurements between 40 ans 58 sec
                [X, P] = prediction(X, P, Q, F);
            else
                [X, P] = prediction(X, P, Q, F);
                [X, P] = update(X, P, y, R, H);
            end
        end

        X_arr(i, :) = X;
    end  

    figure;
    subplot(3,1,1);
    plot(t, posX, 'LineWidth', 2);
    hold on;
    plot(t_sens, posX_sens, '.', 'MarkerSize', 18);
    plot(t_sens, X_arr(:, 1), 'k.', 'MarkerSize', 14);
    hold off;
    grid on;
    title('PositionX');
    legend('Ground Truth', 'Sensor', 'Estimation');

    subplot(3,1,2);
    plot(t, velX, 'LineWidth', 2);
    hold on;
    plot(t_sens, X_arr(:, 3), 'k.', 'MarkerSize', 14);
    hold off; 
    grid on;
    title('VelocityX');
    legend('Ground Truth', 'Estimation');

    subplot(3,1,3);
    plot(t, accX, 'LineWidth', 2);
    hold on;
    plot(t_sens, X_arr(:, 5), 'k.', 'MarkerSize', 14);
    hold off;
    grid on;
    title('AccX');
    legend('Ground Truth', 'Estimation');


    figure;
    subplot(3,1,1);
    plot(t, posY, 'LineWidth', 2);
    hold on;
    plot(t_sens, posY_sens, '.', 'MarkerSize', 18);
    plot(t_sens, X_arr(:, 2), 'k.', 'MarkerSize', 14);
    hold off;
    grid on;
    title('PositionY');
    legend('Ground Truth', 'Sensor', 'Estimation');

    subplot(3,1,2);
    plot(t, velY, 'LineWidth', 2);
    hold on;
    plot(t_sens, X_arr(:, 4), 'k.', 'MarkerSize', 14);
    hold off; 
    grid on;
    title('VelocityY');
    legend('Ground Truth', 'Estimation');

    subplot(3,1,3);
    plot(t, accY, 'LineWidth', 2);
    hold on;
    plot(t_sens, X_arr(:, 6), 'k.', 'MarkerSize', 14);
    hold off;    
    grid on;
    title('AccY');
    legend('Ground Truth', 'Estimation');    

    figure;
    plot(posX, posY, 'LineWidth', 2);
    hold on;
    plot(posX_sens, posY_sens, '.', 'MarkerSize', 18);
    plot(X_arr(:, 1), X_arr(:, 2), 'k.', 'MarkerSize', 18);
    hold off;
    grid on;
    title('Trajectory');
    legend('Ground Truth', 'Sensor', 'Estimation');
    axis equal;

end

function [t, accX, velX, posX, accY, velY, posY, t_sens, posX_sens, posY_sens, posX_var, posY_var] = generate_signals()
    dt = 0.01;
    t=(0:dt:70)';

    posX_var = 8; % m^2
    posY_var = 8; % m^2

    posX_noise = randn(size(t))*sqrt(posX_var);
    posY_noise = randn(size(t))*sqrt(posY_var);

    accX = sin(0.3*t) + 0.5*sin(0.04*t);
    velX = cumsum(accX)*dt;
    posX = cumsum(velX)*dt;

    accY = 0.1*sin(0.5*t)+0.03*t;
    velY = cumsum(accY)*dt;
    posY = cumsum(velY)*dt;

    t_sens = t(1:100:end);

    posX_sens = posX(1:100:end) + posX_noise(1:100:end);
    posY_sens = posY(1:100:end) + posY_noise(1:100:end);
end

function [X] = init_kalman(X, y)
    X(1) = y(1);
    X(2) = y(2);
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

模擬位置信號在 40 秒到 58 秒之間消失，但​​通過估計的速度和加速度進行估計。

如您所見，即使沒有傳感器更新也可以估計位置