扩展卡尔曼滤波新手教程（三）----中文版

扩展卡尔曼滤波新手教程（三）

说明：本文内容翻译自外文网站The Extended Kalman Filter: An Interactive Tutorial for Non-Experts，仅供学习和参考。
本文是扩展卡尔曼滤波新手教程系列的第三篇，上一篇博客请看：扩展卡尔曼滤波新手教程（二）。

Part11: 线性代数

前面我们得到了根据速度和时间描述距离的方程：
$distance_{current}=distance_{previous}+velocity_{previous}*timestep$

但我们想将该方程归纳为更通用的形式:
$x_k=ax_{k-1}$

幸运的是，很久以前，数学家们就发明了一种“神奇的技巧”，从而实现用同样的形式来表示这两种方程。这种技巧就是不要把一种情况（比如系统状态）看作一个单一的数字，而是看作一种称为向量（vector）的数字列，它就像Excel电子表格中的一列。向量的大小（元素的数量）对应于我们想要对状态编码的量的个数。例如对于距离和速度，我们的向量中有两个元素（item）：
$x_k\equiv \begin{bmatrix} {distance_{k}}\\ {velocity_{k}}\\ \end{bmatrix}$

这里我使用了恒等于号"$\equiv$"来表示这是一个定义：当前状态被定义为一个包含当前距离和当前速度的向量。
那么这能对我们有什么帮助？嗯。。。我们还需要线性代数中的另一个工具----矩阵。如果一个向量就像电子表格中的一列，那么一个矩阵就像是整个电子表格。当我们用一个向量和矩阵相乘时，我们会得到同样大小的另一个向量：
$\begin{bmatrix} {a}&b\\ c&d\\ \end{bmatrix} \begin{bmatrix} {x}\\ {y}\\ \end{bmatrix}=\\ \begin{bmatrix} {ax+by}\\ {cx+dy}\\ \end{bmatrix}$

例如：
$\begin{bmatrix} {8}&8\\ 4&7\\ \end{bmatrix} \begin{bmatrix} {2}\\ {5}\\ \end{bmatrix}=\\ \begin{bmatrix} {31}\\ {43}\\ \end{bmatrix}$

向量和矩阵可以是任意维度的，只要它们的维度相匹配（即矩阵的列数和向量的长度相等）：
$\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}$

我们也可以将两个矩阵相乘得到另一个矩阵：
$\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+bs+cw}&{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}$

两个矩阵相加则更为简单:
$\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}$

现在回到我们刚才的任务，我们定义一个矩阵，并且遵循使用大写字母表示矩阵的惯例：
$A= \begin{bmatrix} 1&timestep\\ 0&1\\ \end{bmatrix}$

这样我们就能得到具有通用形式的方程：
$x_k=Ax_{k-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}\\= \begin{bmatrix} {distance_{k-1}+timestep*velocity_{k-1}}\\ {velocity_{k-1}}\\ \end{bmatrix}$

也就是说，当前距离等于之前的距离加上之前的速度乘以时间步长，当前速度等于之前的速度。如果我们想对一个速度随时间变化的系统建模，我们可以很容易地修改我们的向量和矩阵来包含加速度：
$\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}$

注：有人给我发邮件问我为什么把0放在这个矩阵的右上角，而不是我们高中物理课上学到的$0.5t^2$。这主要涉及到一个简化问题：你可以尝试构建一个离散的（non-chaotic）运动模型，让距离只依赖于前一时刻的距离和速度，而速度只依赖于前一时刻的速度和加速度。(为了方便，可以直接运行这个Python小程序，它能在恒定加速度下生成预期的抛物线距离曲线。)此外，对于一个比较小的时间步长，平方运算会使右上角的值接近于零。故为了简化操作，我们将其置为0。

Part12: 回看预测和更新

下面是我们修改后的系统状态方程：
$x_k=Ax_{k-1}$

这里$x$是一个向量，而A是一个矩阵。之前我们得到的状态方程的形式是：
$x_k=ax_{k-1}+bu_k$

这里的$u_k$是一个控制信号，而$b$是一个缩放系数。我们之前也得到了：
$z_k=cx_{k}+v_k$
这里的$z_k$是测量（观测，或传感器）信号，$v_k$是来自传感器测量精度的噪声信号。那么我们该如何修改这些原始方程以使它们匹配现在的“向量/矩阵”形式呢？线性代数让这一切都变得简单：我们将$b$和$c$都改写成大写字母，使它们成为矩阵，而不是单一的值：
$x_k=Ax_{k-1}+Bu_k \\ z_k=Cx_{k}+v_k$

然后，所有的变量（系统状态、观测值、噪声信号、控制信号）都变成了向量的形式，然后我们就得到了一系列的“矩阵*向量”的乘法。事实上，我们可以将最初得到的方程看作是线性代数形式方程的一种特殊情况，相当于在每个矩阵中只有一个位置具有非零值。
那我们之前推导得出的预测和更新方程呢?让我们来回顾一下它们：
预测：
$\hat{x_k}=a\hat{x_{k-1}}+bu_k \\ {p_k}=a{p_{k-1}} a$
更新：
${g_k}=p_kc/(c{p_{k}}c+r) \\ \hat{x_k}\leftarrow\hat{x_{k}}+ g_k(z_k-c\hat{x_k}) \\ {p_k}\leftarrow(1-g_kc){p_k}$

