由浅入深对卡尔曼滤波的学习

原文来自http://shequ.dimianzhan.com/articles/337/extended-kalman-filter-course-from-shallow-to-deep

搬运过来好好学习

本篇译文翻译自 The Extended Kalman Filter : An Interactive Turorial for Non-Experts. 原文

本文惯例及说明 ：

1. 译文中的Demo请至原文处运行
2. 最好具有高等数学和线性代数基础
3. 本文基本原理和飞控中的EKF代码实现之间是有差距的

　　在接触 OpenPilot 和 Pixhawk飞控时，我经常遇到关于EKF的参考。我谷歌搜索 EKF，引导我到不同的网页和参考论文，但其中大部分都让我难以理解[1]^{[1]}[1]。所以，我决定亲自创建关于EKF基本原理的教程。这套教程假定你具有高中数学水平，并会在适当时候引入高等数学中的线性代数，而不是假定你已经了解它们。我会从一些简单的例子和标准卡尔曼滤波开始，并最终让你理解 EKF。

Part 1 : 一个简单的例子#

　　想像一架飞机正在降落。虽然我们需要担心的事情有好多，如风速、燃料等等，但最重要的是飞机的高度（海平面高度）。做一个简单的近似，我们认为当前高度是上一时刻高度的一部分。举例来说，如果我们每次观测飞机都下降上一时刻高度的2%，那么当前时刻的高度是上一时刻的 98% :

altitudecurrent_time=0.98∗altitudeprevious_timealtitude_{current\_time}=0.98*altitude_{previous\_time}altitudecurrent_time​=0.98∗altitudeprevious_time​

　　工程师将这种一个量根据上一时刻的值来定义的情形称为递归：为了计算当前值，我们必须“返回”上一时刻的值。最终我们回归到一些起始的“基值”，比如已知的起始高度。下图的Demo中移动滑动条，查看飞机以不同的百分比做出高度改变。
（译者注：滑动时 0.98 系数发生变化，高度—时间曲线发生形状变化）



[1]其中两个例外是 Kalman Filtering for Dummies 和 the Wikipedia page。另外一个非常形象化的教程是 本篇教程 。如果你懂得很多数学概念，请查看这篇教程而不用在我的教程中花费任何时间。本教程受益于 OpenPilot 和 DIY Drones 社区有建设性的评论（Tim Wilkin纠正我我表述中很多不精确的地方）

Part 2 : 考虑噪声#

　　当然，真实世界中的测量，比如高度，是从 GPS 或者气压计等传感器中获得的。这些传感器精度各异[2]^{[2]}[2]。如果传感器具有恒定的偏置的误差，我们可以简单得加上或者减去这个偏置值来获得高度。然而，一般来说，传感器的精度每时每刻都是不可预测的，这使观测到的传感器读数是真实高度叠加上噪声。

observed_altitudecurrent_time=altitudecurrent_time+noisecurrent_timeobserved\_altitude_{current\_time}=altitude_{current\_time}+noise_{current\_time}observed_altitudecurrent_time​=altitudecurrent_time​+noisecurrent_time​

　　移动滑动条查看噪声对观测到的高度的影响，噪声被表示为观测高度范围的百分比。（译者注：拖动滑动条可以看到随着噪声加大，曲线变得越来越不光滑）



[2] 举个例子，Garmin公布其气压高度计得读数精度为 “ 10 英尺（合适校准下）”。所以，如果高度计得读数是 1000 英尺，那么我们真实得高度可能是 990~1010 英尺高度中的任何值。

Part 3 : 将 Part1 和 Part2 合并#

　　现在我们有了两个等式来描述飞机的状态 ：

altitudecurrent_time=0.98∗altitudeprevious_timealtitude_{current\_time}=0.98*altitude_{previous\_time}altitudecurrent_time​=0.98∗altitudeprevious_time​observed_altitudecurrent_time=altitudecurrent_time+noisecurrent_timeobserved\_altitude_{current\_time}=altitude_{current\_time}+noise_{current\_time}observed_altitudecurrent_time​=altitudecurrent_time​+noisecurrent_time​

　　这些公式很容易理解，但是他们并不适合除了我们飞机高度示例之外的系统。为了让这些等式更通用化，工程师采取相似的数学惯例，即使用 xxx 、yyy 、zzz 命名变量，aa a 、bbb 等命名常量，并且用角标 kkk 代表时间[3]^{[3]}[3]。所以，我们的等式变成：

xk=axk−1x_{k}=ax_{k - 1}xk​=axk−1​zk=xk+vkz_{k}=x_{k}+v_{k}zk​=xk​+vk​

　　其中 xkx_kxk​ 是我们系统的当前状态,xk−1x_k-1xk​−1是上一时刻的状态， a 是常量。 zkz_kzk​ 是对系统的当前观测值，并且 vkv_kvk​ 是当前测量噪声。卡尔曼滤波器如此流行的一个原因是它允许我们，根据观测值 zkz_kzk​ 、常量 aaa 、整体测量噪声 vvv 来获得当前状态 xkx_kxk​ 的一个良好估计。更进一步，我们也应当考虑到飞机的实际高度变化可能不是完美光滑的。因为任何做过飞机的人都可以告诉你，飞机在着陆过程中一般会经历一定数量的扰流。这个扰流可以定义为噪声，并且可以作为另外一种噪声源（系统过程噪声，不同于上文中的传感器测量噪声） ：

altitudecurrent_time=0.98∗altitudeprevious_time+turbulencecurrent_timealtitude_{current\_time}=0.98*altitude_{previous\_time}+turbulence_{current\_time}altitudecurrent_time​=0.98∗altitudeprevious_time​+turbulencecurrent_time​

更一般的可以写为 ：

xk=axk−1+wkx_k = a x_{k-1} + w_kxk​=axk−1​+wk​

　　其中 wkw_kwk​ 称为过程噪声，就像扰流，因为它是过程中的固有部分，而不是观测或者测量的一部分。为了将焦点聚集在其它主题，我们将会先忽略过程噪声，但是我们在 Part13 传感器融合[4]^{[4]}[4]的部分将会考虑过程噪声。


[3] 为什么不用 ttt 来表示时间，可能因为在这里时间被认为是一系列离散步长序列，对此，使用脚标索引 kkk 是惯例。
[4]在这里我同时忽略了状态等式中的控制信号，因为对于大部分 Kalman Filter 这有点离题(就是不常用)，并且当需要时可以很容易加上。

Part 4 :状态估计#

　　忽略过程噪声，这里是两个等式，它们描述了我们正在观测的系统的状态 ：

xk=axk−1+wkx_k=ax_{k-1}+w_kxk​=axk−1​+wk​zk=xk+vkz_k=x_k+v_k zk​=xk​+vk​

　　因为我们的目标是从观测z zz 中获得状态 xxx ,我们可以将第二个等式重新写为 ：

xk=zk−vkx_k = z_k - v_kxk​=zk​−vk​

　　当然，问题是我们并不知道当前噪声 vkv_kvk​ ，由其定义我们可以知道其是不可预测的。幸运的是，Kalman 对此有深刻见解，他认为我们可以通过考虑当前观测和上一时刻估计的状态值来估计当前的状态。工程师在变量上画“小帽子”来表示它是估计量 ：所以xk^\hat{x_k}xk​^​是对当前状态的估计。然后，我们能够表达当前的状态的估计为 上一时刻估计值和当前观测值的权衡折衷(译者注：原文单词为tradeoff，即两者综合作用得出的或者理解为动态加权平均)

x^k=x^k−1+gk(zk−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​)

　　其中 gkg_kgk​是表达权衡折衷的增益[5]^{[5]}[5]。我将这个等式高亮为红色，因为我们会直接用它来实现 Kalman 滤波器。
现在，这看起来有点复杂，但是考虑增益项为其极值时。对于gk=0g_k=0gk​=0，我们得到

