24. 卡尔曼滤波器详解——从零开始(4) Kalman Filter from Zero

隔了3个月，有空回来更新这一个还没完成的系列。原本计划去年就要写完的，结果年末事情有些多，忙着忙着把这个事情搁下了。当然我还是心里一直挂着这个事情的。

基于之前3篇文章的基础知识，我们在这篇文章中讲讲基础的Kalman filter的原理和推导。如果还不熟悉信号和概率基础或者已经忘记的朋友可以回看前面三篇。（发现第二篇点赞的人挺少的，我默认很多人都跳过了第二篇。如果你第二篇的内容都有了一定了解，那非常不错。）最后一节：正交投影法，来源于Kalman最开始的推导方法，不过采用了不一样的notation。他的文章写的确实是通俗易懂，建议自己读读吧。

下面是上一篇的传送门。我就不多说废话了，直接开始。

三脚猫Frank：23. 卡尔曼滤波器详解——从零开始(3) Kalman Filter from Zero

本篇目录：

1. Kalman filter 概述 Overview

2. 动力学模型的状态空间方程 Dynamic model - State-space representation

3. 量测方程、递推滤波器 Measurement model, Recursive filter

4. 离散Kalman filter推导 Discrete Kalman filter

5. 离散Kalman filter方程总结 Summary of updating equations

6. Kalman filter的特点与应用注意事项 Features and Notes

7. Kalman原文解析 - 正交投影法推导 Kalman filter from orthogonal principle

8. 总结与评论

1. Kalman filter 概述 Overview

我们在上一篇文章中介绍了Wiener filter——这是一种基于minimum mean square error标准的最优滤波器。它的本质就是一个线性最小均方差估计器（LMMSE estimator）。不过在形成估计问题时，我们仅仅考虑了量测方程，并没有关心信号本身的变化规律。另一方面，我们那时还要求被估计信号是一个WSS(Wide-sense stationary, 宽平稳) 信号，即其期望是常值，自相关函数仅与一个时间常数有关（忘记了的话，请参考[4]）。我们希望filter能够同样适用于非平稳信号。

考虑到以上两点，并且将原来最优滤波器的批处理算法改成递推(recursive)算法，Kalman filter就这样诞生了。我们注意到Kalman filter还是一种线性滤波器，采用的还是LMMSE原则，但有几个特点：

采用递推或者叫迭代算法——利用上一时刻的估计值和新的观察值进行估计，不需要保存所有历史的测量值，便于在计算机上执行，也适合多维信号的处理。
考虑了信号的动力学方程——考虑将信号的动力学模型用状态空间方程来表达，加上量测方程，可以利用状态变化规律来提高估计精度。
适用于非平稳信号—— 噪声一般建模都是平稳白噪声，动力学信息已知。在动力学方程已知的情况下，状态信号的统计学信息是可以实时确定的。因为无论是平稳还是非平稳信号都可以被处理。

就目前来说，使用Kalman filter有几个假设要满足：

系统的动力学模型和量测模型都必须为线性。
动力学模型是比较准确的，符合实际系统的行为。
动力学模型中的噪音和观测噪音都是零均值且方差已知的白噪声。

针对三个假设，我们要线性化系统、尽可能保证建模的准确和对实际应用中的有色噪声进行白化。

2. 动力学模型的状态空间方程 Dynamic model - State-space representation

我们考虑一个general的离散线性随机系统，其状态方程是:

$x_{k}=\mathbf{\Phi}_{k-1}x_{k-1}+\mathbf{B}_{k-1}u_{k-1}+\mathbf{\Gamma}_{k-1}w_{k-1}\tag{1}$

其中状态变量 $x$ 是一个 $n$ 维向量（简单起见不加粗了），下标代表了 $k$ 时刻； $u_k$ 是 $k$ 时刻的 $p$ 维控制向量； $w_{k}$ 是状态噪声，认为是零期望白噪声； $\mathbf{B}_k,\mathbf{\Gamma}_{k}$ 分别为 $k$ 时刻的系数或者传递矩阵；噪音的互协方差矩阵为：

$E[(\mathbf{\Gamma}_{k}w_{k})(\mathbf{\Gamma}_{l}w_{l})^T]\\=\mathbf{\Gamma}_{k}E(w_{k}w^T_{l})\mathbf{\Gamma}_{l}^T\\ = \begin{cases} \mathbf{\Gamma}_kQ_k\mathbf{\Gamma}_k^T, & k=l\\ 0, & k\neq{l} \end{cases} \tag{2}$

其中 $Q_k$ 是 $k$ 时刻时噪音的自协方差矩阵（后文简称为协方差），非负定。(2)表明了不同时刻的噪音是互不相关的（白噪音的性质）。