我们可能也想把所有的常量$a,b,c,r$全部变成大写字母$A,B,C,R$，然后就搞定了。但是对于线性代数来说，不能直接这样做。你可能会记得我之前说过要解释为什么我没有将$ap_{k-1}a$简化成$a^2p_{k-1}$。答案是线性代数乘法不像普通的乘法那样直接。要得到正确的答案，我们必须以确定的顺序执行乘法；例如，$AxB$不一定和$BxA$相等。我们也可能会需要对矩阵进行转置，该操作用矩阵右上角的符号$T$表示。对矩阵进行转置的过程实际上是将矩阵的行和列元素进行互换，下面是一些示例：
$\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}$

有了这些理论基础，我们可以重写预测方程：
$\hat{x_k}=A\hat{x_{k-1}}+Bu_k \\ {P_k}=A{P_{k-1}} A^T$

注意这里我们对$A$和$P_k$都写成了大写字母，以表示它们都是矩阵形式。我们已经知道为什么$A$是矩阵；至于为什么$P_k$也是矩阵，我们暂且放一放，后面会讨论这个问题。

那我们的更新方程呢?第二个更新公式,直接变成:
$\hat{x_k}\leftarrow\hat{x_{k}}+ g_k(z_k-C\hat{x_k})$

但是卡尔曼增益的更新还包含了除法:
${g_k}=p_kc/(c{p_{k}}c+r)$

由于乘法和除法具有很大的相关性,因此我们用乘法运算来替代矩阵除法. 我们首先利用熟悉的上标$^{-1}$来替换除法运算($a^{-1}=1/a$), 然后重写更新方程:
${g_k}=p_kc(c{p_{k}}c+r)^{-1}\\$

现在, 我们将$c,r,g$以及常数$1$转换为矩阵, 而且还需要用到转置的$C$($C^T$)----对于它们为什么需要时矩阵, 我们再一次先暂且放一放. 转换之后, 其他两个更新方程变成了:
${G_k}=P_kC^T/(C{P_{k}}C^T+R)^{-1}\\ P_k\leftarrow(I-G_kC)P_k$

那么我们应该如何计算$(C{P_{k}}C^T+R)^{-1}$,也就是矩阵$(C{P_{k}}C^T+R)$的逆矩阵呢?就如同普通的倒数运算, $a*a^{-1}=1$, 一个矩阵乘以它的逆矩阵之后能够得到一个单位矩阵. 而单位矩阵和任何其他矩阵之后, 得到的结果始终和原矩阵相同:
$AA^{-1}=I$
例如对于$3\times3$矩阵,
$I= \begin{bmatrix} 1&0&0\\ 0&1&0\\ 0&0&1 \end{bmatrix}$

计算矩阵的逆并不像对矩阵的每个元素求逆那么简单，这在大多数情况下都会得到错误的答案。矩阵求逆的细节超出了本教程的范围，但是有一些优秀的资源，比如MathWorld，可以帮助您学习它。更好的是，通常你不需要自己编写代码, 它内置在Matlab等语言中，可以也可以在Python和其他流行语言中通过一些功能包获取。

Part13: 传感器融合简介

现在我们已经以线性代数的形式完成了我们卡尔曼滤波器的一系列方程:
模型：
$\hat{x_k}=A\hat{x_{k-1}}+Bu_k \\ {z_k}=C{x_{k}}+v_k$
预测：
$\hat{x_k}=A\hat{x_{k-1}}+Bu_k \\ {P_k}=A{P_{k-1}} A^T$
更新：
${G_k}=P_kC^T(C{P_{k}}C^T+R)^{-1} \\ \hat{x_k}\leftarrow\hat{x_{k}}+G_k(z_k-C\hat{x_k}) \\ {P_k}\leftarrow(I-G_kC){P_k}$

这看上去似乎只是能够给我们的状态变量添加一些额外的元素! 而事实上，借助线性代数可以使卡尔曼滤波器拥有一个非常有价值的能力，我们称传感器融合.
再回到我们之前飞机的例子, 我们注意到飞行员能够得到的信息(观测值)远不止海拔高度: 他们还拥有显示飞机空速(airspeed)、地速(groundspeed)、航向(heading)、经纬度、外部温度等的仪表。想象一下，一架飞机只有三个传感器，每个传感器都对应于状态的一个已知部分: 一个显示海拔高度的气压计(barometer)、一个显示航向的罗盘(compass)和一个显示空速的流速计( Pitot tube)。暂时假设这些传感器是完全精确的(无噪声信号), 这样观测方程:
$z_k=Cx_{k-1}+v_k$
就变成了:
$\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}$
这里是第一个系统示例，它不再只是传感器和状态值之间的简单一一对应关系。不过此处我们的$C$矩阵有些不切实际，因为值可能不是1。不过该示例的重点是矩阵中的非零值对应于传感器和系统状态向量的一个元素之间的关系。任何这样的系统都能为我们提供传感器融合的机会; 也就是说，能够将多个传感器(气压计、GPS)的观测数据结合起来，从而推断出系统一部分系统状态(如海拔高度等)。

就像我们常常会寻求第二位医生对某一病症的意见一样，直觉告诉我们，对于一些重要的事情，最好有不止一个信息来源。在下一节中，我们将看到卡尔曼滤波器是如何利用多传感器融合来实现比使用单一传感器更好的系统状态估计的。

Part14: 传感器融合示例

未完待续…