网上的资料永远都是参差不齐，经典的卡尔曼滤波让我折腾好久都没完全搞懂，现在总算找到点门路，特此记录下，后附原文来源。

利用卡尔曼滤波我们可以做什么？
我们举一个玩具的栗子：你开发了一款小型机器人，它可以在树林里自主移动，并且这款机器人需要明确自己的位置以便进行导航。

我们可以通过一组状态变量  来描述机器人的状态，包括位置和速度：

注意这个状态仅仅是系统所有状态中的一部分，你可以选取任何数据变量作为观测的状态。在我们这个例子中选取的是位置和速度，它也可以是水箱中的水位，汽车引擎的温度，一个用户的手指在平板上划过的位置，或者任何你想要跟踪的数据。
我们的机器人同时拥有一个GPS传感器，精度在10m。这已经很好了，但是对我们的机器人来说它需要以远高于10m的这个精度来定位自己的位置。在机器人所处的树林里有很多溪谷和断崖，如果机器人对位置误判了哪怕只是几步远的距离，它就有可能掉到坑里。所以仅靠GPS是不够的。

同时我们可以获取到一些机器人的运动的信息：驱动轮子的电机指令对我们也有用处。如果没有外界干扰，仅仅是朝一个方向前进，那么下一个时刻的位置只是比上一个时刻的位置在该方向上移动了一个固定距离。当然我们无法获取影响运动的所有信息：机器人可能会受到风力影响，轮子可能会打滑，或者碰到了一些特殊的路况；所以轮子转过的距离并不能完全表示机器人移动的距离，这就导致通过轮子转动预测机器人位置不会非常准确。
GPS传感器也会告知我们一些关于机器人状态的信息，但是会包含一些不确定性因素。我们通过轮子转动可以预知机器人是如何运动的，同样也有一定的不准确度。
如果我们综合两者的信息呢？可以得到比只依靠单独一个信息来源更精确的结果么？答案当然是YES，这就是卡尔曼滤波要解决的问题。

卡尔曼滤波如何看待你的问题
我们再来看下需要解决的问题，同样是上边的系统，系统状态包括位置和速度。

我们不知道位置和速度的准确值；但是我们可以列出一个准确数值可能落在的区间。在这个范围里，一些数值组合的可能性要高于另一些组合的可能性。

卡尔曼滤波假设所有的变量（在我们的例子中为位置和速度）是随机的且符合高斯分布（正态分布）。每个变量有一个平均值ｕ，代表了随机分布的中心值（也表示这是可能性最大的值），和一个方差  ，代表了不确定度。
在现实中，速度和位置是有关联的。如果已经确定位置的值，那么某些速度值存在的可能性更高。
假如我们已知上一个状态的位置值，现在要预测下一个状态的位置值。如果我们的速度值很高，我们移动的距离会远一点。相反，如果速度慢，机器人不会走的很远。
这种关系在跟踪系统状态时很重要，因为它给了我们更多的信息：一个测量值告诉我们另一个测量值可能是什么样子。这就是卡尔曼滤波的目的，我们要尽量从所有不确定信息中提取有价值的信息！
这种关系可以通过一个称作协方差的矩阵表述。简而言之，矩阵中的每个元素  表示了第i个状态变量和第j个状态变量之间的关系。（你可能猜到了协方差矩阵是对称的，即交换下标i和j并无任何影响）。协方差矩阵通常表示为Σ，它的元素则表示为  。。

利用矩阵描述问题
我们对系统状态的分布建模为高斯分布，所以在k时刻我们需要两个信息：最佳预估值  （平均值，有些地方也表示为u），和它的协方差矩阵 

(这里我们只记录了位置和速度，但是我们可以把任何数据变量放进我们的系统状态里)
下一步，我们需要通过k-1时刻的状态来预测k时刻的状态。请注意，我们不知道状态的准确值，但是我们的预测函数并不在乎。它仅仅是对k-1时刻所有可能值的范围进行预测转移，然后得出一个k时刻新值的范围。

我们可以通过一个状态转移矩阵  来描述这个转换， 把k-1时刻所有可能的状态值转移到一个新的范围内，这个新的范围代表了系统新的状态值可能存在的范围，如果k-1时刻估计值的范围是准确的话。
通过一个运动公式来表示这种预测下个状态的过程：

整理为矩阵:

我们现在有了一个状态转移矩阵，可以简单预测下个状态，但仍不知道如何更新协方差矩阵。
这里我们需要另一个公式。如果我们对每个点进行矩阵A转换，它的协方差矩阵Σ会发生什么变化呢？
Easy，直接告诉你结果。

结合（4）和（3）：

外界作用力
我们并没有考虑到所有影响因素。系统状态的改变并不只依靠上一个系统状态，外界作用力可能会影响系统状态的变化。
例如，跟踪一列火车的运动状态，火车驾驶员可能踩了油门使火车提速。同样，在我们机器人例子中，导航软件可能发出一些指令启动或者制动轮子。如果我们知道这些额外的信息，我们可以通过一个向量  来描述这些信息，把它添加到我们的预测方程里作为一个修正。
假如我们通过发出的指令得到预期的加速度a，上边的运动方程可以变化为：

矩阵形式：

 称作控制矩阵，  称作控制向量（没有任何外界动力影响的系统，可以忽略该项）。

我们增加另一个细节，假如我们的预测转换矩阵不是100%准确呢，会发生什么？
外界不确定性
如果状态只会根据系统自身特性演变那将不会有任何问题。如果我们可以把所有外界作用力对系统的影响计算清楚那也不会有任何问题。
但是如果有些外力我们无法预测呢？假如我们在跟踪一个四轴飞行器，它会受到风力影响。如果我们在跟踪一个轮式机器人，轮子可能会打滑，或者地面上的突起会使它降速。我们无法跟踪这些因素，并且这些事情发生的时候上述的预测方程可能会失灵。
我们可以把“世界”中的这些不确定性统一建模，在预测方程中增加一个不确定项。

这样，原始状态中的每一个点可以都会预测转换到一个范围，而不是某个确定的点。可以这样描述：  中的每个点移动到一个符合方差  的高斯分布里。另一种说法，我们把这些不确定因素描述为方差为  的高斯噪声。

这会产生一个新的高斯分布，方差不同，但是均值相同。

对  简单叠加，可以拿到扩展的方差，这样就得到了完整的预测转换方程。

新的预测转换方程只是引入了已知的可以预测的外力影响因素。
新的不确定性可以通过老的不确定性计算得到，通过增加外界无法预测的、不确定的因素成分。

此处有些疑惑： 是外部环境影响的界定，那对于加速度的不确定性是包含在 里还是里面？
到这里，我们得到了一个模糊的估计范围，一个通过  和  描述的范围。如果再结合我们传感器的数据呢？

通过测量值精炼预测值
我们可能还有一些传感器来测量系统的状态。目前我们不用太关心所测量的状态变量是什么。也许一个测量位置一个测量速度。每个传感器可以提供一些关于系统状态的数据信息，每个传感器检测一个系统变量并且产生一些读数。

注意传感器测量的范围和单位可能与我们跟踪系统变量所使用的范围和单位不一致。我们需要对传感器做下建模：通过矩阵 将状态变量转换成与传感器同样的单位。例如状态变量是记录x与y值，而传感器记录的是直接距离，那么就需要将状态变量x与y求平方和的开根号。

我们可以得到状态量对观测量转变的函数：

卡尔曼滤波也可以处理传感器噪声。换句话说，我们的传感器有自己的精度范围，对于一个真实的位置和速度，传感器的读数受到高斯噪声影响会使读数在某个范围内波动。

我们观测到的每个数据，可以认为其对应某个真实的状态。但是因为存在不确定性，某些状态的可能性比另外一些可能性更高。

我们将这种不确定性的方差为描述为  。读数的平均值为  。

也就是

所以现在我们有了两个高斯分布，一个来自于我们预测值，另一个来自于我们测量值。

两个分布是独立同分布的，如何融合两个高斯分布呢？那我们概率论就有接触过。

高斯乘法
我们从一维数据开始，一维高斯（均值u,方差  ）被定义为：

我们想知道两个高斯分布相乘会发生什么。蓝色曲线代表了两个高斯分布的交集部分。

把（9）带入（10）然后做一些变换，可以得到

因式分解出一个部分，表示为k

注意你是如何将处理之前的预测值，仅仅是简单将两者叠加相乘就可以得到新的预测值。现在看下这个公式是多么简单。
如果是一个多维矩阵呢？我们将（12）与（13）表示为矩阵形式。Σ表示协方差矩阵，  表示平均向量：

K被称为卡尔曼增益，待会会用到。
简单，我们快结束了。