x^k=x^k−1+0(zk−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​

　　换句话说，当增益为零，观测不起作用，我们直接从上一时刻的状态得到当前状态。（译者注：一般系统状态方程是建模得到的微分方程，卡尔曼第一条公式得到一步预报值，预测的含义便由此而来，这里等号右边第一个xk−1^\hat{x_k-1} xk​−1^​应该指一步预报值，当并没有微分方程时，我们就令一步预报值等于上一时刻的最优估计值xk−1^\hat{x_k-1}xk​−1^​ 。这里用观测修正一步预报值时直接采用了上一时刻最优估计值，原文作者并未区分）
对于 gk=1g_k=1gk​=1， 我们得到

x^k=x^k−1+1(zk−x^k−1)=x^k−1+zk−x^k−1=zk\hat{x}_k = \hat{x}_{k-1} + 1(z_k - \hat{x}_{k-1}) = \hat{x}_{k-1} + z_k - \hat{x}_{k-1} = z_kx^k​=x^k−1​+1(zk​−x^k−1​)=x^k−1​+zk​−x^k−1​=zk​

　　换句话说，当增益是 1 ,过去的状态不再重要，并且我们完全从当前观测中获得当前状态估计。当然，实际增益值将会落在这两个极值之间。尝试移动下方滑动条来查看增益对当前状态估计的效果 ：



[5]变量 kkk 经常被用作代表增益，因为作为 Kalman 增益而广为人知。为了对 Rudolf Kalman (卡尔曼鲁道夫)表示尊重，我发现对于变量和脚标使用相同的字母令人困惑，所以，我选择了另外一个字母 ggg 。

Part 5 : 计算增益#

　　现在我们有了这样一个公式，它基于上一时刻的状态估计 xk−1^\hat{x_k-1}xk​−1^​、当前观测 zkz_kzk​ 和当前增益 gkg_kgk​ 来计算当前状态量的估计值x^k\hat{x}_kx^k​

x^k=x^k−1+gk(zk−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​)

　　译者注：展开这个公式我们可以看到卡尔曼滤波的实质：

x^k=（1−gk）x^k−1+gkzk\hat{x}_k =（1-g_k）\hat{x}_{k-1} +g_kz_kx^k​=（1−gk​）x^k−1​+gk​zk​

即动态加权平均上一时刻的最优估计和当前观测，而这个动态加权是卡尔曼滤波的聪明之处。

　　接下来，我们如何计算增益？答案是：非直接的，从噪声中计算出增益。回想一下，每一次观测都伴随一个特定的噪声值 ：

zk=xk+vkz_k = x_k + v_kzk​=xk​+vk​

　　我们并不知道具体一次观测的噪声，但是我们通常知道平均噪声 ：例如，厂商公布的传感器精度近似告诉我们它输出噪声大小，我把这个值称为 rrr。这里并未使用脚标，因为噪声并不和时间相关，而是传感器本身的属性。然后，我们根据 rrr 能够计算当前增益 gkg_kgk​ :

gk=pk−1/(pk−1+r)g_k = p_{k-1}/(p_{k-1} + r)gk​=pk−1​/(pk−1​+r)

其中pkp_kpk​是由递归计算的预测误差[6]^{[6]}[6] :

pk=(1−gk)pk−1p_k = (1 - g_k)p_{k-1}pk​=(1−gk​)pk−1​

　　至于状态估计等式，让我们想想这两个等式的意义，然后再继续。我们假定在上一次预测中误差 pk−1p_k-1pk​−1是0。然后当前增益 gk=0/（0+r）=0g_k = 0/（0+r）=0gk​=0/（0+r）=0 ，并且下一状态估计将会和当前状态估计一模一样。这是可以理解的，因为如果我们的预测是准确的，我们就不应当修改预测来得到状态估计。在另一个极端情形，假定预测误差是 1 ，然后增益将是 gk=1/(1+r)g_k=1/(1+r)gk​=1/(1+r)。如果 rr r是0，或者我们系统传感器中只有一点点噪声，那么增益将会是 1 ，并且我们新的状态估计 xkx_kxk​ 将受到我们观测 zkz_kzk​ 的强烈影响。 但随着 rrr 变大，增益能变得任意小。换句话说，当系统噪声足够严重,使 gkg_kgk​ 很小，糟糕的预测（这里我觉得应该是修正或者叫做更新，而不是预测）不得不被忽略。噪声严重得使我们不能校正糟糕的预测。（这里原文作者说先不考虑系统过程噪声，应该只有传感器引入的观测噪声或者称为测量噪声）
　　关于第三个等式，计算预测误差 pkp_kpk​ 递归自其上一时刻的值 pk−1p_k-1pk​−1 和当前增益 gkg_kgk​ 。再一次，我们使用极端情形来帮我们思考 ：当 gk=0g_k=0gk​=0 ，我们由 pk=pk−1p_k = p_k-1pk​=pk​−1。所以，就如状态估计一样，一个零增益意味着预测误差没有更新。当 gk=1g_k=1gk​=1，我们有 pkp_kpk​=0 。因此，最大增益对应于零预测误差，此时仅用当前观测来更新当前状态。


[6]专业上讲，rrr实际上是噪声信号的方差，即各个噪声值与其平均值的平均平方距离。如果允许这种噪声的方差随时间变化，卡尔曼滤波器同样有效，但在大多数应用中，它可以被假定为常数。相似的，pkp_kpk​ 专业上讲是第 kkk 步估计过程的协方差。它是我们预测的平均平方误差。事实上，正如 Tim Wilkin 所指出的，状态是随机变量/矢量（随机过程的一个瞬时值），并且它根本上并没有真实值！估计仅仅是描述状态的过程模型的最可能的值而已。

Part 6 : 预测和更新#

　　我们已经准备好了来运行我们的Kalman滤波器并且可以看到一些结果。首先，你可能想知道我们最初等式中的常量aaa发生了什么 ：

xk=axk−1x_k = a x_{k-1}xk​=axk−1​

它似乎从我们状态估计等式中消失了 ：

x^k=x^k−1+gk(zk−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​)

　　答案是，我们需要这两个等式来估计状态。事实上，这两个等式，基于不同种类的信息，代表了对于状态的估计。我们最开始的等式代表了关于状态将会怎样的预测，并且我们的第二个等式代表了上述预测之后的更新(修正)，这基于观测[7]^{[7]}[7]。所以我们重写我们最开始的等式为（加上 “小帽子”）：

x^k=ax^k−1\hat{x}_k = a \hat{x}_{k-1}x^k​=ax^k−1​

最终，我们在误差预测中也使用常量a[8]^{[8]}[8] :

pk=apk−1ap_k = a p_{k-1} apk​=apk−1​a

　　这两个等式一起代表了 Kalman 滤波器的预测阶段。所以我们能够周期得预测/更新，预测/更新……，我们可以想重复多少次就多少次。



[7]专业得讲，第一次估计叫做先验估计（即观测之前），第二次叫做后验估计，并且大多数情况下会引入额外的上标或下标以示区分。因为我尝试着让事情变得简单，并且在你所熟悉的编程语言中编写代码，我避免将概念进一步复杂化
（所以没用更准确的专业术语）


[8]正如 Zichao Zhang（张子超，是谁呀）所指出的，我们乘了两次 aaa ，因为预测误差 pkp_kpk​ 本身就是平方误差。因此，pkp_kpk​ 乘以与状态值相关的系数的平方。对于把误差预测表达为apk−1aap_k-1aapk​−1a而不是a2pk−1a^2p_k-1a2pk​−1将会在Part 12中变得清晰。

Part 7 : 运行滤波器#

所以，现在我们有了运行Kalman 滤波器的所有东西 ：
预测 ：

x^k=ax^k−1\hat{x}_k = a \hat{x}_{k-1}x^k​=ax^k−1​pk=apk−1ap_k = a p_{k-1} apk​=apk−1​a

更新：

gk=pk/(pk+r)g_k = p_k / (p_k + r)gk​=pk​/(pk​+r)x^k←x^k+gk(zk−x^k)\hat{x}_k \leftarrow \hat{x}_k + g_k(z_k - \hat{x}_k)x^k​←x^k​+gk​(zk​−x^k​)pk←(1−gk)pkp_k \leftarrow (1 - g_k)p_kpk​←(1−gk​)pk​

　　应当指出，我在更新的最后两行没有使用标准等号，而是使用赋值（箭头）符号。虽然这样用不符传统，但传递出这样的思想，xk^\hat{x_k}xk​^​和 pkp_kpk​ 的更新是修正它们的当前值（当前值从预测步骤中获得），而不是根据之前的步骤定义它们（正如预测步骤所做的一样[9]^{[9]}[9]。

为了尝试我们的滤波器，我们将需要 ：

1. 观测序列zkz_kzk​
2. 状态估计变量的初始值（基值）x0^\hat{x_0}x0​^​ 。这可以是我们第一次的观测值 z0z_0z0​
3. 预测误差 p0p_0p0​ 的初始值。它不可以是 0 ，否则由于乘法，pkp_kpk​ 将会一直是 0 ，所以我们随意地设置其为 1

　　对于本次实验的观测而言，我们不会尝试观测一个真实的系统（像飞机即将降落），而是设定这样一个情形，状态变量的理想化迭代公式是 xk=0.75xk−1x_k=0.75x_k-1xk​=0.75xk​−1，起点 x0=1000x_0=1000x0​=1000，我们在其上添加位于区间 [-200,+200] 的随机噪声[10]^{[10]}[10] vkv_kvk​来作为观测值。

　　一旦你准备好运行滤波器，点击 Run 按钮来看卡尔曼滤波器是如何产生一个噪声信号（红色）的光滑版本（绿色），而这个光滑版本经常显著地更接近原始干净的信号（蓝色）。你可以尝试不同的 x0x_0x0​ rrr 和 aaa 的值。

　　译者注：卡尔曼滤波器状态方程一般为微分方程，这保证了最优估计（Kalman滤波）的连续性，而离散的观测序列保证了最优估计会收敛。卡尔曼滤波是最优估计的意思是，卡尔曼滤波器的输出是无偏估计，并且其最终估计值与真值的方差最小。无偏估计是指最终估计值的均值等于真值的均值，而方差最小保证了最终估计值离开真值的波动范围最小。两条指标加持，卡尔曼滤波器被称为最优估计。

事实上，如果你查看这个网页的源代码，你会发现预测和更新的 JavaScript 是如此简单：

     // Predict
    xhat = a * xhat;
    p    = a * p * a;
    // Update
    g    = p  / (p  + r);
    xhat = xhat + g * (z - xhat);
    p    = (1 - g) * p;

我感谢Marco Camurri和John Mahoney指出本教程早期版本中等式的不一致性。


[10]Kalman滤波器假定噪声服从高斯分布，而我在这里添加的噪声服从均匀分布，虽然这样做，但是从本教程的层面看不会有太大区别。

Part 8 : 一个更切实际的模型#

　　回想描述我们系统的两条等式 ：

xk=axk−1x_k = a x_{k-1}xk​=axk−1​zk=xk+vkz_k = x_k + v_kzk​=xk​+vk​

　　其中 xkx_kxk​ 是系统的当前状态 , xk−1x_k-1xk​−1 是上一步状态，aaa 是常量， zkz_kzk​ 是我们对系统的当前观测， vkv_kvk​ 是当前观测噪声。虽然这两条等式适用于很多种类的系统，但是并不适用于所有情形。首先，我们没有考虑飞行员在飞机上练习时的时变控制，例如，向前向后移动控制杆。为了考虑上控制项，我们引入另一个带脚标的变量 uku_kuk​ 来代表飞行员发送给飞机的控制信号的值。就如前一时刻的状态 xk−1x_k-1xk​−1 乘以常系数 aaa，这个控制信号也可以乘以一个我们喜欢的常量，称为 bbb 。所以，状态的完整等式变为 ：

xk=axk−1+bukx_k = a x_{k-1}+bu_kxk​=axk−1​+buk​

其中，新的分量用蓝色高亮。
　　通常，除了噪声之外的任何信号都可以乘上常量（线性放缩），所以观测等式 zkz_kzk​ 能被重写为[11]^{[11]}[11]：

zk=cxk+vkz_k =c x_k + v_kzk​=cxk​+vk​

[11]为了与最初的例子保持一致，我选择了使用变量abca b cabc来表示常量，而不是选择更为常规的fbhf b hfbh

Part 9 : 修正估计#

这里再一次列写出对于系统中状态和观测变量的更实际\通用的等式 ：

xk=axk−1+bukx_k = a x_{k-1} + b u_kxk​=axk−1​+buk​zk=cxk+vkz_k = c x_k + v_kzk​=cxk​+vk​

如我们所料，模型中新分量的引入需要预测和更新等式做出相应的修改 ：
预测：

x^k=ax^k−1+buk\hat{x}_k = a\hat{x}_{k-1}+ bu_kx^k​=ax^k−1​+buk​pk=apk−1ap_k = ap_{k-1}a pk​=apk−1​a

更新：

gk=pkc/(cpkc+r)g_k = p_kc/ (cp_kc+r)gk​=pk​c/(cpk​c+r)x^k←x^k+gk(zk−cx^k)\hat{x}_k \leftarrow \hat{x}_k + g_k(z_k - c\hat{x}_k)x^k​←x^k​+gk​(zk​−cx^k​)pk←(1−gkc)pkp_k \leftarrow (1 - g_kc)p_kpk​←(1−gk​c)pk​

　　这里是我们飞机Demo的一个扩展，展示了更长的时长，并增加了代表飞行员稳定地拉回控制杆以使飞机高度上升地控制信号。尝试移动滑动条来调整不同常量地值。就如先前的 Demo 一样，原始信号为蓝色，观测信号为红色，Kalman 滤波后的信号为绿色。

 

Part 10：增加速度项#

　　回想我们关于飞机高度的原始等式 ：

altitudecurrent_time=0.98∗altitudeprevious_timealtitude_{current\_time}=0.98*altitude_{previous\_time}altitudecurrent_time​=0.98∗altitudeprevious_time​

　　写为更通用的形式 ：

xk=axk−1x_k = a x_{k-1}xk​=axk−1​

　　回想高中数学和物理，这种公式似乎有点奇怪。毕竟，高度，是一种距离（海拔高度或地面以上），因为我们学习过这样的公式：

distance=velocity∗timedistance=velocity*timedistance=velocity∗time

　　我们能够将这两种思考距离的方式统一吗？答案是肯定的，但是这需要两步来实现。
　　首先，我们需要在我们高中所学的公式中引入当前时刻和上一时刻的概念，并且思考以离散时间步长考虑距离，而不是整个距离 ：

distancecurrent=distanceprevious+velocityprevious∗(timecurrent−timeprevious)distance_{current}=distance_{previous}+velocity_{previous}*(time_{current}-time_{previous})distancecurrent​=distanceprevious​+velocityprevious​∗(timecurrent​−timeprevious​)

　　换句话说，我们现在在哪，是从上一时刻我们在哪 加上 我们移动的距离获得的。如果我们以固定时间间隔或者时间步长（1s ,100ns，6个月等等）执行这个计算，然后我们就能简化为 ：

distancecurrent=distanceprevious+velocityprevious∗timestepdistance_{current}=distance_{previous}+velocity_{previous}*timestepdistancecurrent​=distanceprevious​+velocityprevious​∗timestep

　　这个等式使我们更加接近我们的一般形式：

xk=axk−1x_k = a x_{k-1}xk​=axk−1​

　　但是我们看起来仍然有两个不同的系统：一个包含一个简单的乘积，而另外一个包含乘积和求和。得到一个两者统一的等式，我们还需要线性代数。

Part 11 : 线性代数#

　　我们有了根据速度和时间来表达距离的等式：

distancecurrent=distanceprevious+velocityprevious∗timestepdistance_{current}=distance_{previous}+velocity_{previous}*timestepdistancecurrent​=distanceprevious​+velocityprevious​∗timestep

我们尝试将其与更一般的方程式统一起来 ：

xk=axk−1x_k = a x_{k-1}xk​=axk−1​

　　我们是很幸运的，数学家很久以前发明了一个 “奇怪的技巧“，即 以同样的方式表示两种方程。技巧就是把这样一种情况（像系统状态）不认为是一个单一的数字，而是数字列表，这称为矢量，它就像Excel表格中的一列一样。矢量的大小（元素个数）相应于我们想要编码的关于状态的数量。对于距离和速度，我们有两项在矢量中：

xk≡[distancekvelocityk]x_k \equiv \begin{bmatrix} distance_k \\ velocity_k \end{bmatrix}xk​≡[distancek​velocityk​​]

　　在这里我使用了三等号来表明这是一个定义：当前状态被定义为矢量，它包含了当前距离和速度。
所以，线性代数如何帮助我们？Well，我们从其中获得的另外一个工具是矩阵。如果将矢量比作电子表格中的列，那么矩阵就像整个电子表格或者值表。当我们将矩阵乘以向量时，我们得到另一个相同大小得向量 ：

[abcd][xy]=[ax+bycx+dy]\begin{bmatrix} a & b \\ c & d \end{bmatrix} \begin{bmatrix} x \\ y \end{bmatrix} = \begin{bmatrix} ax + by \\ cx + dy \end{bmatrix}[ac​bd​][xy​]=[ax+bycx+dy​]

例如：

[8347][25]=[3143]\begin{bmatrix} 8 & 3 \\ 4 & 7 \end{bmatrix} \begin{bmatrix} 2 \\ 5 \end{bmatrix} = \begin{bmatrix} 31 \\ 43 \end{bmatrix}[84​37​][25​]=[3143​]

矢量和矩阵可以是任意大小，只要它们匹配：

[abcdefghi][xyz]=[ax+by+czdx+ey+fzgx+hy+iz]\begin{bmatrix} a & b & c\\ d & e & f\\ g & h & i \end{bmatrix} \begin{bmatrix} x \\ y \\ z \end{bmatrix} = \begin{bmatrix} ax + by + cz\\ dx + ey + fz\\ gx + hy + iz \end{bmatrix}⎣⎡​adg​beh​cfi​⎦⎤​⎣⎡​xyz​⎦⎤​=⎣⎡​ax+by+czdx+ey+fzgx+hy+iz​⎦⎤​

我们也可以将两个矩阵相乘来获得另一个矩阵：

[abcdefghi][rstuvwxyz]=[ar+bu+cxas+bv+cyat+bw+czdr+eu+fxds+ev+fydt+ew+fzgr+hu+ixgs+hv+iygt+hw+iz]\begin{bmatrix} a & b & c\\ d & e & f\\ g & h & i \end{bmatrix} \begin{bmatrix} r & s & t \\ u & v & w \\ x & y & z \end{bmatrix} = \begin{bmatrix} ar+bu+cx & as+bv+cy & at+bw+cz\\ dr+eu+fx & ds+ev+fy & dt+ew+fz\\ gr+hu+ix & gs+hv+iy & gt+hw+iz\\ \end{bmatrix}⎣⎡​adg​beh​cfi​⎦⎤​⎣⎡​rux​svy​twz​⎦⎤​=⎣⎡​ar+bu+cxdr+eu+fxgr+hu+ix​as+bv+cyds+ev+fygs+hv+iy​at+bw+czdt+ew+fzgt+hw+iz​⎦⎤​

矩阵相加很简单，我们仅仅需要将相应位置得元素相加：

[abcdefghi]+[rstuvwxyz]=[a+rb+sc+td+ue+vf+wg+xh+yi+z]\begin{bmatrix} a & b & c\\ d & e & f\\ g & h & i \end{bmatrix} + \begin{bmatrix} r & s & t \\ u & v & w \\ x & y & z \end{bmatrix} = \begin{bmatrix} a+r & b+s & c+t\\ d+u & e+v & f+w\\ g+x & h+y & i+z \end{bmatrix}⎣⎡​adg​beh​cfi​⎦⎤​+⎣⎡​rux​svy​twz​⎦⎤​=⎣⎡​a+rd+ug+x​b+se+vh+y​c+tf+wi+z​⎦⎤​

回到手中得任务，我们定义矩阵：

A=[1timestep01]A = \begin{bmatrix} 1 & timestep\\ 0 & 1 \end{bmatrix}A=[10​timestep1​]

遵从用大写字母表示矩阵的惯例。然后我们的通用等式得到统一：

xk=Axk−1x_k = A x_{k-1}xk​=Axk−1​

就和我们想要得一样，展开来看：

[distancekvelocityk]=[1timestep01][distancek−1velocityk−1]\begin{bmatrix} distance_k\\ velocity_k \end{bmatrix} = \begin{bmatrix} 1 & timestep\\ 0 & 1 \end{bmatrix} \begin{bmatrix} distance_{k-1}\\ velocity_{k-1} \end{bmatrix}[distancek​velocityk​​]=[10​timestep1​][distancek−1​velocityk−1​​]=[1∗distancek−1+timestep∗velocityk−10∗distancek−1+1∗velocityk−1]= \begin{bmatrix} 1 * distance_{k-1} + timestep * velocity_{k-1}\\ 0 * distance_{k-1} + 1 * velocity_{k-1}\\ \end{bmatrix}=[1∗distancek−1​+timestep∗velocityk−1​0∗distancek−1​+1∗velocityk−1​​]=[distancek−1+timestep∗velocityk−1velocityk−1]= \begin{bmatrix} distance_{k-1} + timestep * velocity_{k-1}\\ velocity_{k-1}\\ \end{bmatrix}=[distancek−1​+timestep∗velocityk−1​velocityk−1​​]

　　换句话说，当前距离是前一时刻得距离加上 前一时刻的速度乘以时间步长，而当前速度认为不变。如果我们想要为一个速度时变系统建模，我们很容易得修改我们的矢量和矩阵来包含加速度[12]^{[12]}[12]:

[distancekvelocitykaccelerationk]=[1timestep001timestep001][distancek−1velocityk−1accelerationk−1]\begin{bmatrix} distance_k\\ velocity_k\\ acceleration_k \end{bmatrix} = \begin{bmatrix} 1 & timestep & 0\\ 0 & 1 & timestep \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} distance_{k-1}\\ velocity_{k-1}\\ acceleration_{k-1} \end{bmatrix}⎣⎡​distancek​velocityk​accelerationk​​⎦⎤​=⎣⎡​100​timestep10​0timestep1​⎦⎤​⎣⎡​distancek−1​velocityk−1​accelerationk−1​​⎦⎤​

 