由于Kalman filter用到了状态方程进行预测估计，每次产生新的估计会收受到之前估计的影响，因此需要了解估计误差是如何随着状态方程中状态的演变进行传播的，这种现象称之为误差传播（Propagation of the error）。下面推导误差的均值（mean)与协方差(covariance)的传播。

Remark: 让我们回顾一下为什么考察误差这两个指标？因为估计误差在估计理论中一个随机变量，而它们是随机变量的一阶矩和二阶矩的代表，是表征随机变量的重要统计量。

让我们考虑方程(1)， $k-1$ 时刻的状态估计为 $\hat{x}_{k-1}$ 。由于白噪音是各个时间相互独立，无法预测的，我们根据仅根据状态方程的确定部分来更新下一个时刻 $k$ 的状态估计 $\hat{x}_{k}$ ，即：

$\hat{x}_{k}=\mathbf{\Phi}_{k-1}\hat{x}_{k-1}+\mathbf{B}_{k-1}u_{k-1}\tag{3}$

Remark: 如果你细心阅读Kalman filter的各种教材，可能会发现大部分书都没有考虑输入 $u$ 。这是因为系数矩阵和控制律是假设已知的，实际上总可以减去 $\mathbf{B}_{k-1}u_{k-1}$ 这一部分信号，来将分析过程进行简化。我们这里就把 $u$ 带上，其实差别不大。这就好比我们在考虑系统是否满足observability的时候，输入带来的影响如果是已知的，就可以被去除。感兴趣的可以回顾本专栏之前关于observability的文章。

那么 $k$ 时刻的状态估计误差定义为：

$\tilde{x}_k=\hat{x}_k-x_k=\mathbf{\Phi}_{k-1}\tilde{x}_{k-1}-\mathbf{\Gamma}_{k-1}w_{k-1}\tag{4}$

我们考虑状态估计误差的期望或者均值：

$\begin{align} E[\tilde{x}_k]=&E[\hat{x}_k-x_k]\\=&\mathbf{\Phi}_{k-1}E[\tilde{x}_{k-1}]-\mathbf{\Gamma}_{k-1}E[w_{k-1}] \\=&\mathbf{\Phi}_{k-1}E[\tilde{x}_{k-1}]\tag{5} \end{align}$

如果 $E[\tilde{x}_{k-1}]$ 为0，则 $E[\tilde{x}_k]$ 为0。这意味着状态估计的无偏性是可以随着状态的转移而传递的，状态的演变不会影响无偏性。其中 $E[w_{k-1}]$ 为0是因为白噪声的均值或者期望为0。

继续考虑状态估计误差的 $k$ 时刻的协方差：

$P_k=E[\tilde{x}_k\tilde{x}_k^T]\tag{6}$

$P_k=E[\tilde{x}_k\tilde{x}_k^T]=E[(\mathbf{\Phi}_{k-1}\tilde{x}_{k-1}-\mathbf{\Gamma}_{k-1}w_{k-1})(\mathbf{\Phi}_{k-1}\tilde{x}_{k-1}-\mathbf{\Gamma}_{k-1}w_{k-1})^T]\\ =\mathbf{\Phi}_{k-1}E[\tilde{x}_{k-1}\tilde{x}_{k-1}^T]\mathbf{\Phi}^T_{k-1}+\mathbf{\Gamma}_{k-1}E[w_{k-1}w^T_{k-1}]\mathbf{\Gamma}^T_{k-1}\\ =\mathbf{\Phi}_{k-1}P_{k-1}\mathbf{\Phi}^T_{k-1}+\mathbf{\Gamma}_{k-1}Q_{k-1}\mathbf{\Gamma}^T_{k-1}$

Remark: (6)的结果需要用到假设 $\tilde{x}_{k-1}$ 和 $\mathbf{\Gamma}_{k-1}{w_{k-1}}$ 是不相关的。

由于 $Q_{k-1}$ 是噪声的协方差矩阵，是非负定的，估计误差的协方差 $P_k$ 实际上是随着时间推移非减（实际上往往是递增的）。因此很容易发现，随着状态的演化进行，虽然估计依旧是无偏估计，但方差在不断变大，过一段时间之后估计就会非常糟糕。因此我们需要别的修正手段来改进。

Remark: 理解误差传播的分析，我们就明白了，单靠状态方程的推演去估计状态是不可靠的。Kalman filter就用到了新的量测来修正状态估计值，提高估计精度。

3. 量测方程、递推滤波器 Measurement model, Recursive filter

有了状态演变的动力学模型后，我们需要提出量测方程(Measurement model or equation) 。量测方程的选择会影响最终估计结果——这直接影响我们在最小化MSE时得到的形式和结果。一般形式是这样子的，其中： $\mathbf{F}$ 是一系列非线性函数。

$\mathbf{z}_k=\mathbf{F}(\mathbf{x}_k,\mathbf{v}_k)\tag{7}$