综合所有信息
我们有两个独立的维度去估计系统状态：
预测值

测量值

将两者相乘带入（15）寻找他们的重叠区域：

从（14）可知，卡尔曼增益

将（16）中的  从两边约去，注意（17）中的K也包含  。得到

至此，我们得到了每个状态的更新步骤  是我们最佳的预测值，我们可以持续迭代（独立于 ）。

以上是对卡尔曼一个全面而又简单的理解，但是如果是非线性关系呢？（这部分说明相对简略，东西有点多，懒得都整理过来，只要理解上面一部分，下面应该容易理解了）

KF的假设之一就是高斯分布的x预测后仍服从高斯分布，高斯分布的x变换到测量空间后仍服从高斯分布。可是，假如F、HF、H是非线性变换，那么上述条件则不成立。

显然, 线性代数是非常棒的，它让我们可以用一种非常简洁的形式来表示像卡尔曼滤波这样的复杂算法。然而，线性代数并不是全部。顾名思义，线性代数仅限于表示线性关系，即, 以直线为特征的关系。要看到这一点，让我们再来看一下矩阵和向量乘法的简单例子:

但是现实中存在大量的无法直接通过类似这样的线性矩阵来表述变量前后的关系。

比方一个简单的线性方程是这样更新的，其中  为外界扰动项。

那么非线性是这样更新的

真实世界中，还可能是这样的

那么我们就需要将非线性转到线性，才能使用卡尔曼滤波，这时候又要用上泰勒展开了

这时候再举一个例子理解会更深刻一些

毫米波雷达的数据

毫米波雷达观察世界的方式与激光雷达有所不同。激光雷达测量的原理是光的直线传播，因此在测量时能直接获得障碍物在笛卡尔坐标系下x方向、y方向和z方向上的距离；而毫米波雷达的原理是多普勒效应，它所测量的数据都是在极坐标系下的。

如下图所示，毫米波雷达能够测量障碍物在极坐标下离雷达的距离ρ、方向角ϕ以及距离的变化率（径向速度）ρ'，如下图所示。

扩展卡尔曼滤波器理论

我们再次祭出卡尔曼老先生给我们留下的宝贵财富，即状态估计时预测和测量值更新时所用到的7个公式，如下图所示。扩展卡尔曼滤波的理论和编程依旧需要使用到这些公式，相比于原生的卡尔曼滤波，只在个别地方有所不同。

我们在做卡尔曼的时候最关键就是找出状态转换与测量的两个高斯分布的均值和方差，然后就可以套公式了。

我们第一个方程状态转换的分布仍然是

，

需要将此时的状态单位转到与测量一样的单位

测量的分布则为

因为是摘录别的文章省得编辑，所以直接用以下形式来表述这个过程

初始化（Initialization）

扩展卡尔曼滤波的初始化，需要将各个变量进行设置，对于不同的运动模型，状态向量是不一样的。

初始化扩展卡尔曼滤波器时需要输入一个初始的状态量x_in，用以表示障碍物最初的位置和速度信息，一般直接使用第一次的测量结果。

预测（Prediction）

完成初始化后，我们开始写Prediction部分的代码。首先是公式

这里的x为状态向量，通过左乘一个矩阵F，再加上外部的影响u，得到预测的状态向量x'。这里的F被称作状态转移矩阵（state transistion matrix）。以2维的匀速运动为例，这里的x为

根据中学物理课本中的公式s1 = s0 + vt，经过时间△t后的预测状态向量应该是

对于二维的匀速运动模型，加速度为0，并不会对预测后的状态造成影响，因此

如果换成加速或减速运动模型，可以引入加速度ax和ay，根据s1 = s0 + vt + at^2/2这里的u会变成：

作为入门课程，这里不讨论太复杂的模型，因此公式

最终将写成

再看预测模块的第二个公式

该公式中P表示系统的不确定程度，这个不确定程度，在卡尔曼滤波器初始化时会很大，随着越来越多的数据注入滤波器中，不确定程度会变小，P的专业术语叫状态协方差矩阵（state covariance matrix）；这里的Q表示过程噪声（process covariance matrix），即无法用x'=Fx+u表示的噪声，比如车辆运动时突然到了上坡，这个影响是无法用之前的状态转移方程估计的。

毫米波雷达测量障碍物在径向上的位置和速度相对准确，不确定度较低，因此可以对状态协方差矩阵P进行如下初始化：