[12]很多人问我，为什么我在矩阵右上角放了一个 0 而不是我们从高中物理中学到得 0.5t20.5t^20.5t2. 这主要是为了简化：通过使距离仅仅取决于上一时刻距离和速度，以及速度取决于上一时刻速度和加速度，你能够建立一个不错得（非混乱）运动模型。为了明白这些，你可以运行这个小 Python 程序，它产生恒定加速度下期望的抛物线距离曲线。同时，对于小的时间步长，平方使右上角的值接近于0。我感谢John Mahoney的这些见解。

Part 12 : 回头再看预测和更新#

这里再一次写出系统状态的修改后的公式 ：

xk=Axk−1x_k = A x_{k-1}xk​=Axk−1​

其中 xxx 是矢量，AAA 是矩阵。正如你回想的那样，这个等式的原始形式是 ：

xk=axk−1+bukx_k = a x_{k-1} + b u_kxk​=axk−1​+buk​

其中 uku_kuk​ 是控制信号，并且 bbb 是 uku_kuk​ 所乘的常系数。我们同时也有等式 ：

zk=cxk+vkz_k = c x_k + v_kzk​=cxk​+vk​

　　其中 zkz_kzk​ 是测量（观测、传感器）信号，vkv_kvk​ 是由于传感器精度受限所增加的噪声。所以，我们如何修改这些原始形式来与矢量/矩阵方法统一？正如你所猜测的，线性代数让这非常简单：我们把系数 bbb 和 ccc 写为大写字母，让他们成为矩阵而不是标量值（单个值）：