Remark(Important!): 我们需要明确很重要的一点——量测方程描述了observation或者measurement $\mathbf{z}_k$ 、真实被估状态 $\mathbf{x}_k$ 与测量噪音 $\mathbf{v}_k$ 之间的关系。这里的observation或者measurement是我们根据量测方程得到的。当构造真实状态的估计 $\mathbf{\tilde{x}}_{k}$ 时，我们还可以自行规定 $\mathbf{\tilde{x}}_{k}$ 与 $\mathbf{z}_k$ 之间满足的关系。我们所谓的“线性估计”，指的是构造的estimator $\mathbf{\tilde{x}}_{k}$ 与测量值 $\mathbf{z}_k$ 之间满足线性关系，而不是说量测方程是线性方程。我们在第三篇中Linear MMSE的推导中也提到了，只要构造的估计量(这里是 $\mathbf{\tilde{x}}_{k}$ )与测量值（这里是 $\mathbf{z}_k$ ）之间是线性关系，那么就可以被称为Linear estimator。但是这样的Linear estimator是不是最佳的，即满足无偏性吗？满足线性估计量中MSE最小吗？即是否是一个Best Linear Unbiased Estimator(BLUE) ，这是不一定的！第三篇文章中，我们说到了只要量测方程也是线性的，那么我们就可以找到一个BLUE。当测量噪音和状态方程的噪声的分布都满足高斯分布时，那么这个BLUE甚至还是UMVUE，即是所有类型estimator（linear or nonlinear)中最优的（MMSE)。

我们不光采用线性估计，也采用线性量测模型，从而希望找到一个BLUE：

$\mathbf{z}_k=\mathbf{H}_k\mathbf{x}_k+\mathbf{\Psi}_k\mathbf{v}_k\tag{7}$

Remark: Again, 噪音 $\mathbf{\Psi}_k$ 传递矩阵可以不需要，但是要已知。所以很多书上索性直接没有加上，而是直接 $\mathbf{z}_k=\mathbf{H}_k\mathbf{x}_k+\mathbf{v}_k$ 。其中 $\mathbf{H}_k$ 为一个时变矩阵，其实多数时候这些系数都是可以认定为常数的，这里写的比较general。这个矩阵表明测量值被认为是真实状态的线性组合，附带上叠加的噪声。

递推滤波器我们在之前的文章（第二篇）中已经提过了。话不多说，采用recursive filter的形式，我们直接提出Kalman filter应该具有的形式：

$\hat{x}_k^{+}=K^{1}_k\hat{x}_k^{-}+K_k{z}_{k}\tag{8}$

其中 $\hat{x}_k^{-}$ 是在 $k$ 时刻，根据状态方程推算出来的 $x_k$ 的估计值。 $\hat{x}_k^{+}$ 是 $k$ 时刻经过 $z_k$ 修正过的 $x_k$ 的估计值。而 $K^{1}_k，K^{2}_k$ 是两个时变增益矩阵，目前待定。之所以采用 $\hat{x}^-_{k}$ 而不是 $\hat{x}^+_{k-1}$ ，是因为 $\hat{x}^-_{k}$ 已经通过了状态方程得到了更新，会比 $\hat{x}^+_{k-1}$ 上一步filter得到的结果更加准确一些。

remark: 上面的状态和测量都是向量，下面为了简便就不写成向量形式了，请记在心中。
第一次看这些公式的时候，你可能会被这些++--的符号给吓到了。其实没有那么复杂。

所以接下来我们的目标就是确定这两个增益矩阵的更新规律，从而得到Kalman filter的表达式。

4. 离散Kalman filter推导 Discrete Kalman filter

有了前面几篇文章和上面的一些准备工作推导工作将变得十分简单和容易理解。首先定义估计误差：

$\tilde{x}_k^+=\hat{x}^+_k-x_k\\ \tilde{x}_k^-=\hat{x}^-_k-x_k\tag{9}$

我们首先只考虑动力学方程对状态估计的影响。

假设我们已经得到了 $k-1$ 时刻经过(8)修正的 $\hat{x}_{k-1}^+$ 。经过动力学方程，根据(3)，我们得到了下一时刻 $k$ 的未经(8)修正的 $\hat{x}_{k}^-$ ：

$\hat{x}_{k}^-=\mathbf{\Phi}_{k-1}\hat{x}^+_{k-1}+\mathbf{B}_{k-1}u_{k-1}\tag{10}$

假设上一时刻 $k-1$ 经过(8)修正后的估计是无偏的，即 $E[\hat{x}^+_{k-1}]=0$ 。那么 $k$ 时刻未经量测修正的估计误差的期望为：

$E[\tilde{x}_k^{-}]=E[\hat{x}_{k}^{-}-x_k]=\mathbf{\Phi}_{k-1}E[\hat{x}^+_{k-1}]-\mathbf{\Gamma}_{k-1}E[w_{k-1}]=0\tag{11}$

