学习Kalman Fusion的源代码，解释性代码片段

目前，我正在阅读《特伦先生：概率机器人》一书。我发现理解过滤器的概念确实很有帮助，但是我想在例如中看到一些代码。 Matlab。是否值得购买《面向卡尔曼的初学者过滤器：MATLAB示例》一书，或者您是否建议其他来源来学习代码片段？

#1 楼

我读过这本书，发现它不必要地变得晦涩难懂。不幸的是，这些代码片段并不是很有用，因为当使用诸如Eigen，OpenCV，boost或仅Matlab之类的矩阵库时，它们看起来可能与方程式完全相同。为了更好地了解卡尔曼滤波器，您应该首先回顾多元高斯随机变量，然后回顾泰勒级数展开式，然后认识到我们只是为测量和状态的联合分布发明了协方差矩阵。

例如，这是我为使用EKF的机器人漫游车编写的一些代码。


void DifferentialKinematics::ekfMeasurement(Eigen::MatrixXd&X,Eigen::MatrixXd&Q,Eigen::MatrixXd&Z,Eigen::MatrixXd&R){
  Eigen::MatrixXd I=Eigen::MatrixXd::Identity(3,3);
  Eigen::MatrixXd ZP = Eigen::MatrixXd(Z);
  Eigen::MatrixXd XP = Eigen::MatrixXd(X);
  Eigen::MatrixXd z = Z-X;
  Eigen::MatrixXd S =Q+R;

  Eigen::MatrixXd K=Q*S.inverse();

  X = X + K*(z);
  Q = (I-K)*Q;
}
/**
 * Update given wheel speeds left, right.
 * Both are in/out
 */
void DifferentialKinematics::ekfPredict(float left, float right, Eigen::MatrixXd&pose, Eigen::MatrixXd&cv){
  float hth=pose(2)+getTheta(left,right)/2;
  float dd = std::fabs(getVel(left,right));
  updatePose(left,right,pose);

  //Form Jacobian
  Eigen::MatrixXd V=Eigen::MatrixXd::Zero(3,2);
  V(0,0)=(cos(hth)+(dd/width)*sin(hth))/2;
  V(0,1)=(cos(hth)-(dd/width)*sin(hth))/2;
  V(1,0)=(sin(hth)-(dd/width)*cos(hth))/2;
  V(1,1)=(sin(hth)+(dd/width)*cos(hth))/2;
  V(2,0)=(-1/width);
  V(2,1)=(1/width);

  Eigen::MatrixXd G;
  G=Eigen::MatrixXd::Identity(3,3);
  G(0,2)=-dd*sin(hth);
  G(1,2)=cos(hth)*dd;

  Eigen::MatrixXd M=Eigen::MatrixXd::Zero(2,2);
  M(0,0)=left &lt 0? -1*left*kl:left*kl;
  M(1,1)=right &lt 0? -1*right*kr:right*kr;

  Eigen::MatrixXd R=V*M*V.transpose();
  Eigen::MatrixXd r2 = G*cv*G.transpose()+R;
  cv = r2; 
  return;
}

$ \ begingroup $
我在最后一年的论文中实施了EKF。我从来没有感觉到我真正了解自己在做什么，因为数学似乎已经超出了我的脑海。现在我知道为什么了，我从未听说过您提到的任何术语：）
$ \ endgroup $
– ThomasH
13年3月13日在18:30

#2 楼

我发现最有用的两个链接是用于Undergrads1的卡尔曼滤波器和用于假人的卡尔曼滤波器。他们在理论上并不高。和学生戴夫的卡尔曼滤波教程。最后一个具有可玩的matlab代码，并且易于遵循。

#3 楼

如果您正在寻找一种可以直观地了解卡尔曼滤波器实际功能的资源，我建议您参考位于此处的Udacity机器人人工智能课程的第二课： / course / cs373。

该课程是免费的，其中包含视频讲座和简单的代码示例。给出的解释有些简单，但确实有助于建立关于过滤器如何工作以及为什么起作用的直觉。