xk=Axk−1+Bukx_k = A x_{k-1} + B u_kxk​=Axk−1​+Buk​zk=Cxk+vkz_k = C x_k + v_kzk​=Cxk​+vk​

　　然后，所有的变量（状态、观测、噪声、控制）都被认为是矢量，并且我们有一组矩阵矢量乘法[13]^{[13]}[13]。
所以，我们预测和更新等式会怎样？回想一下：
预测 ：

x^k=ax^k−1+buk\hat{x}_k = a \hat{x}_{k-1} + b u_kx^k​=ax^k−1​+buk​pk=apk−1ap_k = a p_{k-1} apk​=apk−1​a

更新 ：

gk=pkc/(cpkc+r)g_k = p_k c / (c p_k c + r)gk​=pk​c/(cpk​c+r)x^k←x^k+gk(zk−cx^k)\hat{x}_k \leftarrow \hat{x}_k + g_k(z_k - c \hat{x}_k)x^k​←x^k​+gk​(zk​−cx^k​)pk←(1−gkc)pkp_k \leftarrow (1 - g_k c) p_kpk​←(1−gk​c)pk​

　　我们想要将所有常量 abcra b c r abcr 用大写字母代替，以将其变为矩阵 ABCRA B C RABCR 。但是对于线性代数，这有点棘手。你可能记得我承诺解释为什么我没有简化 apk−1aap_k-1aapk​−1a 为a2pk−1a^2p_k-1a2pk​−1（平方没有写成2次方）。答案是线性代数乘法并不像普通乘法一样简单。为了获得正确答案，我们通常不得不以一定的顺序执行乘法，例如，AxBAxBAxB 不一定等于 BxABxABxA。我们可能也需要转置矩阵，以矩阵右侧上标TTT表示。通过把每行变成列和每列变成行，我们完成矩阵的转置，像这样 ：