$k$ 时刻未经量测修正的估计误差的协方差矩阵，根据(6)，为：

$P^-_k=\mathbf{\Phi}_{k-1}P^+_{k-1}\mathbf{\Phi}^T_{k-1}+\mathbf{\Gamma}_{k-1}Q_{k-1}\mathbf{\Gamma}^T_{k-1}\tag{12}$

Remark: 这里的 $P^+_{k-1}$ 是在 $k-1$ 时刻经过filter估计，经过修正的状态估计，因而为正号。

接下来我们考虑经过滤波器(8)处理后，得到的估计状态，以及其统计学特性。(8)的结构已经确定了。我们采用无偏性和最小方差来确定两个增益矩阵。我们希望滤波器修正后的状态估计是无偏的：

$\begin{align} E[\tilde{x}_k^+]=&E[\hat{x}_k^+-x_{k}]\\=&E[K^{1}_k\hat{x}_k^{-}+K_k{z}_{k}-x_k]\\ =&K^1_kE[\hat{x}^-_k-x_k]+K_k^1E[x_k]+K_kE[H_{k}x_k+\Phi_kv_k]-E[x_k]\\ =&K_k^1E[\tilde{x}^-_k]+[K_k^1+K_kH_k-I]E[x_k]+K_k\Phi_k{E}[v_k]\\ =&[K_k^1+K_kH_k-I]E[x_k]\\=&0 \end{align}\tag{13}$

由(13)得到滤波器(8)无偏的条件为： $K_k^1+K_kH_k-I=0$ . 把这个条件代入(8)我们得到：

$\hat{x}_k^{+}=(I-K_kH_k)\hat{x}_k^{-}+K_k{z}_{k}\\ =\hat{x}^{-}_k+K_k{(z_k-H_k\hat{x}^{-}_k)}\tag{14}$

这就是我们常见的recursive filter 以上一个估计值 $\hat{x}_k^{-}$ 和新的量测误差 $(z_k-H_k\hat{x}^-_k)$ 更新估计的形式。

接下来我们计算经过(8)更新后的状态估计误差的协方差矩阵：

$\begin{align} \tilde{x}_k^+=&\hat{x}_k^+-x_{k} \\=&K^{1}_k\hat{x}_k^{-}+K_k{z}_{k}-x_k\\ =&K^1_k\tilde{x}^{-}_k+K_k^1x_k+K_k[H_{k}x_k+\Psi_kv_k]-x_k\\ =&[K_k^1+K_kH_k-I]x_k+K_k^1\tilde{x}^{-}_k+K_k\Psi_kv_k \\=&(I-K_kH_k)\tilde{x}^{-}_k+K_k\Psi_kv_k \end{align}\tag{15}$

$P_k^+=E[(I-K_kH_k)\tilde{x}^{-}_k+K_k\Psi_kv_k)(I-K_kH_k)\tilde{x}^{-}_k+K_k\Psi_kv_k)^T]\\ =(I-K_kH_k)P^{-}_k(I-K_kH_k)^T+K_kR_kK_k^T\tag{16}$

其中定义了 $R_k=\Phi_kE(v_kv_k^T)\Phi_k^T$ ,用到了 $\tilde{x}^{-}_k$ 与 $v_k$ 不相关。我们定义 $k$ 时刻状态估计的方差和为 $P_k^+$ 的对角线元素之和（因为对角线是各状态分量的方差），为了找到 $K_k$ ，我们有：

$J_k=tr[P_k^+]\tag{17}$

让(17)对 $K_k$ 求偏导，利用B对称时， $\frac{\partial}{\partial{A}}[trace(ABA^T)]=2AB$ ，我们有：

$-2(I-K_kH_k)P_k^{-}H_k^T+2K_kR_k=0\tag{18}$

于是我们得到 $K_k$ 的表达式：

$K_k=P^-_kH^T_k(R_k+H_kP_k^-H_k^T)^{-1}\tag{19}$

将（19）得到的增益矩阵 $K_k$ 代入(16)，我们最终得到更新后的协方差矩阵为：

$P_k^+=(I-K_kH_k)P^{-}_k\tag{20}$

到这里推导就完成了。我们看到整个推导的过程用到了两个重要的标准，一个是无偏性，另一个是最小方差。其次我们还假设了量测方程的线性的，并且构造的滤波器(8)也是满足估计值与测量量之间是线性关系。

5. 离散Kalman filter方程总结 Summary of updating equations

我们下面把Kalman filter的更新方程放在一起列出来：

动力学方程状态更新公式： $\hat{x}_{k}^-=\mathbf{\Phi}_{k-1}\hat{x}^+_{k-1}+\mathbf{B}_{k-1}u_{k-1}$

首先要采用动力学方程将上一次经过滤波器修正的状态 $\hat{x}^+_{k-1}$ 进行更新，此时由于噪声是无法预测的，因而不必考虑噪声。

