扩展卡尔曼滤波新手教程（一）----中文版
扩展卡尔曼滤波新手教程（一）
说明：本文内容翻译自外文网站The Extended Kalman Filter: An Interactive Tutorial for Non-Experts，仅供学习和参考。
在和无人机系统打交道的过程中，我频繁地接触到关于扩展卡尔曼滤波器（Extended Kalman Filter EKF）的相关内容。Google这个术语查到的大量资料让我感到很费解，所以我决定创建自己的教程用于EKF的学习和教学。不过，Kalman Filtering for Dummies和the Wikipedia page是两个重要的例外，本教程借鉴了它们中的很多内容，但是不得不说，本教程会比它们更加生动、有趣。本教程也得益于OpenPilot和DIY Drones社区成员的帮助，另外，Tim Wilkin帮作者纠正了很多不够准确的表述。
本教程假设读者只有高中阶段数学基础，因此会对用到的一些较高级数学知识，如线性代数等进行必要的介绍。首先让我们从几个简单的例子和标准（线性）卡尔曼滤波器开始，逐渐深入学习实际的扩展卡尔曼滤波器的原理和实现。
Part1：一个简单的例子
假设一个飞机即将着陆。尽管我们可能需要担心很多事情,像空气速度,燃料等,但是最应该关心的是飞机的海拔高度。通过一个非常简单的近似,我们可以认为飞机的当前高度是前一时刻高度和一个百分数的乘积。例如,如果飞机在我们每次观察它时，下降了它前一次高度的2%，那么它在当前时间的海拔高度是前一时刻高度的98%。：
a l t i t u d e c u r r e n t _ t i m e ( z ) = 0.98 ∗ a l t i t u d e p r e v i o u s _ t i m e altitude_{current\_time} (z) =0.98*altitude_{previous\_time} altitudecurrent_time(z)=0.98∗altitudeprevious_time
工程师一般使用术语递归来表示上面这样一个公式，因为一个量的当前值由它的上一个值决定。这意外着，最终要计算当前值，我们必须“回溯”到上一个值。最终，我们回溯到某个初始的“基本情况”，例如上面的例子中，是飞机开始下降时的起始高度，如下图所示。
Part2：处理噪声
当然，在真实世界中，像海拔高度的测量是通过如GPS或气压计这样的传感器完成的。这些传感器往往具有不同的精度。例如，For example, Garmin 公司将其测高计产品的输出精度表述为“10英尺“。因此，可以说，如果测高计读到1000英尺的输出结果，那么我们的实际高度可能是990到1010英尺的任意值。如果传感器输出始终保持偏移一个常数值，我们就可以简单地加或减这个常数值来决定我们的海拔高度。然而实际上，传感器的精确度会不断地发生无法预知的变化，从而导致我们观测到的传感器读数始终是真实海拔高度的带有噪声的版本：
o b s e r v e d _ a l t i t u d e c u r r e n t t i m e = a l t i t u d e c u r r e n t _ t i m e + n o i s e c u r r e n t _ t i m e observed\_altitude_{current_time} = altitude_{current\_time} + noise_{current\_time} observed_altitudecurrenttime=altitudecurrent_time+noisecurrent_time
我们可以将测量噪声表示为可观测高度范围的百分比，若在观测过程中伴随了相对测量值10%的噪声，则此时的海拔高度的观测值随时间的变化如下图所示：
Part3：合并等式
现在我们有两个等式来描述飞机的状态：
a l t i t u d e c u r r e n t _ t i m e ( z ) = 0.98 ∗ a l t i t u d e p r e v i o u s _ t i m e o b s e r v e d _ a l t i t u d e c u r r e n t t i m e = a l t i t u d e c u r r e n t _ t i m e + n o i s e c u r r e n t _ t i m e altitude_{current\_time} (z) =0.98*altitude_{previous\_time} \\ observed\_altitude_{current_time} = altitude_{current\_time} + noise_{current\_time} altitudecurrent_time(z)=0.98∗altitudeprevious_timeobserved_altitudecurrenttime=altitudecurrent_time+noisecurrent_time
这两个等式都是非常容易理解的，但是他们还不足以用来表示一般情况下通用的系统情况。为了使他们更通用，工程师们采用了更具普遍性的数学符号来表示这两个等式----使用x,y,z来表示变量，使用a和b表示常量，使用下标 k k k来表示时间(为什么不用 t t t表示时间？或许是因为在这里，时间被处理成了一系列离散的点，因此 k k k这样的索引类型的变量可能会更加适合)。于是我们的等式就变成了这样：
x k = a ∗ x k − 1 z k = x k + v k x_{k} =a*x_{k-1} \\ z_{k} = x_{k} + v_{k} xk=a∗xk−1zk=xk+vk
其中上述第一个等式我们可以称之为状态方程，这里的 x k x_k xk表示系统的当前状态， x k − 1 x_{k-1} xk−1表示系统的前一状态， a a a是常量（在我们前面的例子中为0.98）， z k z_k zk是系统的当前观测值， v k v_k vk是当前测量噪声值。卡尔曼滤波器如此受欢迎的其中一个原因就是它让我们可以在已知观测值 z k z_k zk、常数 a a a和测量噪声总量 v v v的情况下，得到一个当前真实状态 x k x_k xk的比较好的估计值。
为了更好地完成飞机高度随时间下降的曲线图，我们还应考虑飞机实际的高度变化可能不会是一个平滑的过程。因为任何一个开过飞机的人都会告诉你，当他们驾驶飞机降落时，飞机实际上都会经历一定数量的振荡。这些振荡是由噪声定义的，所以可以被看作是另一种噪声信号：
a l t i t u d e c u r r e n t _ t i m e ( z ) = 0.98 ∗ a l t i t u d e p r e v i o u s _ t i m e + t u r b u l e n c e c u r r e n t _ t i m e altitude_{current\_time} (z) =0.98*altitude_{previous\_time} +turbulence_{current\_time} altitudecurrent_time(z)=0.98∗altitudeprevious_time+turbulencecurrent_time
更具一般性的表示：
x k = a ∗ x k − 1 + w k x_{k} =a*x_{k-1} +w_k xk=a∗xk−1+wk
其中， w k w_k wk称为过程噪声，因为就像飞机的振荡一样，它是过程中固有的部分，不是由观测或测量带来的。为了集中精力研究其他的部分，我们将会暂时忽略过程噪声，但是在传感器融合一节我们将会继续研究过程噪声的相关影响。（我还忽略了状态方程中的控制-信号分量，因为它与大多数卡尔曼滤波方程相切，并且在需要时很容易添加。）
Part4：状态估计
下面是我们之前得到的描述当前观测到的系统状态的两个等式（请暂时忽略过程噪声 w k w_k wk）：
x k = a ∗ x k − 1 z k = x k + v k x_{k} =a*x_{k-1} \\ z_{k} = x_{k} + v_{k} xk=a∗xk−1zk=xk+vk
因为我们的目标是根据观测值 z z z来获取状态量 x x x,因此我们可以将第二个等式改写为：
x k = z k − v k x_{k} = z_{k} - v_{k} xk=zk−vk
现在的问题是我们不知道当前的测量噪声 v k v_k vk：它是无法预期的。幸运的是，Kalman发现，我们可以通过同时考虑当前观测值和上一个估计的系统状态来估计当前系统状态。工程师们在变量上方使用一个类似"帽子"的标志"^"来表示该变量是估计得到的：所以 x k ^ \hat{x_k} xk^就表示当前状态的估计。然后，我们可以将这个估计值表示为上一个估计和当前观测值的折衷：
x k ^ = x k − 1 ^ + g k ^ ( z k ^ − x k − 1 ^ ) \hat{x_k}=\hat{x_{k-1}}+\hat{g_k}(\hat{z_k}-\hat{x_{k-1}}) xk^=xk−1^+gk^(zk^−xk−1^)
这里的 g g g表示折衷的"增益"（变量 k k k也常用于表示增益，因为这能表示该增益是卡尔曼增益，但是由于用同一个字母既表示变量又表示下标容易让人产生疑惑，因此我改用字母 g g g表示增益）。我将这个等式标为红色，因为它是我们在运行卡尔曼滤波器时需要直接使用的一个等式。
现在，这看上去有些复杂，但是实际上我们可以直接考虑增益 g k g_k gk取两个极端值的情况。当 g k = 0 g_k=0 gk=0时，我们得到：
x ^ k = x ^ k − 1 + 0 ( z k − x ^ k − 1 ) = x ^ k − 1 \hat{x}_{k} = \hat{x}_{k-1} +0(z_k- \hat{x}_{k-1} ) = \hat{x}_{k-1} x^k=x^k−1+0(zk−x^k−1)=x^k−1
换言之, 当增益是0时,观测值对状态估计没有任何影响,于是我们当前的状态估计值和上一个状态估计值相等.。对于 g k = 1 g_k=1 gk=1的情况, 我们得到
x ^ k = x ^ k − 1 + 1 ( z k − x ^ k − 1 ) = z k \hat{x}_{k} = \hat{x}_{k-1} +1(z_k- \hat{x}_{k-1} ) = z_k x^k=x^k−1+1(zk−x^k−1)=zk
由上式可知，当增益是1时, 上一个状态对估计没有任何影响, 于是我们得到和当前观测值完全相等的状态估计.
当然, 实际的增益值更有可能落在这两个极端值的中间.。例如，当卡尔曼增益 g k = 0.5 g_k=0.5 gk=0.5，当前观测值 z k = 105 z_k=105 zk=105，上一个系统状态估计值 x k − 1 = 110 x_{k-1}=110 xk−1=110，我们可以得到当前系统状态估计值：
x k ^ = 107.5 \hat{x_k}=107.5 xk^=107.5
Part5: 计算增益
现在我们有了一个我们可以实际使用的公式来根据上一个状态估计值 x ^ k − 1 \hat{x}_{k-1} x^k−1、当前观测值 z k z_k zk和当前增益 g k g_k gk计算当前状态估计值 x ^ k \hat{x}_k x^k：
x ^ k = x ^ k − 1 + g k ( z k − x ^ k − 1 ) \hat{x}_{k} = \hat{x}_{k-1} + g_k(z_k- \hat{x}_{k-1} ) x^k=x^k−1+gk(zk−x^k−1)
所以我们该如何计算增益呢？回答是: 通过噪声间接计算。让我们回忆一下, 每个观测值都是和特定的噪声值相关的:
z k = x k + v k z_k=x_k+v_k zk=xk+vk
我们不知道对于每个观测值的特定噪声值是多少，但是我们一般可以知道平均噪声水平：例如, 传感器的精度指标能够告诉我们其输出噪声的大致水平，我们将这个噪声称为 r r r；它没有下标，因为它和时间无关，它表示的是传感器的属性。然后, 我们可以通过 r r r来计算当前的增益 g k g_k gk:
g k = p k − 1 / ( p k − 1 + r ) g_k=p_{k-1}/(p_{k-1}+r) gk=pk−1/(pk−1+r)
这里的 p k p_k pk是通过递归计算得到的预测误差（技术上讲， r r r实际上是噪声信号的方差；即个别噪音值与其平均值之差的平方和的平均数。尽管若这种噪声方差是随时间变化的，卡尔曼滤波器也能很好地工作，但在大多数应用中，我们可以假定它为常数。同样的， p k p_k pk在技术上是估计过程在步骤 k k k时的协方差；它是我们预测中的平方误差的平均值。实际上，正如蒂姆•威尔金(Tim Wilkin)向我指出的那样，状态是一个随机的(stochastic [random-like])变量/向量（variable/vector），即是随机过程的瞬时值，而且它根本不存在一个“真实”值！对于描述系统状态的过程模型，估计值仅仅是系统状态的最有可能的值！）：
p k = ( 1 − g k ) p k − 1 p_k=(1-g_k)p_{k-1} pk=(1−gk)pk−1
与状态估计公式一样，在继续进行之前，让我们考虑一下这两个公式的含义。
我们先来考虑前一个预测误差 p k − 1 p_{k-1} pk−1为0的情况。这时我们当前的增益 g k g_k gk将为 0 / ( 0 + r ) = 0 0/(0+r)=0 0/(0+r)=0，然后我们的下一个状态估计将会和当前的状态估计相同。这是很有意义的，因为如果我们的估计是准确的，那么我们不应该调整我们的状态估计。当预测误差 p k = 1 p_k=1 pk=1，增益将变为 1 / ( 1 + r ) 1/(1+r) 1/(1+r)。如果 r r r是0----例如如果在系统测量中只有非常小的噪声, 那么增益将为1，我们新的状态估计 x k x_k xk将会完全受观测值 z k z_k zk的影响。但是随着 r r r不断增大, 增益将会变的任意小。换句话说, 当系统噪声足够大时，糟糕的预测将会被忽略。噪声战胜了我们纠正错误预测的能力，我们将继续使用前一个状态估计作为当前的状态估计。
至于第三个公式，如何能根据上一个 p k − 1 p_{k-1} pk−1的值递归计算预测误差 p k p_k pk和当前增益 g k g_k gk?再次, 观察增益极限值的情况有助于我们理解这个问题：当 g k = 0 g_k=0 gk=0时，我们有 p k = p k − 1 p_k=p_{k-1} pk=pk−1。所以，和状态估计类似，增益为0意味着我们不需要更新预测误差。反之，当 g k = 1 g_k=1 gk=1, 我们有 p k = 0 p_k=0 pk=0。因此最大增益对应于零预测错误概率，且仅使用当前观测值来更新当前系统状态。
下面请看本教程的第二部分:扩展卡尔曼滤波新手教程（二）----中文版.