[abcd]T=[acbd]\begin{bmatrix} a & b\\ c & d \end{bmatrix} ^T= \begin{bmatrix} a & c\\ b & d \end{bmatrix}[ac​bd​]T=[ab​cd​][abcdefghi]T=[adgbehcfi]\begin{bmatrix} a & b & c\\ d & e & f \\ g & h & i \end{bmatrix} ^T = \begin{bmatrix} a & d & g\\ b & e & h \\ c & f & i \end{bmatrix}⎣⎡​adg​beh​cfi​⎦⎤​T=⎣⎡​abc​def​ghi​⎦⎤​

拥有了这些知识，我们能够重写我们的预测等式如下 ：

x^k=Ax^k−1+Buk\hat{x}_k = A \hat{x}_{k-1} + B u_kx^k​=Ax^k−1​+Buk​Pk=APk−1ATP_k = A P_{k-1} A^TPk​=APk−1​AT

　　指出我们将 AAA 和 PkP_kPk​ 用大写字母重写，表明它们是矩阵。我们已经知道，为什么 AAA 是矩阵，我将会在后面解释为什么 PkP_kPk​ 会是矩阵。我们的更新等式会怎么样？对于状态估计 xk^\hat{x_k}xk​^​ 第二个更新等式是简单的（没有除法）：

x^k←x^k+gk(zk−Cx^k)\hat{x}_k \leftarrow \hat{x}_k + g_k(z_k - C \hat{x}_k)x^k​←x^k​+gk​(zk​−Cx^k​)

但是增益更新涉及到了除法 ：

gk=pkc/(cpkc+r)g_k = p_k c / (c p_k c + r)gk​=pk​c/(cpk​c+r)

　　因为乘法和除法是如此紧密相关，我们遇到一个关于矩阵除法的相似问题正如我们在矩阵乘法时遇到的那样。为了明白我们如何处理这些问题，让我们首先重写我们第一个更新等式，并使用熟悉的上标 -1次幂，来将除法表示为逆（a−1=1/aa^{-1} = 1/aa−1=1/a）：

gk=pkc(cpkc+r)−1g_k = p_k c (c p_k c + r)^{-1}gk​=pk​c(cpk​c+r)−1

　　现在，轮到 ccc 、rrr 、 ggg和常量 111 变成矩阵，转置 CCC 如果需要如此 ，再一次推迟解释为什么它们需要是矩阵 ：

Gk=PkCT(CpkCT+R)−1G_k = P_k C^T (C p_k C^T + R)^{-1}Gk​=Pk​CT(Cpk​CT+R)−1Pk←(I−GkC)PkP_k \leftarrow (I - G_k C) P_kPk​←(I−Gk​C)Pk​

　　接下来，我们如何计算(CpkCT+R)−1(C p_k C^T + R)^{-1}(Cpk​CT+R)−1正如普通的求逆，其中 a∗a−1=1a * a^{-1} = 1a∗a−1=1我们需要矩阵求逆产生这样的效果，一个矩阵乘以它自己的逆产生单位矩阵，单位矩阵乘以另外一个矩阵不会改变 ：

AA−1=IA A^{-1} = IAA−1=I

其中单位矩阵（对3×3矩阵）

I=[100010001]I = \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}I=⎣⎡​100​010​001​⎦⎤​

　　计算矩阵的逆并不和求矩阵中每个元素的逆一样简单，这将会在绝大多数情形下是不正确的。虽然矩阵求逆超出了本教程的范围，有绝佳的资源像MathWorld来学习。更好的是，你通常并不需要自己写代码。它内置于编程语言之中，像 Matlab，并且通过在 Python中调用包来使用，其他语言类似。


[13]事实上，我们可以认为这些等式的原始标量形式是线性代数形式的一种特殊情形。

Part 13 : 传感器融合#

现在我们有了线性代数（矢量、矩阵）形式的卡尔曼滤波器的所有公式。
模型 ：

xk=Axk−1+Bukx_k = A x_{k-1} + B u_kxk​=Axk−1​+Buk​zk=Cxk+vkz_k = C x_k + v_kzk​=Cxk​+vk​

预测 ：

x^k=Ax^k−1+Buk\hat{x}_k = A \hat{x}_{k-1} + B u_kx^k​=Ax^k−1​+Buk​Pk=APk−1ATP_k = A P_{k-1} A^TPk​=APk−1​AT

更新 ：

Gk=PkCT(CPkCT+R)−1G_k = P_k C^T (C P_k C^T + R)^{-1}Gk​=Pk​CT(CPk​CT+R)−1x^k←x^k+Gk(zk−Cx^k)\hat{x}_k \leftarrow \hat{x}_k + G_k(z_k - C \hat{x}_k)x^k​←x^k​+Gk​(zk​−Cx^k​)Pk←(I−GkC)PkP_k \leftarrow (I - G_k C) P_kPk​←(I−Gk​C)Pk​

　　似乎为了在状态变量中增加额外的项,我们做了很多的工作。事实上，线性代数的使用支持了kalman滤波器极其有价值的功能，即传感器融合。回到我们的飞机的例子中，我当时指出除了高度，飞行员还需要更多的信息（观测）：飞行员有仪表盘来显示飞机的空速、地速、航向、纬度和经度、外部气温等等。想像一架飞机仅有三个传感器，每一个都相应于（多维矢量）状态的给定部分：气压计测量高度、罗盘测量航向、皮托管测量空速。暂时假设这些传感器完全准确（没有噪声）。然后我们的观测等式 ：

zk=Cxk−1+vkz_k = C x_{k-1} + v_kzk​=Cxk−1​+vk​

展开 ：

[barometerkcompasskpitotk]=[100010001][altitudek−1headingk−1airspeedk−1]\begin{bmatrix} barometer_k\\ compass_k \\ pitot_k \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} altitude_{k-1}\\ heading_{k-1} \\ airspeed_{k-1} \end{bmatrix}⎣⎡​barometerk​compassk​pitotk​​⎦⎤​=⎣⎡​100​010​001​⎦⎤​⎣⎡​altitudek−1​headingk−1​airspeedk−1​​⎦⎤​