滤波器状态迭代更新公式 $\hat{x}^+_k=\hat{x}^-_k+K_k[z_k-H_k\hat{x}^{-}_k]$

这是Kalman filter的核心迭代方程，我们看到每次新的状态都是在 $k$ 时刻通过状态方程，但是未经修正的状态 $\hat{x}^-_k$ 的基础上，加上增益 $K_k$ 与 $[z_k-\hat{x}^{-}_k]$ 的乘积，后者我们也称之为innovation（新息）。

滤波器增益更新公式： $K_k=P^-_kH^T_k(R_k+H_kP_k^-H_k^T)^{-1}$

这是Kalman filter中增益的更新公式，增益将随着时间进行而变化。其中的 $H_k$ 是量测方程中已知的矩阵。

滤波器协方差更新公式： $P_k^+=(I-K_kH_k)P^{-}_k(I-K_kH_k)^T+K_kR_kK_k^T=(I-K_kH_k)P^{-}_k$

这是被filter更新过后的估计误差的协方差矩阵公式，我们利用此公式可以计算 $K_{k+1}$ 。

动力学方程协方差更新公式： $P^-_k=\mathbf{\Phi}_{k-1}P^+_{k-1}\mathbf{\Phi}^T_{k-1}+\mathbf{\Gamma}_{k-1}Q_{k-1}\mathbf{\Gamma}^T_{k-1}$

经过动力学方程后，协方差也得到了更新，因此两种协方差更新公式交替使用不断更新，从而使得 $K_k$ 也更新。

Kalman filter具体工作流程是这样的：

首先设置好更新所必需的初值： $\hat{x}_0^-$ 与 $P^-_0$ ，以及 $u_0,B_0,H_0$ 。然后计算 $K_0$ ,通过第一次测量 $z_0$ 代入滤波器迭代方程得到 $\hat{x}_0^+$ ，并更新 $P_0^+$ 。将 $\hat{x}_0^+$ 通过动力学方程更新后得到 $\hat{x}_1^-$ ，更新 $P^-_1$ 。这样我们就可以根据上面5个方程不断迭代得到 $\hat{x}_1^+,\hat{x}_2^+,\hat{x}_3^+,...$ 。其余的 $u_k,B_k,H_k$ 属于已知量可以提前离线计算好。

6. Kalman filter的特点与应用注意事项 Features and Notes

使用Kalman filter一系列的假设应当满足，之前也提到过了：动力学模型是线性的，量测模型也是线性的，状态噪声和量测噪声均为零均值的白噪声，这两种噪声，以及噪声与状态之间互不相关。否则滤波结果并不是线性最优的。当然不满足的时候也有解决办法，这里暂时我们不去深究。

我们看到Kalman filter与Wiener filter很大一点不同就是前者考虑了动力学模型，而后者并没有。然而单独使用动力学模型进行更新，我们从方程(6)中已经看到了估计误差的方差在将不断变大。Kalman filter采用了recursive filter的结构，因而更加适合计算机执行，免去了不断存储历史数据的问题。除此之外，它们相同之处是都采用了MMSE的标准来计算最优滤波器。我们在下一节将采用之前提到的正交投影法来推导Kalman filter，这也是Kalman本人采用的推导方法。

当量测方程中的代表噪声协方差的矩阵 $R_k$ 增大时，对应的增益 $K_k$ 将下降，代表了精度较差的测量值应该有较低的权重。当未修正协方差矩阵 $P_k^-$ 增大时， $K_k$ 将增大来分配更大的权重给测量值以修正；反之，就将 $K_k$ 减少来分配更多权重给动力学方程的一步预测。而 $P_k^-$ 直接受到 $Q_k$ 的影响。因而看到有人说：“ $Q_k$ 表示对模型的信任程度，R表示对量测的信任程度，卡尔曼滤波是在模型和量测之间进行均衡。”我们还观察到 $K_k,P_k^+,P_k^-$ 都可以离线直接提前计算，这样可以节省算力，提高效率。

Kalman filter的初值是相当重要的。在上面无偏性的推导中我们知道，初值的无偏性非常重要。如果我们知道初始的真值，那么我们就可以直接采用真值。但是多数时候真值获得是很奢侈的，可以采用初值的期望和初值的协方差矩阵来初始化计算 $\hat{x}_0^-=E[x(0)]，P_0^-=E[\hat{x}_0^--x(0)][\hat{x}_0^--x(0)]^T$ 。然而更多的时候，一无所知时，初值是靠猜测和尝试的，往往会令 $\hat{x}_0^-=0$ ，而令 $P_0^-=\alpha{I}$ ，其中 $\alpha$ 是一个很大数，迫使滤波器能够快速收敛。这类应用中的问题，很难在此一一说明，唯有多实践多查其他文献和资料才能有更多的体会。

