在 C++ 中实现卡尔曼滤波器

Posted 2023-02-21

【中文标题】在 C++ 中实现卡尔曼滤波器

【英文标题】：Implementing Kalman filter in C++

【发布时间】：2021-08-03 04:49:31

【问题描述】：

我想使用 eigen 库在 C++ 中实现一个扩展的卡尔曼滤波器，因为我对机器人技术感兴趣，这似乎是一个很好的练习，可以更好地学习 C++，而且它似乎是一个有趣的项目。我希望我可以发布我的代码以获得一些关于编写课程的反馈，以及我的下一步应该从这里开始。所以我从网上的课堂上得到了方程式

到目前为止，我已经硬编码了一个大小为 2x1 的状态向量和一个测量数组作为测试，但我想更改它，以便我可以声明一个任意大小的状态向量，我会将测量数组移动到 main.cpp 文件。我刚开始就这样做了，所以我可以简单地声明这个类的对象并快速测试功能，到目前为止一切似乎都在工作。我接下来想做的是创建另一个类，它从某个源获取测量值并将其转换为特征矩阵以传递给这个卡尔曼滤波器类。我的主要问题是：

我应该将测量更新和状态预测作为两个不同的功能吗？真的有关系吗？我首先这样做是因为我认为它更容易阅读。

我应该在类构造函数中设置状态向量之类的东西的大小，还是为此设置一个初始化函数之类的东西更好？

我读到最好让作为矩阵的类成员实际上是指向矩阵的指针，因为它使类更轻。这是什么意思？如果我想在 PC 上运行它而不是像树莓派这样的东西，这很重要吗？

在measurementUpdate函数中，y、S、K应该是类成员吗？它会使类更大，但是当程序循环时我不会构造和销毁 Eigen 对象？这是好的做法吗？

是否应该有一个接受测量输入的类成员，还是只将值传递给测量更新函数更好？有关系吗？

是否值得为此尝试实现一个类，还是只使用一个实现过滤器的函数更好？

删除了这个，因为它不是问题。

我正在考虑实现一些 getter 函数，以便检查状态变量和协方差矩阵，将这些成员公开而不具有 getter 函数会更好吗？

抱歉，如果这是张贴在错误的地方，如果这些是超级基本的问题，我对这些东西很陌生。感谢所有帮助，感谢所有建议。

标题：

#include "eigen3/Eigen/Dense"
#include <iostream>
#include <vector>

class EKF 
public:

  EKF();
  void filter(Eigen::MatrixXd Z);

private:
  void measurementUpdate(Eigen::MatrixXd Z);
  void statePrediction();

  Eigen::MatrixXd P_; //Initial uncertainty
  Eigen::MatrixXd F_; //Linearized state approximation function
  Eigen::MatrixXd H_; //Jacobian of linearrized measurement function
  Eigen::MatrixXd R_; //Measurement uncertainty
  Eigen::MatrixXd I_; //Identity matrix
  Eigen::MatrixXd u_; //Mean of state function
  Eigen::MatrixXd x_; //Matrix of initial state variables

;  

来源：

EKF::EKF() 
  double meas[5] = 1.0, 2.1, 1.6, 3.1, 2.4;
  x_.resize(2, 1);
  P_.resize(2, 2);
  u_.resize(2, 1);
  F_.resize(2, 2);
  H_.resize(1, 2);
  R_.resize(1, 1);
  I_.resize(2, 2);
  Eigen::MatrixXd Z(1, 1);
  for(int i = 0; i < 5; i++)
    Z << meas[i];
    measurementUpdate(Z);
    //statePrediction();
  


void EKF::measurementUpdate(Eigen::MatrixXd Z)
  //Calculate measurement residual
  Eigen::MatrixXd y = Z - (H_ * x_);
  Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_;
  Eigen::MatrixXd K = P_ * H_.transpose() * S.inverse();

  //Calculate posterior state vector and covariance matrix
  x_ = x_ + (K * y);
  P_ = (I_ - (K * H_)) * P_;


void EKF::statePrediction()
  //Predict next state vector
  x_ = (F_ * x_) + u_;
  P_ = F_ * P_ * F_.transpose();