现在想象我们有另外一个传感器测量高度，例如 GPS 。气压计和 GPS 都会被高度影响。所以我们的等式变为 ：

[barometerkcompasskpitotkgpsk]=[100010001100][altitudek−1headingk−1airspeedk−1]\begin{bmatrix} barometer_k\\ compass_k \\ pitot_k \\ gps_k \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} altitude_{k-1}\\ heading_{k-1} \\ airspeed_{k-1} \end{bmatrix}⎣⎢⎢⎡​barometerk​compassk​pitotk​gpsk​​⎦⎥⎥⎤​=⎣⎢⎢⎡​1001​0100​0010​⎦⎥⎥⎤​⎣⎡​altitudek−1​headingk−1​airspeedk−1​​⎦⎤​

　　现在我们有了这个系统的第一个例子，这个系统没有简单的，传感器和状态变量之间一一对应关系[14]^{[14]}[14]。任何这样的系统（即多个传感器互补测量同一个量）都为我们提供了传感器融合的机会，也就是，能够结合多个传感器（气压计、GPS）的读数来推断状态的一个分量（这里是高度）。
正如当我们寻求第二位医生对某种疾病的看法时，我们的直觉告诉我们，拥有一个以上重要信息来源会更好。在接下来的部分，我们将会看到kalman滤波器是如何使用传感器融合来获得比只用一个传感器更好的状态估计。


[14]我们的C矩阵让人觉得有点不切实际，因为矩阵中的元素可能不是1。可以这样认为，C矩阵中的非零值对应着相应传感器和状态矢量分量之间的某种关系。

Part 14 : 传感器融合示例#

　　为了感受传感器融合是如何工作的，让我们将系统限制到只有一个状态值[15]^{[15]}[15]。为了进一步简化，我们假定我们不知道状态变换模型（矩阵A），并且我们不得不依赖于传感器的值。也许我们在使用两种不同的温度计来测量外部温度。所以，我们将会设置状态变换矩阵为1 ：

x^k=Ax^k−1=1∗x^k−1=x^k−1\hat{x}_k = A \hat{x}_{k-1} = 1 * \hat{x}_{k-1} = \hat{x}_{k-1}x^k​=Ax^k−1​=1∗x^k−1​=x^k−1​

　　因为缺乏温度计的状态变换模型（没有系统状态微分方程），我们假定当前状态和上一时刻的状态没有变化。
当然，对于传感器融合，在观测矢量 zkz_kzk​ 中我们需要不止一个传感器的值，在本例中我们可以认为是这两个温度计的当前读数。我们假定两个传感器对温度估计做出相等的贡献，所以，C矩阵是这样的 :

zk=Cxk+vk=[11]xk+vkz_k = C x_k + v_k = \begin{bmatrix} 1 \\ 1 \end{bmatrix} x_k + v_kzk​=Cxk​+vk​=[11​]xk​+vk​

　　现在我们有了为了完成预测和更新所需要的三个矩阵（ACRA C RACR）中的两个(ACA CAC)。接下来，我们如何获得RRR？
回想之前我们单个传感器的示例，我们定义 rr r为观测噪声信号vk v_kvk​ 的方差，即和均值之间的平均平方离散程度。对于有多于两个传感器的系统，RRR 是包含每对传感器之间协方差的矩阵。RRR 矩阵对角线上的元素将会是每个传感器的 rrr 值，也就是传感器关于自己的方差。非对角线元素代表了一个传感器的噪声随着其它传感器噪声而变化的程度。对于这个例子和很多现实世界的应用而言，我们假定这些值是 0 (也就是可以简单认为传感器之间是不相关的，互不影响)。我们曾经在温度稳定的气候可控条件下观测我们的温度计，并且观测到他们的值在平均值附近平均上下波动 0.8°，也就是说，温度计读数的标准差是 0.8 ，即方差为 0.8×0.8=0.64 。这给了我们矩阵 RRR ：

R=[0.64000.64]R =\begin{bmatrix} 0.64 & 0\\ 0 & 0.64\end{bmatrix}R=[0.640​00.64​]

　　现在我们应该明白为什么 PkP_kPk​ 和 GkG_kGk​ 必须是矩阵：正如之前脚注所提到的，PkP_kPk​ 是步骤 kkk 中估计过程的协方差。所以，就像传感器协方差矩阵 RRR，PkP_kPk​ 也是一个矩阵。并且，因为 GkG_k Gk​是关联这些矩阵的增益，所以 GkG_kGk​ 也必须是矩阵，并且对于每一个协方差值都有一个增益值。这些矩阵 PkP_k Pk​和 GkG_kGk​ 的维度大小被它们所代表的含义决定。在我们现在的例子中，PkP_kPk​ 的维度大小是 1×1（即单值）,因为它代表了唯一估计值 xk^\hat{x_k} xk​^​关于其自身的协方差。增益 GkG_k Gk​是一个 1×2 的矩阵，因为它将单个状态估计xk^\hat{x_k} xk​^​和两个传感器观测联系起来。
拥有了对于传感器融合的理解，我们先把温度计放到一边，回到飞机的例子上。把之前所有东西放在一起，对于我们的飞机，我们获得如下预测和更新等式(和之前一样，使用协方差噪声值在 0 到 200 英尺之间)：
预测 ：

x^k=Ax^k−1=1∗x^k−1=x^k−1\hat{x}_k = A \hat{x}_{k-1} = 1 * \hat{x}_{k-1} = \hat{x}_{k-1}x^k​=Ax^k−1​=1∗x^k−1​=x^k−1​Pk=APk−1AT=1∗Pk−1∗1=Pk−1P_k = A P_{k-1} A^T = 1 * P_{k-1} * 1 = P_{k-1}Pk​=APk−1​AT=1∗Pk−1​∗1=Pk−1​

更新 ：

Gk=PkCT(CPkCT+R)−1=Pk[1  1]([11]Pk[1  1]+[20000180])−1G_k = P_k C^T (C P_k C^T + R)^{-1} =P_k \begin{bmatrix} 1~~1 \end{bmatrix} (\begin{bmatrix} 1 \\ 1 \end{bmatrix} P_k \begin{bmatrix} 1~~1 \end{bmatrix} + \begin{bmatrix} 200 & 0\\ 0 & 180 \end{bmatrix})^{-1}Gk​=Pk​CT(CPk​CT+R)−1=Pk​[1  1​]([11​]Pk​[1  1​]+[2000​0180​])−1x^k←x^k+Gk(zk−Cx^k)=x^k+Gk(zk−[11]x^k)\hat{x}_k \leftarrow \hat{x}_k + G_k(z_k - C \hat{x}_k) =\hat{x}_k + G_k(z_k - \begin{bmatrix} 1 \\ 1 \end{bmatrix} \hat{x}_k)x^k​←x^k​+Gk​(zk​−Cx^k​)=x^k​+Gk​(zk​−[11​]x^k​)Pk←(I−GkC)Pk=(1−Gk[11])PkP_k \leftarrow (I - G_k C) P_k =(1 - G_k\begin{bmatrix} 1 \\ 1 \end{bmatrix}) P_kPk​←(I−Gk​C)Pk​=(1−Gk​[11​])Pk​

　　事实证明，如果我们不重新引入我们前面提到的东西：过程噪声，那么我们山穷水尽的状态变换模型会让我们陷入困境（有点不懂）。回想之前，对于单个变量系统的状态变换的完整等式是：

xk=axk−1+wkx_k = a x_{k-1} + w_kxk​=axk−1​+wk​

其中 wkw_kwk​ 是给定时间的过程噪声。用线性代数将上式重写为：

xk=Axk−1+wkx_k = A x_{k-1} + w_kxk​=Axk−1​+wk​

　　但是，实际上我们并没有在预测/更新模型中考虑过程噪声。加上过程噪声很容易，正如我们使用 R 来代表测量噪声vkv_kvk​的协方差一样，我们使用 QQQ 来代表过程噪声 wkw_kwk​ 的协方差。然后，我们在预测 PkP_kPk​ 时做出一点修改，即简单得增加这个协方差 ：