还有一点注意的是，这里提出的Kalman filter是针对离散系统的。往往连续系统需要首先转化为离散系统。关于连续状态方程离散化的问题，可以参考[1] 5.3节。

7. Kalman原文解析 - 正交投影法推导 Kalman filter from orthogonal principle

1960年Kalman在 ASME - Journal of Basic Engineering（现已停刊) 上发表了名为：A new approach to Linear Filtering and Prediction Problems.的文章[7]，正式引入了Kalman filtering。我在第三篇文章里讲了好久LMMSE，也提到了正交投影法是如何帮助我们推导出Wienner filter的。我们这里采用正交投影法来推导Kalman filter。关于正交投影原理请你参考上一篇，我这里直接开始。

以一个简单的例子大概理解一下正交投影法：有一个平面外的向量（不正交于平面），问平面内能否找到另外一个向量，使其成为该向量最佳的估计？最佳的标准是这两个向量的之差的模最小。我们很容易理解，当直接把向量沿着垂直平面的方向投影就可以在平面中找到其“最佳估计”———这是一个可以由平面的两个基的线性组合表示的向量。它们之间的差向量正好垂直于平面，其距离最短，我们可以用直角三角形的斜边总是最长的来解释。

那么说白了，我们根据现有的测量值 $z_k, z_{k-1},z_{k-2},..., z_0$ 来寻找当下状态或者信号 $x_k$ 的最佳线性估计，也就是在 $z_k, z_{k-1},z_{k-2},..., z_0$ 这些向量（注意这里的 $z_k$ 等等是向量，每一次的测量值可以是有很多个的信号组成的）的线性组合构成的空间 $\mathcal{Z}_k=\{{z_k|z_k=\sum^k_{i=0}\alpha_iz_i}\}$ 中寻找一个最佳估计（现在不是平面了，是多维空间）。那么应用正交投影理论，我们很容易明白，其最佳估计正是 $x_k$ 在这个空间中的投影 $\hat{x}_k$ 。根据正交分解定理（见第三篇)，总有：

$x_k=\hat{x}_k+\tilde{x}_k\tag{21}$

其中 $\hat{x}_k\in{\mathcal{Z}_k}, \tilde{x}_k\perp{\mathcal{Z}_k}$ 。正式一点来讲，有以下定理（原出自[7] Theorem 2)：

定理：当 $\{x_{k}\},\{z_{k}\}$ 都是零均值的随机过程。如果随机过程是Gaussian的，或者optimal estimate被限制为观测值的线性函数，并且loss function是二次函数 $L(e_k)=e^2_k,e_k=x_k-\hat{x}_k$ 。那么
$\begin{align}\hat{x}_k&=\text{optimal estiamte of }x_{k}\text{ given }{z_k}\\ &=\text{orthogonal projection } \hat{x}_k \text{ of } x_k \text{ on } \mathcal{Z}_k \end{align}\tag{22}$

所以该定理确认了orghogonal projection就是我们要找的optimal linear estimate。

接下来的问题，我们需要recursively地计算这个optimal estimate。我们假设 $\mathcal{Z}_{k-1}$ ，即前 $k$ 个测量值(0到 $k-1$ ) 已知，并且 $z_k$ 已经测量到了。新的测量值 $z_k$ 在 $\mathcal{Z}_{k-1}$ ，根据正交分解定理，也应该有投影 $\hat{z}_k$ （投影为0的概率在现实中极小）以及正交部分 $\tilde{z}_k$ 。投影在 $\mathcal{Z}_{k-1}$ 那部分说明这些信息本身就存在于原来的测量值中，而正交于 $\mathcal{Z}_{k-1}$ 的 $\tilde{z}_k$ 属于新的信息，不包含在旧的测量值 $\mathcal{Z}_{k-1}$ ， $\tilde{z}_k$ 构成了一个linear manifold $\mathcal{\tilde{Z}}_k$ 。所谓的linear manifold就是Hilbert space中的一个对加法和数乘的封闭子集，类比于有限维向量空间中的子空间的存在。我们自然可以写成：

$\mathcal{Z_k}=\mathcal{Z}_{k-1}\cup\tilde{\mathcal{Z}}_k\tag{22}$

即加入新的测量值后的空间是原来空间，以及它与新测量值正交部分的并。

Remark: 在Kalman的原文[7]中，它采用了 $\hat{E}[x(t_1)|\mathcal{Y}(t)]$ 的符号来表示orthogonal projection。这个符号是受到了条件期望的启发，但是实际上并不是按期望的计算方法来计算的。

现在我们把第 $k$ 的optimal estimate 从 $\hat{x}_k$ 改记为 $\hat{x}_k^+$ ，以+和-区分是否只是经过了Kalman filter的更新，含义和之前的定义是一样的。我们用Kalman的符号 $\hat{E}[x_k|\mathcal{Z}_k]$ 来表示 $x_k$ 在 $\mathcal{Z}_k$ 上的投影，显然我们有：