#4 楼

一旦真正了解了卡尔曼方程式，您就可以轻松地将方程式/模型转换为代码。我强烈建议（手工）在讲座主题MI63：卡尔曼过滤罐加注-卡尔曼过滤器应用中使用示例。提供跟踪随机变量的具体示例。

在UNC-Chapel Hill，COMP 145-团队18：卡尔曼过滤器学习工具-动态和测量模型。

您可以使用Excel或Matlab这样的简单示例来实现示例。但是，如果您打算在C ++中实现Kalman方程，我建议使用Eigen软件包，如前面提到的Josh。

$ \ begingroup $
感谢您的回答，凯文（Kevin），欢迎来到机器人技术。提供URL的名称以及URL的名称很有用，以防URL死掉，这样人们可以尝试通过谷歌（学者）等来找到这些文件。我已经编辑了答案，为您添加了这些。
$ \ endgroup $
– Mark Booth♦
13年5月25日在1:43

#5 楼

几年来，我一直在收集有关卡尔曼滤波和目标跟踪的书籍。它被广泛认为是该领域的经典之作。它也相对便宜。

第二个地方是布鲁克纳的跟踪和简化的卡尔曼滤波。它主要针对雷达处理。他费了很大的劲才能首先解释更简单的跟踪滤波器，然后证明卡尔曼滤波器并不是什么魔术，只是提出滤波器系数的一种更灵活的方法。

他们解决了多目标跟踪的关联问题。布莱克曼的雷达应用多目标跟踪是一个不错的开始。与Artech House的所有书籍一样，新书也很昂贵。亚马逊表示，有一些廉价的二手书可供使用。目标跟踪。对于将传感器报告与现有轨道关联和/或在弹出新消息时启动新轨道的问题，这是一种可靠的方法。您需要解决关联问题以进行多目标跟踪。不幸的是，至少对我来说，里德的论文几乎是难以理解的。（是的，有一些更简单的方法，但是它们不那么健壮。）

#6 楼

卡尔曼滤波器基于一些假设-具有独立正态分布噪声的线性过程。噪声随时间变化是独立的。每个度量也都是独立的。本文应该足以允许您实现卡尔曼滤波器。互联网上也存在其他来源。这里的大多数人将无法对您提到的书发表意见-因为我们还没有阅读，我也永远不会阅读，因为我不需要。特别是详细的方程式，显示了卡尔曼滤波器采取的每个步骤。特别是给定的方程式实现了一个滤波器，该滤波器允许协方差（测量中的噪声）在不同的时间步上不同，并保持估计状态的协方差（精度）的更新估算值。

还有一个

实现卡尔曼滤波器的另一种方法是将其固定为状态空间线性时不变滤波器。相关方程式为$ x（k）= x（k-1）+ Ky（k）$。这是对整个方程组的简化，因为如果协方差固定，则$ K $-卡尔曼增益固定。也可以从离散时间代数Ricatti方程的解中获得增益。

#7 楼

我有偏见（因为我已经两次上过他的课程），但是我认为Marin Kobilarov教授对KF / EKF的推导远胜于Thrun的推论。我已经阅读了先前答案中提到的几乎所有有关KF / EKF的书，并且我认为Marin的讲义对于最佳状态估计的理论方面非常有用。不过，Thrun的演讲远非最糟糕的。 Marin方法的优点在于，他从优化的角度开发了KF测量更新：他从简单的最小二乘法开始，经历了线性和非线性测量模型最优的必要条件和充分条件。

该课程的网站上还有一些易于理解的matlab代码：https://asco.lcsr.jhu.edu/en530-603-f2017-applied-optimal-control/

第10章专门讨论最佳状态估计，即KF / EKF：https：//asco.lcsr.jhu.edu/docs/EN530_603_F2015/lecture10.pdf

PS。在我的口语/素质考试中，我被要求从记忆中手动得出EKF。如果我没有学会上述方法（我觉得很直观），我想我会迷失在数学细节上。与往常一样，YMMV。

编程黑洞网