Pk=APk−1AT+QP_k = A P_{k-1} A^T + QPk​=APk−1​AT+Q

　　有趣的是，QQQ 矩阵的非零元素即使很小，也非常有助于保持我们的估计状态值正常运行。下面是一个小巧的传感器融合的Demo,允许你改变 QQQ 和 RRR 矩阵中的元素值和改变两个传感器的偏置（恒定的不精确性，或者噪声均值）。正如你所看到的，当传感器从不同方向偏置时，传感器融合能够提供一个比使用单个传感器更接近真值的近似。



[15]我采用得这个材料来自Antonio Moran的excellent的关于卡尔曼滤波器的PPT。Matlab / Octave用户可能想要尝试我放在Github上的版本，这一版本包含了更为通用的Kalman滤波器的实现。

Part 15 : 非线性#

　　现在为止，我们很容易看到线性代数是很棒的，它让我们以一种简洁的的方式表达复杂的算法，像卡尔曼滤波器。然而线性代数并不是万能的。正如其名字所指出的，线性代数受限于表达线性关系，即以直线为特征。为了明白这一点，再一次考虑矩阵乘以矢量的例子 ：

[abcd][xy]=[ax+bycx+dy]\begin{bmatrix} a & b \\ c & d \end{bmatrix} \begin{bmatrix} x \\ y \end{bmatrix} = \begin{bmatrix} ax + by \\ cx + dy \end{bmatrix}[ac​bd​][xy​]=[ax+bycx+dy​]

　　下面的Demo允许你调整常量 abcda b c dabcd 的值来看乘积结果矢量是如何变化的。如你所见，所有的常量调整导致了另外一条直线段（矩阵乘以矢量仍然保持线性）

　　另一方面，出去走走，你将发现自然界中很少是线性的。你所见到的拥有直线的事物—建筑、道路、电线杆等等都是人造的，然而大部分其他事物（树木、鸟类、云）是曲线的或者分形的。由于传感器和电机以及其他工件都是用物理材料制成的，因此它们的行为同样倾向于非线性，如果超出一些限制的范围。
在高中数学中你所学到的很多函数，像f(x)=ax2+bx+cf(x) = ax^2 + bx+cf(x)=ax2+bx+c, f(x)=sin(x)f(x) = sin(x)f(x)=sin(x), log(x)log(x)log(x)等等，大部分是非线性的，所以，我们很容易选择一个。下面的Demo，我根据有趣的曲线形状挑选了几个不同的非线性函数，将上一Demo中的蓝色直线段映射为曲线。

Part 16 : 处理非线性#

　　尽管非线性可能为任何系统引入全新世界，但我们仍然抱有希望。正如你在之前Demo中所注意的，前三个非线性函数都具有一个有用的属性：它们是光滑曲线，这与最后一个尖锐的转弯相反。数学家把这些光滑曲线函数称为可微。正如任何学过微积分的人可以证明，可微函数（光滑曲线）能够由一系列连续的小直线段建模到任意精度。即使没有微积分，你能看一下下面的Demo，它允许你通过调整直线段△x的大小近似函数f（x）=x2f（x）=x^2f（x）=x2

　　然而，这些直线段是如何帮助处理 Kalman 滤波器中的非线性关系？考虑一个很简单的滤波器，其传感器符合下列线性等式 ：

zk=3xk+2z_k = 3 x_k +2zk​=3xk​+2

　　这个等式告诉我们传感器读数一直是相应状态值的三倍加上 2 ：不管状态值是什么，传感器读数一直是三倍状态值加上 2 .现在来考虑非线性传感器等式 ：

zk=log2(xk)z_k = log_2(x_k)zk​=log2​(xk​)

　　这个等式表明传感器读数是状态值以 2 为底的对数值：这是很典型的关系，例如，我们对音高的感觉是频率的函数。即使你之前没有听说过对数，可以看一下下列近似值表格，这个表格展示了状态 xkx_kxk​ 和传感器读数 zkz_kzk​ 之间的关系，这并不如之前的 Demo 直观 ：

　　在这里你可以看到，并没有常量 aaa 和 zk=axk+bz_k=ax_k+bzk​=axk​+b 。然而，我们能使用上面的小线段来推导不同的 aka_kak​ 和 bkb_kbk​ 集合，每个时间步长对应一个 kkk ，这样做就用很多直线段近似了非线性对数关系。如果你研究过微积分，你可能记得我们能够直接计算很多非线性函数（log sin cos 等等）的一阶导数。如果你没有学过微积分，不要感到忧桑，一个函数的一阶导数实际上是在给定点上的最佳线性近似（高数中线性主部）。稍微 Google 一下，你可以明白 log2(x)log_2(x)log2​(x) 的一阶导数近似是 1/0.693x1/{0.693x}1/0.693x ，这是有意义的，随着 xxx的增加，log2(x)log_2(x)log2​(x) 的值越来越大。

Part 17 : 一个非线性卡尔曼滤波器#

接下来，一阶导数对我们有什么用？Well,这里是我们的线性 Kalman 滤波器的等式集合，使用了一个没有状态变换和控制信号的模型，但拥有过程噪声，单个传感器，和单个状态值 ：
模型 ：

xk=xk−1+wkx_k = x_{k-1} + w_kxk​=xk−1​+wk​zk=cxk+vkz_k = c x_k + v_kzk​=cxk​+vk​

预测 ：

x^k=x^k−1\hat{x}_k = \hat{x}_{k-1}x^k​=x^k−1​pk=pk−1+qp_k = p_{k-1} + qpk​=pk−1​+q

更新 ：

gk=pkc(cpkc+r)−1g_k = p_k c (c p_k c + r)^{-1}gk​=pk​c(cpk​c+r)−1x^k←x^k+gk(zk−cx^k)\hat{x}_k \leftarrow \hat{x}_k + g_k(z_k - c \hat{x}_k)x^k​←x^k​+gk​(zk​−cx^k​)pk←(1−gkc)pkp_k \leftarrow (1 - g_k c) p_kpk​←(1−gk​c)pk​

　　现在我们将会修改这些等式来反映传感器的非线性。使用符号 hhh 来代表任何非线性函数（就像我们例子中的 log2log_2log2​）[16]^{[16]}[16]，并且 ckc_kck​ 代表它在时间步长 kkk 的一阶导数，我们获得 ：
模型 ：

xk=xk−1+wkx_k = x_{k-1} + w_kxk​=xk−1​+wk​zk=h(xk)+vkz_k =h(x_k)+ v_kzk​=h(xk​)+vk​

预测 ：

x^k=x^k−1\hat{x}_k = \hat{x}_{k-1}x^k​=x^k−1​pk=pk−1+qp_k = p_{k-1} + qpk​=pk−1​+q

更新 ：

gk=pkck(ckpkck+r)−1g_k = p_kc_k(c_kp_kc_k+r)^{-1}gk​=pk​ck​(ck​pk​ck​+r)−1x^k←x^k+gk(zk−h(x^k))\hat{x}_k \leftarrow \hat{x}_k + g_k(z_k-h(\hat{x}_k))x^k​←x^k​+gk​(zk​−h(x^k​))pk←(1−gkck)pkp_k \leftarrow (1 - g_kc_k)p_kpk​←(1−gk​ck​)pk​

　　正如我们 Part14 中的传感融合 Demo ,下面的 Demo 展示了一个时变的温度波动，但是仅使用非线性响应并没有恒定偏置的单一传感器。你能够从三个非线性传感器函数中选择来比较我们的非线性 Kalman 滤波器和线性 Kalman 滤波器。正如你所见，无论如何调整线性版本中 ccc 参数，都不足以获得与非线性版本相同好的拟合原始信号（真值）结果。右边的图显示了非线性函数自身的形状 ：



[16]为什么不把这个函数写作f？我们将会在很多页面中看到为什么

Part 18 : 计算导数#