$\begin{align} \hat{x}_k^+=&\hat{E}[x_k|\mathcal{Z}_k] \\=&\hat{E}[x_k|\mathcal{Z}_{k-1}]+\hat{E}[x_k|\mathcal{\tilde{Z}}_k] \\=&\hat{x}^-_k+\hat{E}[x_k|\mathcal{\tilde{Z}}_k]\tag{23} \end{align}$

Remark: 这里的分解成立是因为 $\tilde{\mathcal{Z}_k}$ 是正交于 $\mathcal{Z}_{k-1}$ 的，所以正交投影相当于分成了两部分，注意并不是条件期望。如果是条件期望的话，不符合这样的计算方式。

$\hat{E}[x_k|\mathcal{\tilde{Z}}_k]$ 这一项 $x_k$ 代表了在 $\tilde{\mathcal{Z}}_k$ 的投影，该投影实际上就是一个向量，故而能够定义一个系数矩阵，即我们的filter增益矩阵使得投影可以表示为：

$\hat{E}[x_k|\mathcal{\tilde{Z}}_k]=K_k\tilde{z}_k=K_k(z_k-\hat{z}_k)\tag{24}$

因为 $\hat{z}_k$ 是 $z_k$ 在 $\mathcal{Z}_{k-1}$ 上的投影：

$\begin{align} \hat{z}_k&=\hat{E}[z_k|\mathcal{Z}_{k-1}]\\&=\hat{E}[H_kx_k+\Psi_{k}v_k|\mathcal{Z}_{k-1}] \\&=H_k\hat{E}[x_k|\mathcal{Z}_{k-1}]\\&=H_k\hat{x}^-_{k} \end{align} \tag{25}$

因此(23) 变成了：

$\begin{align} \hat{x}_k^+=&\hat{x}^-_k+K_k(z_k-H_k\hat{x}^-_{k})\tag{26} \end{align}$

估计误差此时为：

$\begin{align} \tilde{x}_k^+=&\hat{x}^+_k-x_k\\ =&\hat{x}^-_k+K_k(z_k-H_k\hat{x}^-_{k})-x_k\\ =&\tilde{x}_k^{-}+K_k(H_kx_k+\Psi_kv_k-H_k\hat{x}_k^{-})\\ =&(I-K_kH_k)\tilde{x}_k^{-}+K_k\Psi_kv_k \end{align}\tag{27}$

我们发现和(15)是完全一致的。那么协方差 $P_k^+$ 就是(16)不用重复写了。

我们下面就是要找到这个 $K_k$ 。我们知道此时 $x_k-\hat{E}[x_k|\tilde{\mathcal{Z}}_k]$ 这部分是与 $\tilde{\mathcal{Z}}_k$ 正交的，也就是说当前状态与其在新测量上的投影部分的误差是正交的，于是这两者乘积的期望为0。

$\begin{align} 0=&E[(x_k-\hat{E}[x_k|\tilde{\mathcal{Z}}_k])\cdot\tilde{z}^T_k]\\ =&E[(x_k-K_k\tilde{z}_k)\cdot\tilde{z}^T_k]\\ =&E[x_k\tilde{z}^T_k]-K_kE[\tilde{z}_k\tilde{z}^T_k]\tag{28} \end{align}$

其中

$\begin{align} E[x_k\tilde{z}^T_k]=&E[(\bar{x}_{z_{k-1}}+\tilde{x}_{z_{k-1}})\tilde{z}^T_k]\\ =&E[\tilde{x}_{z_{k-1}}\tilde{z}^T_k]\\ =&E[(x_k-\bar{x}_{z_{k-1}})\tilde{z}^T_k]\\ =&E[(\Phi_{k-1}x_{k-1}+B_{k-1}u_{k-1}+\Gamma_{k-1}w_{k-1}-E[x_k|\mathcal{Z}_{k+1}])\tilde{z}^T_k]\\ =&E[(\Phi_{k-1}x_{k-1}+B_{k-1}u_{k-1}+\Gamma_{k-1}w_{k-1}-\Phi_{k-1}\hat{x}^-_{k-1}-B_{k-1}u_{k-1})\tilde{z}^T_k]\\ =&E[(-\Phi_{k-1}\tilde{x}^{-}_{k-1}+\Gamma_{k-1}w_{k-1})\tilde{z}_k^T]\\\tag{29} \end{align}$

Remark: 其中 $\bar{x}_{z_{k-1}}$ 是 $x_k$ 在 $\mathcal{Z}_{k-1}$ 上的投影， $\tilde{x}_{z_{k-1}}$ 是正交于 $\mathcal{Z}_{k-1}$ 的分量。

$\tilde{z}_k=z_k-\hat{z}_k=H_kx_k+\Psi_kv_k-H_k\hat{x}^-_{k}\\ =-H_k\Phi_{k-1}\tilde{x}^-_{k-1}+H_kw_{k-1}+\Psi_kv_k$

