扩展卡尔曼滤波器被用于从里程计数据和landmark观察值 估计机器人的状态（姿势）。EKF通常被描述为单独的状态估计（机器人被赋予一张完美的地图）。因此，它没有（在SLAM中使用EKF所需的）地图更新。SLAM中，对比一个状态估计EKF尤其矩阵是变化的，很难想出怎样实现，由于它几乎从未被提到过。我们将仔细检查每一步。大部分EKF是标准的，一旦建立矩阵，基本上仅是一组方程式。
过程的概述
一旦landmark extraction和数据联合到位，SLAM过程可看作三步：
- 使用里程计数据更新当前状态估计
- 从重新观察的landmarks更新估计的状态
- 添加新landmarks至当前状态
第一步很简单。仅是添加一次对机器人的控制到旧状态估计。也就是，机器人在点(x,y)带有旋转θ，并且控制量是(dx,dy)，旋转变化是dθ。第一步的结果是机器人的新状态：(x+dx,y+dy)，旋转θ+dθ。
第二步应考虑重新观察的landmarks。使用当前姿势的估计，有可能估计landmark应该在哪个位置。通常会有些差别，这叫做innovation。所以innovation基本上是估计的机器人姿势和真实的机器人姿势之间的差别，取决于机器人能看见什么。在第二步，每个观察到landmark的不确定性同样被更新来反映最近的变化。一个例子是如果当前landmark位置的不确定性很小。从这个不确定性很小的位置重新观察一个landmark将增加landmark的确定性，也就是关于当前机器人位置的landmark的方差。
第三步新的landmarks被添加至状态和机器人全局地图。使用当前姿势的信息和添加的关于新landmarks和旧landmarks之间关系的信息完成这一步。
矩阵
系统状态：X
X连同它的协方差矩阵 几乎是系统中最重要的矩阵之一，它包括机器人的姿势，x，y和θ。此外，它包括每个landmark的x和y位置。矩阵如右图所示。垂直矩阵保证所有方程式有效。X的大小是1列，3+2*n行，其中n是landmarks的数量。
通常数值保存为米或毫米的范围。哪个被使用不重要，只要使用相同单位。方位保存为角度或弧度。
协方差矩阵：P
两个变量的协方差提供这两个变量相关联程度的一个度量。相关性的概念被用于测量变量间的线性依赖程度。协方差矩阵P是系统中十分主要的矩阵 。它包括机器人姿势的协方差，landmarks的协方差，机器人姿势和landmarks间的协方差，最后它包括landmarks间的协方差。右图显示协方差矩阵P的内容。第一个单元，A包括机器人姿势的协方差。它是个3乘3矩阵（x，y和θ）。B是第一个landmark的协方差。它是个2乘2矩阵，因为landmark没有方向θ。一直延续至C，它是最后一个landmark的协方差。单元D包括机器人状态和第一个landmark间的协方差。单元E包括第一个landmark和机器人状态间的协方差。E能通过置换子矩阵D推断得到。F包括最后一个landmark和第一个landmark间的协方差，G包括第一个landmark和最后一个landmark间的协方差，同样 它能通过置换F推断得到。所以尽管协方差矩阵可能看上去复杂，它实际上是系统地建立起来的。最初机器人没有看到任何landmarks，协方差矩阵P只包含矩阵A。协方差矩阵必须使用一些对角线的缺省值进行初始化。这反映在初始位置的不确定性。根据具体实施情况，如果在一些计算中未包含初始不确定性，通常会发生奇异，所以包含一些初始误差是一个好主意，尽管有理由相信初始机器人姿势是确切的。
卡尔曼增益：K
卡尔曼增益K计算我们有多少信任观察到的landmark，和我们希望从他们提供的新的知识中获得多少。根据landmarks，如果我们能看到机器人应该向右移动10厘米，我们使用卡尔曼增益来找出我们实际修正姿势的多少，可能只有5厘米，因为我们不完全信任landmarks，而是寻找里程计和landmark修正之间的折衷。使用观察到的landmarks的不确定性连同测距仪品质的度量 和机器人的里程计性能完成这一步。如果测距仪相比机器人里程计性能实在太差，我们当然不太信任它，所以卡尔曼增益会很低。相反，如果测距仪比机器人里程计性能好很多，卡尔曼增益会很高。矩阵如右图所示。第一行显示应该从状态X的第一行的innovation获得多少增益。第一行第一列描述应该从innovation的距离方面获得多少增益，第一行第二列描述应该从innovation的方位方面获得多少增益。两者都对应状态的第一行，机器人姿势的x值。矩阵通过机器人姿势延续；前三行，landmarks；新的每两行。矩阵的大小是2列和3+2*n行，其中n是landmarks的数量。
测量模型H的雅可比行列式
测量模型H的雅可比行列式和测量模型密切相关，当然，所以先仔细检查测量模型。测量模型定义了怎样计算一个预期的测量值（观察到的landmark位置）的距离和方位。使用下列公式实现，用h表示：
其中lambda x是landmark的x位置，x是当前估计的机器人的x位置，lambda y是landmark的y位置，y是当前估计的机器人的y位置。θ是机器人旋转。这将给我们预测的 到landmark距离和方位的测量值。这个矩阵的关于x，y和θ的雅可比行列式，H是：
H告诉我们当x，y和θ变化时，距离和方位变化多少。第一行的第一个元素是距离关于x轴的变化。第二个元素是关于y轴的变化。最后一个元素是关于θ的变化。当然这个值是零，因为当机器人旋转时距离没有变化。第二行给出同样的信息，除了这是对于landmark方位的变化。这是普通的EKF状态估计H的内容。当进行SLAM时我们需要为landmarks提供一些额外的值。
预测模型的雅可比行列式：A
就像H，预测模型的雅可比行列式和预测模型密切相关，当然，所以我们先仔细检查预测模型。预测模型定义了给定旧姿势和控制输入，怎样计算一个预测的机器人姿势。使用下列公式完成，用f表示：
其中x和y是机器人位置，θ是机器人旋转，Δt是推力的变化，q是误差项。我们直接使用来自ER1系统里程计输入的位置的变化，所以我们直接使用x，y，θ和Δx，Δy和Δθ，以及过程噪声，在之后描述：
总之，当计算雅可比行列式A时，我们假定线性化的版本为：
计算和H矩阵相同，除了现在又多一行对应机器人旋转量。由于它仅用于机器人位置估计，它同样不会扩展余下的landmarks。 从第一个矩阵可以看出，预测模型，-Δt*sinθ即-Δy，-Δt*cosθ即-Δy。所以我们能使用控制项，为：
SLAM特定雅可比行列式Jxr和Jz
当进行SLAM，有些只用于SLAM的雅可比行列式。这当然在融合新特征，唯一步骤区别于使用EKF的常规状态估计。第一个是Jxr。它基本上和预测模型的雅可比行列式相同，除了没有旋转项。它是landmarks的预测雅可比行列式，没有包括θ的预测，关于从X（获得的）机器人状态[x，y，θ]：
雅可比行列式Jz同样是landmarks的预测模型的雅可比行列式，但这次关于[距离，方位]。即：
634
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