　　如果你已经到这里了，你已经占据地利了，还差一点点天时就可以很好的理解扩展卡尔曼滤波器了。还有两件事情需要考虑：
　　1. 在并不知道显示函数时，如何从实际信号中计算一阶导数
　　2. 如何推广我们的单值非线性状态/观测模型到多值系统

　　为了回答第一个问题，我们指出一个函数的一阶导数被定义当时间步长接近零时，函数的连续值之间的差值除以时间步长的极限：

f′(x)=lim⁡Δx→0f(x+Δx)−f(x)Δxf'(x) = \lim_{\Delta x \to 0} \frac{f(x+\Delta x) - f(x)}{\Delta x}f′(x)=Δx→0lim​Δxf(x+Δx)−f(x)​

如果你不理解这个等式，不要忧桑：可以这样近似一阶导数

(yk+1−yk)timestep\frac{(y_{k+1} - y_k)} {timestep}timestep(yk+1​−yk​)​

　　实际上，如下面 Demo 展示的，这个有限差分公式通常是对一阶导数的很好的近似。Demo 允许你选择同上的三个函数。但是这次你能够选择是导数公式还是有限差分 ：

　　如果一个信号（像传感器值 zkz_kzk​ ）是另一个信号(状态 xkx_kxk​ )的函数，我们能够用第一个信号的连续差值除以第二个信号的连续差值来表示不是关于时间的有限差分，而是分子关于分母中信号的有限差分 ：

zk+1−zkxk+1−xk\frac{z_{k+1} - z_k}{x_{k+1}-x_k}xk+1​−xk​zk+1​−zk​​

Part 19 : 雅可比矩阵#

　　现在来回答我们的第二个问题，如何推广单值非线性状态/观测模型到多值系统，这时你应当回忆在我们线性模型中传感器分量的等式 ：

zk=Cxkz_k = C x_kzk​=Cxk​

对一个拥有两个状态值和三个传感器的系统，我们能够重写上式为 ：

zk=[c11c12c21c22c31c32][xk1xk2]=[c11xk1+c12xk2c21xk1+c22xk2c31xk1+c32xk2]z_k = \begin{bmatrix} c_{11} & c_{12} \\ c_{21} & c_{22} \\ c_{31} & c_{32} \\ \end{bmatrix} \begin{bmatrix} x_{k1} \\ x_{k2} \end{bmatrix} = \begin{bmatrix} c_{11}x_{k1} + c_{12} x_{k2}\\ c_{21}x_{k1} + c_{22} x_{k2}\\ c_{31}x_{k1} + c_{32} x_{k2} \end{bmatrix}zk​=⎣⎡​c11​c21​c31​​c12​c22​c32​​⎦⎤​[xk1​xk2​​]=⎣⎡​c11​xk1​+c12​xk2​c21​xk1​+c22​xk2​c31​xk1​+c32​xk2​​⎦⎤​

　　虽然你之前可能没有见过类似的概念，但这非常简单：CCC矩阵元素的双数字索引不止表明每个元素的行和列的位置，更重要的是，元素索引（位置）所表达的关系。例如，c12c12c12是这样一个系数，它将第一个传感器的当前值 zk1z_k1zk​1 与当前状态的第二个分量xk2x_k2xk​2 关联起来。
　　对于一个非线性模型，同样存在这样一个矩阵，其行数等于传感器的数量，列数等于状态变量的个数（请看上面C矩阵）。然而，这个矩阵将会包含传感器值对于相应状态值的一阶导数（线性近似）的当前值。数学家把它叫做偏导数，这些偏导数组成的矩阵称为雅可比矩阵。计算雅可比矩阵超出了现有本教程的范畴[17]^{[17]}[17]，但是这个基于Matlab的EKF教程和这个基于Matlab的实现EKF_GPS_Example涉及了很少的代码，以供学习。

　　如果这些概念令你困惑，那么考虑这样一项调查，要求一群人以1到5对几种产品进行评级。每种产品的得分将是该产品所有人评分的平均值。要了解一个人是如何影响某个产品的评分，我们会查看该人对该产品的评分。每个这样的人/产品评分都像偏导数，而这样的人/产品评级表就像雅可比矩阵。用人来代替传感器，用最终得分代替状态，然后你就扩展卡尔曼滤波器的传感器模型。
　　上面我们已经将模型中的观测公式推广到非线性，这里，剩下的只有将状态变换公式推广到非线性。换句话说，线性模型中的

xk=Axk−1+wkx_k = A x_{k-1} + w_kxk​=Axk−1​+wk​

变为

xk=f(xk−1)+wkx_k = f(x_{k-1}) + w_kxk​=f(xk−1​)+wk​

其中AAA被状态变换函数 fff 的雅可比矩阵代替。实际上，惯例是使用 FkF_kFk​ 代表雅可比矩阵（因为相应于 fff 并且随时间变化），并且使用 HkH_kHk​ 代表传感器函数 hhh 的雅可比矩阵。将控制信号 uku_kuk​ 合并到状态变换模型中，我们得到扩展卡尔曼滤波器的完整公式 ：
模型 ：

xk=f(xk−1,uk)+wkx_k = f(x_{k-1}, u_k) + w_kxk​=f(xk−1​,uk​)+wk​zk=h(xk)+vkz_k = h(x_{k}) + v_kzk​=h(xk​)+vk​

预测 ：

x^k=f(x^k−1,uk)\hat{x}_k = f(\hat{x}_{k-1}, u_k)x^k​=f(x^k−1​,uk​)Pk=Fk−1Pk−1Fk−1T+Qk−1P_k = F_{k-1} P_{k-1} F^T_{k-1} + Q_{k-1}Pk​=Fk−1​Pk−1​Fk−1T​+Qk−1​

更新 ：

Gk=PkHkT(HkPkHkT+R)−1G_k = P_k H_k^T (H_k P_k H_k^T + R)^{-1}Gk​=Pk​HkT​(Hk​Pk​HkT​+R)−1x^k←x^k+Gk(zk−h(x^k))\hat{x}_k \leftarrow \hat{x}_{k} + G_k(z_k - h(\hat{x}_{k}))x^k​←x^k​+Gk​(zk​−h(x^k​))Pk←(I−GkHk)PkP_k \leftarrow (I - G_k H_k) P_kPk​←(I−Gk​Hk​)Pk​

[17]在我所见的大多数EKF示例中，状态变换函数简单得就是恒等函数f(x)=xf(x)=xf(x)=x。所以雅可比矩阵就是Part12中描述得单位矩阵。对于观测函数h h h和它的雅可比矩阵也是同样。

Part 20 : TinyEKF#

　　如果你已经读到了这里，你已经准备好开始做真正EKF的实验了。TinyEKF 是我用 C/C++ 写的，主要在流行的飞控 Pixhawk、Multiwii32、OpenPilot 使用的微控制器如 Ardunio、Teensy、STM32 上运行的 EKF 实现。查看了一些飞控里面的 EKF 代码，我发现其很难与本教程所表达的理解相关联（飞控中的EKF经过大量实测修改，不单单是懂了这几条理论公式就能懂）所以我决定写一个简单的 EKF 实现，并在常用的微控制器上运行，占用 “tiny” 数量的内存，然而仍然足够灵活来应用于不同的工程项目中。我还编写了一个 Python 实现，因此你可以在实际的微控制器上运行之前对 EKF 进行原型设计。
　　TinyEKF 仅仅要求你写一个简单的模型函数，添加状态变换函数f(x) f(x)f(x) 的值和它的雅可比矩阵 F（x）F（x）F（x）,传感器函数 h(x)h(x)h(x) 和它的雅可比矩阵 H（x）H（x）H（x）。然后将观测矢量 zz z传递给迭代公式，预测和更新将会自动处理。点击绿色的TinyEKF超链接到原文作者的Github。
我提供了三个示例：

1. 基于Yo Chong的Matlab implementation，我写了一个纯C代码的例子，它使用存储在文件中的GPS 卫星数据。
2. Ardunio上运行的C++实现的传感器融合例子
3. Python写的鼠标追踪示例（需要OpenCV