$\begin{align} E[x_k\tilde{z}^T_k] =&E[(-\Phi_{k-1}\tilde{x}^-_{k-1}+\Gamma_{k-1}w_{k-1})\tilde{z}_k^T]\\\tag{30} \\ =&E[-\Phi_{k-1}\tilde{x}_{k-1}^{-}(-H_k\Phi_{k-1}\tilde{x}^-_{k-1}+H_kw_{k-1}+\Psi_kv_k)^T]\\ &+E[\Gamma_{k-1}w_{k-1}(-H_k\Phi_{k-1}\tilde{x}^-_{k-1}+H_kw_{k-1}+\Psi_kv_k)^T]\\ =&E[\Phi_{k-1}\tilde{x}_{k-1}^{-}(\tilde{x}_{k-1}^{-})^T\Phi_{k-1}^TH^T_k]+E[\Gamma_{k-1}w_{k-1}w_{k-1}^TH^T_k]\\ =&P_k^-H_k^T \end{align}$

$\begin{align} E[\tilde{z}_k\tilde{z}_k^T]=H_kP^{-}_{k}H^T_k+R_k \end{align}\tag{31}$

代入(28)得到：

$K_k=P^-_kH^T_k(R_k+H_kP_k^-H_k^T)^{-1}\tag{32}$

8. 总结与评论

这篇我们把离散系统版本的Kalman filter完整地推导了一遍，从正交投影的方法，还有直接采用最小方差的方法。不管哪一种方法，结果都是一样的，由5个差分方程组成，不断迭代，只要初值选择合适，就会最终收敛。两种方法，其实本质上都以MMSE为标准，寻找Linear optimal estimate的。注意我们的假设需要成立才是optimal的。

Kalman filter的应用很广泛，实际应用中会碰到各种各样问题。我们把原理已经讲了，然而具体的实践内容就根据自己需要去摸索了。

之后我会讲一下continuous Kalman filter, 又名Kalman-Bucy filter, 以及使用广泛的Extended Kalman filter(EKF), Unscented Kalman filter(UKF), Adaptive Kalman filter(AKF)等等。还是以原理为主，时间有限，实际的代码算法就靠你自己去实践学习了。

再之后打算继续推进control的话题。原来Kalman filter只是为了LQG control做个铺垫，没想到事情繁忙一拖拖了3 个多月。那就下次再见了。

参考资料 Reference

[1] Gelb, A. (1974).Applied optimal estimation. MIT press.

[2] 王可东(2019).Kalman滤波基础及MATLAB仿真，北京航空航天大学出版社

[3] Grewal, M. S., & Andrews, A. P. (2015).Kalman filtering: Theory and Practice with MATLAB Fourth Edition. John Wiley & Sons.

[4] What is Stationary Process ?

[5] MMSE估计的来龙去脉

[6] Discrete Kalman Filter Tutorial

[7]Kalman, R. E.(March 1, 1960). "A New Approach to Linear Filtering and Prediction Problems." ASME.J. Basic Eng. March 1960; 82(1): 35–45.https://doi.org/10.1115/1.36625

24. 卡尔曼滤波器详解——从零开始(4) Kalman Filter from Zero

三脚猫Frank

1. Kalman filter 概述 Overview

2. 动力学模型的状态空间方程 Dynamic model - State-space representation

3. 量测方程、递推滤波器 Measurement model, Recursive filter

4. 离散Kalman filter推导 Discrete Kalman filter

5. 离散Kalman filter方程总结 Summary of updating equations

6. Kalman filter的特点与应用注意事项 Features and Notes

7. Kalman原文解析 - 正交投影法推导 Kalman filter from orthogonal principle

8. 总结与评论

为你推荐

【自动驾驶】运动规划丨速度规划丨时间维度

关于机器人状态估计(2)-大白话贝叶斯与概率

EKF SLAM学习笔记01

SLAM第三讲实践：相机位姿 VisualizeGeometry的实现（Pangolin安装以及cpp实现）

欧拉角速度与角速度的关系推导——欧拉运动方程

Power PMAC运动控制器 —— 学习笔记3

评论（0）

关于作者

三脚猫Frank

27

0

0

2

18. MIMO标准型 Brunovsky Canonical Form

20. LQR控制器— 线性二次型调节器 Linear Quadratic Regulator

17. 内模原理 Internal Model Principle

相关推荐

[8]基于NMPC的无碰撞轨迹跟踪在冗余蛇形机械臂上的仿真

优化方法理论合集（5）——波尔茨问题

SLAM -- 通过毫米波雷达详解EKF始末（2）

CoppeliaSim中力（矩）传感器的方向问题

（二十五）通俗易懂理解——批归一化（Batch Normalization）

PMAC应用八-SDK的使用2

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板