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

Question

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

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

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

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

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

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

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

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

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

8. 我正在考虑实现一些 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();
} 

Answer 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——如果过滤器调整正确的话。