使用 Eigen 实现 3D 位置的卡尔曼滤波器

Posted 2023-02-21

【中文标题】使用 Eigen 实现 3D 位置的卡尔曼滤波器

【英文标题】：Kalman Filter Implementation for 3D Position using Eigen

【发布时间】：2018-05-23 15:13:36

【问题描述】：

我使用 C++ 中的 Eigen 库编写了一个卡尔曼滤波器实现，并使用此 link 的实现来测试我的滤波器：我的预测步骤如下所示：

void KalmanFilter::Predict()

    // state Estimate = state transition matrix * previous state
    // No control input present.
    x = A * x;


   // State Covariance Matrix = (State Transition Matrix * Previous State 
   Covariance matrix * (State Transition Matrix)^T ) + Process Noise

   P = A * P * A.transpose() + Q;

而我的更新步骤是：

void KalmanFilter::Update(VectorXd z)

    //Kalman Gain = (State Covariance Matrix * Measurement matrix.transpose) * (H*P*H^T + Measurement Noise)^-1
    K = (P * H.transpose()) * (H * P * H.transpose()+ R).inverse();

    //Estimated Stated = Estimated state + Kalman Gain (Measurement Innovation)
    x = x + K*(z - H * x);

   //State Covariance matrix = (Identity Matrix of the size of  x.size * x.size) - K* H * P;
    long x_size = x.size();
    MatrixXd I = MatrixXd::Identity(x_size, x_size);
    P = (I - K * H) * P ;

我的初始值是：

pos_x = 0.0;
pos_y = 0.0;
pos_z = 1.0;
vel_x = 10.0;
vel_y = 0.0;
vel_z = 0.0;
acc_x = 0.0;
acc_y = 0.0;
acc_z = -9.81;

我通过循环执行以下操作来生成“假数据”：

double c = 0.1; // Drag resistance coefficient
double damping = 0.9 ; // Damping

double sigma_position = 0.1 ; // position_noise

// Create simulated position data
for (int i = 0; i < N; i ++)

    acc_x = -c * pow(vel_x, 2);  // calculate acceleration ( Drag Resistance)

    vel_x += acc_x * dt;  // Integrate acceleration to give you velocity in the x axis.

    pos_x += vel_x * dt; // Integrate velocity to return the position in the x axis

    acc_z = -9.806 + c * pow(vel_z, 2); // Gravitation + Drag

    vel_z += acc_z * dt;   // z axis velocity

    pos_z += vel_z * dt;  // position in z axis

    // generate y position here later.

    if(pos_z < 0.01)
    
        vel_z = -vel_z * damping;
        pos_z += vel_z * dt;
    

    if (vel_x < 0.1)
    
        acc_x = 0.0;
        acc_z = 0.0;
    

    // add some noise
    pos_x = pos_x + sigma_position * process_noise(generator); 
    pos_y = pos_y + sigma_position * process_noise(generator);
    pos_z = pos_z + sigma_position * process_noise(generator);

然后我运行我的预测并逐步更新：

// Prediction Step
kalmanFilter.Predict();

// Correction Step
kalmanFilter.Update(z);

其中 z 是一个 3 x 1 向量，包含 pos_x, pos_y and pos_z

我的状态转换矩阵A 如下所示：

A <<    1, 0, 0, dt, 0, 0, dt_squared, 0 , 0,
        0, 1, 0, 0,  dt, 0, 0, dt_squared, 0,
        0, 0, 1, 0, 0, dt, 0, 0, dt_squared,
        0, 0, 0, 1, 0, 0, dt, 0, 0,
        0, 0, 0, 0, 1, 0, 0 , dt, 0,
        0, 0, 0, 0, 0, 1, 0, 0, dt,
        0, 0, 0, 0, 0, 0, 1, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 1, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 1;

dt_squared 是 (dt * dt) /2;

P is  

P<< 100, 0, 0, 0, 0, 0, 0, 0, 0,
            0, 100, 0, 0, 0, 0, 0, 0, 0,
            0, 0, 100, 0, 0, 0, 0, 0, 0,
            0, 0, 0, 100, 0, 0, 0, 0, 0,
            0, 0, 0, 0, 100, 0, 0, 0, 0,
            0, 0, 0, 0, 0, 100, 0, 0, 0,
            0, 0, 0, 0, 0, 0, 100, 0, 0,
            0, 0, 0, 0, 0, 0, 0, 100, 0,
            0, 0, 0, 0, 0, 0, 0, 0, 100;

和

   R << 1, 0, 0,
         0, 1, 0,
         0, 0, 1;

和

Q = G * G.transpose()* a * a;  

G 是一个 9 x 1 的矩阵

G << dt_squared, dt_squared, dt_squared, dt, dt, dt, 1, 1, 1;

a =  0.1 //( acceleration process noise)

我的问题是我对 y 和 z 的估计位置偏离了“真实”位置。如果你看下面的图表，

这是 pos_x 的样子：

这是 pos_y 的样子：

最后是 Z：

这是我第一次尝试使用卡尔曼滤波器，我不确定我在这里做错了什么。我的最终目标是用它来估计无人机的位置。此外，我还有以下问题：

例如，在无人机的现实生活中，如果您不能直接观察过程，您如何选择过程噪声？您只是选择任意值吗？

对于这篇长篇文章，我深表歉意。任何帮助表示赞赏。

【问题讨论】：

我对你的实现很好奇。你能在你的github上指点我吗！谢谢

【参考方案1】：

我不确定这是代码相关问题、算法实现问题还是预期问题。

您确实意识到，如果虚假数据中有太多操纵，像这样的过滤器将无法再现真实数据，甚至任何接近真实数据的数据。

另外，您的图表不存在。

我知道我的回复不符合社区标准，但我不能发表评论，否则我会这样做。

在您提供绘图并根据更新率检查路径曲率之前，我不会尝试详细说明。过滤器也需要针对特定​​系统进行“调整”。您可能需要使用噪声参数来更好地调整它。对于操纵轨道，可能需要使用更高阶的过滤器、Singer 或 Jerk 过滤器……过滤器需要对系统进行足够好的建模。根据您的更新矩阵，您似乎有一个抛物线（二阶）估计。您可能还想在其他非软件或代码特定的论坛中询问此问题。

【讨论】：

我没有足够高的分数来将图片附加到帖子上，这就是我将它们作为链接提供的原因。他们在那里，但不够明显。但是在将相同的数据集应用到我在网上找到的其他卡尔曼滤波器实现之后，结果与我的相似。我的脑震荡是我相信噪声参数和我的初始状态协方差矩阵只需要一些调整

您估计的实际系统是什么？看起来 z 位置变量在导数中有跳跃不连续性。一个简单的白噪声滤波器很可能无法捕捉到这一点。您已将首次尝试应用到难以跟踪的系统！看起来它在第一次“反弹”之前做得很好。你有我们可以称之为机动目标的东西。您需要一个 Jerk 过滤器来捕捉该点，或者一个 MMI - 一个库或机动过滤器，以及在检测到这种机动时分支到一个新过滤器的逻辑。

简而言之，我会说您的过滤器可以在应有的位置工作，但是您将简单的过滤器应用于复杂的移动目标。坚持下去。

【参考方案2】：

每个系统都有差异。假设过滤器的方差为 +-1%，而实际值的方差为 +-5%；如果您预测一个值，您必须选择更新以使用预测值或测量值。取决于你更相信哪一个。 否则，您的过滤器确实总是根据自己的值开发......