void EKF::filter(Eigen::MatrixXd Z)
  measurementUpdate(Z);
  statePrediction();
 

【问题讨论】：

不用担心。你在问正确的问题。 1 - 可读性是关键，2 - 如果大小是常量，则声明一个常量，如果不是，则使用容器并让它担心内存管理，3 - 通过指针（或引用）传递容器（矩阵）是首选，因为你是不传递完整副本（它节省堆栈空间），4 - 取决于调用它们的次数。保存 1M 临时对象构造的更大类是一个很好的权衡，5 - 没关系，6 - 除非 EXP 做一些新的事情，那么一个函数就足够了，7？，8 - getter 应该是公共的。跨度>

话虽如此，好的练习将新的学习限制在练习的范围内。意思是，一个好的 C++ 练习侧重于实现语言的特性。除非您已经知道传感器过滤器等背后的矩阵运算和数学运算，否则只会在您正在练习的 C++ 之上增加不必要的复杂层。就像您不打开源文件来学习数学一样。将它们分开，直到您对两者都感到满意为止。 （如果你打网球，你的高尔夫比赛会受到影响，等等......）同时做这两件事没有错 - 但要了解你的注意力将如何分散。

最后一个提示——欢迎使用 Stack Overflow。您在问题的格式方面做得非常好，这反映了您的努力。您的问题的唯一缺点是过于广泛。它并没有真正提出一个可以简洁回答的编程问题。请参阅About 页面和How to Ask a Question。 （这将使人们不会因为需要更多关注而对问题投反对票——我不会投反对票）。当您遇到麻烦时，请随时提出更多针对性强的问题。

谢谢！这是很多帮助

@user7538434 如果您对卡尔曼部分感兴趣，而不是对矩阵编码感兴趣，您可以考虑使用 Eigen 矩阵库。这将帮助您专注于算法，而不是他们自己的矩阵计算。

【参考方案1】：

有一点需要考虑，它会影响对 ypur 问题的回答，即您想要制作的过滤器有多“通用”。

卡尔曼滤波器没有限制测量的采样率是恒定的，也没有限制每次都获得所有测量。唯一的限制是测量以递增的时间顺序出现。如果你想支持这一点，那么你的测量函数将传递可变大小的数组，并且 H 和 R 矩阵也将具有可变大小，而且 F 和 Q 矩阵（虽然是恒定形状）需要知道时间更新——特别是你需要一个函数来计算 Q。

作为我的意思的一个例子，考虑某种测量船的例子，它有一个每秒给出一个位置的 dgps 传感器，一个每秒给出两次船航向的陀螺罗盘，以及一个能给出每两秒一个拖曳浮标的范围和方位。在这种情况下，我们可以得到这样的测量结果：

at 0.0 gyro and dgps
at 0.0 gyro
at 1.0 gyro and dgps
at 1.5 gyro
at 2.0 gyro, dgps and rgps

等等。所以我们在不同的时间得到不同数量的观察结果。

在不同的主题上，我总是发现有一种方法可以查看过滤器的效果。有点令人惊讶的是，状态协方差矩阵并不是一种看待这一点的方式。在线性（而不是扩展）过滤器中，可以在看到任何数据之前计算所有时间的状态协方差！这对于扩展的情况是不正确的，因为状态协方差通过测量雅可比依赖于状态，但这是对观察的非常弱的依赖。我认为最有用的质量衡量标准是那些基于测量结果的衡量标准。容易计算的是“创新”——测量值与使用预测状态计算的值之间的差异——以及残差——测量值与使用更新状态计算的值之间的差异。随着时间的推移，这些中的每一个都应该具有均值 0。如果您想变得更漂亮，则可以使用归一化残差。如果 ita 是创新向量，则归一化残差是

T = inv(S)
u = T*ita
nr[i] = u[i]/sqrt( T[i][i])

归一化残差的好处是，每个（随着时间的推移）都应该有均值 0，但也应该有 sd 1——如果过滤器调整正确的话。

【讨论】：

哦，我明白了，那么在变量测量数组的第一个点上，您的意思是在一个实例上可能没有测量值，而下一个实例可能有多个？所以我必须考虑这个事实？还要感谢有关检查过滤器性能的建议，我肯定也会尝试实现它。谢谢！