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

Question

我想使用特征库在 C++ 中实现扩展卡尔曼滤波器，因为我对机器人技术很感兴趣，这似乎是一个很好的练习，可以提高 C++ 的水平，而且它似乎是一个有趣的项目。我希望我可以 post 我的代码获得一些关于编写 classes 的反馈以及我的下一步应该从这里开始。所以我从 class 在线

得到了方程式

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

我应该将测量更新和状态预测作为两个不同的功能吗？真的有关系吗？我首先这样做是因为我认为它更容易阅读。
我应该在 class 构造函数中设置诸如状态向量之类的东西的大小，还是为它设置诸如初始化函数之类的东西更好？
我读到更好的做法是让 class 矩阵成员实际上是指向矩阵的指针，因为它使 class 更轻。这是什么意思？如果我想在 PC 上运行与 raspberry pi 之类的东西相比，这重要吗？
measurementUpdate函数中，y,S,K应该是class成员吗？它会使 class 变大，但我不会在程序循环时构造和破坏 Eigen objects 吗？这是好的做法吗？
是否应该有一个 class 成员接受测量输入，还是只将一个值传递给测量更新函数更好？重要吗？
为此尝试实现一个 class 是否值得，还是只有一个实现过滤器的函数更好？
删除了这个，因为它不是问题。
我正在考虑实现一些 getter 函数，这样我就可以检查状态变量和协方差矩阵，只让这些成员 public 而没有getter 个函数？

抱歉，如果 post 发错地方了，如果这些是非常基本的问题，我对这些东西还很陌生。感谢所有帮助，感谢所有建议。

header:

#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

要考虑的一件事会影响您对问题的回答，那就是如何'generic'您要制作的过滤器。

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

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

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

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

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

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

归一化残差的好处在于，每个（随着时间的推移）均值应该为 0，而且 sd 为 1——如果过滤器调整正确。

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

Implementing Kalman filter in C++

c++

robotics

kalman-filter