由于Q对整个系统存在影响，但又不能太确定对系统的影响有多大。对于简单的模型来说，这里可以直接使用单位矩阵或空值进行计算，即

根据以上内容和公式

观测（Measurement）

观测的第一个公式是

这个公式的目的是计算观测到的观测值z与预测值x'之间差值y。

前面提到过毫米波雷达观测值z的数据特性，如下图所示：

由图可知观测值z的数据维度为3×1，为了能够实现矩阵运算，y和Hx'的数据维度也都为3×1。

使用基本的数学运算可以完成预测值x'从笛卡尔坐标系到极坐标系的坐标转换，求得Hx'，再与观测值z进行计算。需要注意的是，在计算差值y的这一步中，我们通过坐标转换的方式避开了未知的旋转矩阵H的计算，直接得到了Hx’的值。

为了简化表达，我们用px，py以及vx和vy表示预测后的位置及速度，如下所示：

其中测量值z和预测后的位置x'都是已知量，因此我们很容易计算得到观测与预测的差值y。

毫米波雷达在转换时涉及到笛卡尔坐标系和极坐标系的位置、速度转换，这个转化过程是非线性的。因而在处理类似毫米波雷达这种非线性的模型时，习惯于将计算差值y的公式写成如下，以作线性和非线性模型的区分。

对应上面的式子，这里的h(x')为：

再看卡尔曼滤波器接下来的两个公式

这两个公式求的是卡尔曼滤波器中一个很重要的量——卡尔曼增益K（Kalman Gain），用人话讲就是求差值y的权值。第一个公式中的R是测量噪声矩阵（measurement covariance matrix），这个表示的是测量值与真值之间的差值。一般情况下，传感器的厂家会提供。如果厂家未提供，我们也可以通过测试和调试得到。S只是为了简化公式，写的一个临时变量，不用太在意。

由于求得卡尔曼增益K需要使用到测量矩阵H，因此接下来的任务就是得到H。

毫米波雷达观测z是包含位置、角度和径向速度的3x1的列向量，状态向量x'是包含位置和速度信息的4x1的列向量，根据公式y=z-Hx'可知测量矩阵（Measurement Matrix）H的维度是3行4列。即：

从上面的公式很容易看出，等式两边的转化是非线性的，并不存在一个常数矩阵H，能够使得等式两边成立。

如果将高斯分布作为输入，输入到一个非线性函数中，得到的结果将不再符合高斯分布，也就将导致卡尔曼滤波器的公式不再适用。因此我们需要将上面的非线性函数转化为近似的线性函数求解。

在大学课程《高等数学》中我们学过，非线性函数y=h(x)可通过泰勒公式在点(x0,y0)处展开为泰勒级数：

忽略二次以上的高阶项，即可得到近似的线性化方程，用以替代非线性函数h(x)，即：

将非线性函数h(x)拓展到多维，即求各个变量的偏导数，即：

对x求偏导数所对应的这一项被称为雅可比（Jacobian）式。

我们将求偏导数的公式与我们的之前推导的公式对应起来看x的系数，会发现这里的测量矩阵H其实就是泰勒公式中的雅可比式。

多维的雅可比式的推导过程有兴趣的同学可以自己研究一下，这里我们直接使用其结论：

求得非线性函数h(x')对px，py，vx，vy的一阶偏导数，并排列成的矩阵，最终得到雅克比（Jacobian）矩阵H：

其中

接下来就是考验各位高等数学求偏导数功底的时候了！

经过一系列计算，最终得到测量矩阵H为：

根据以上公式可知，在每次预测障碍物的状态后，需要根据预测的位置和速度计算出对应的测量矩阵H，这个测量矩阵为非线性函数h(x')在x'所在位置进行求导的结果。

此处有些疑惑：下面这个公式中这个H是直接使用了雅克比矩阵，还是说泰勒展开式当中其他项也要计算进去？

再看卡尔曼滤波器的最后两个公式

这两个公式，实际上完成了卡尔曼滤波器的闭环，第一个公式是完成了当前状态向量x的更新，不仅考虑了上一时刻的预测值，也考虑了测量值，和整个系统的噪声，第二个公式根据卡尔曼增益K，更新了系统的不确定度P，用于下一个周期的运算，该公式中的I为与状态向量同维度的单位矩阵。

就此卡尔曼与扩展卡尔曼已经全部讲完，相信看完之后会有很好的理解。