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]])

状态

看看我的旧帖子有一个非常相似的问题。在那种情况下，只能测量加速度，而滤波器也只能估算位置和速度。

Using PyKalman on Raw Acceleration Data to Calculate Position

在接下来的文章中，人们也必须预测位置。该模型仅包含两个位置和两个速度。您可以在python代码中找到矩阵。

Kalman filter with varying timesteps

更新

这是我的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

模拟的位置信号在40s和58s之间消失，但通过估计的速度和加速度继续进行估计。

如您所见，即使不更新传感器也可以估算位置

缺少测量值且仅知道位置的情况下的卡尔曼滤波器预测

1 个答案: