How a Kalman filter works, in pictures

本文是一个学习笔记，内容的 原文

What is it？

卡尔曼滤波是一种通用的强大的信息融合（combining information）工具（但是因为最原始卡尔曼滤波是一种线性的，对严重的非线性无法很好的估计，所以有后续的扩展卡尔曼滤波，后续更新），可以在任何有不确定信息（uncertain information）的地方使用，即使系统又很大的干扰也可以得到不错的结果，并且可以利用到你想象不到的相关性。

卡尔曼滤波非常适合这些不断变化的系统。它们具有的优点是：轻量级存储（它们不需要保存上一状态以外的任何历史数据），计算快速，这使得它们非常适合于实时问题和嵌入式系统。

What we do with a Kalman filter?

看一个小栗子：有一个机器人在深林里漫步，她需要知道自己哪里可以走

假设她在状态 $\overrightarrow{x_k}$，包含有位置和速度信息：$\overrightarrow{x_k}=(\vec{p},\vec{v})$

状态就是你系统基本配置（configuration）的数字列表；它可以是任何变量。在这个例子中，状态就是位置和速度，但是它可以是油罐中液体的总量数据、引擎的温度、用户手指在平板上的位置，或者任何你想要观察的数据。

我们的机器人有一个 GPS，精确约为10米，正常工作，但是我们对机器人定位的精度要求高于10米。树林里有很多沟和崖，如果机器人走错几英尺，就可能跌落悬崖。所以仅有 GPS 是不够的。

我们还知道机器人如何移动的一些信息：她发送给轮子的指令，她的头的朝向以及没有障碍，下一步，它可能就沿着这个方向前进。当然，它不可能知道运动过程中的所有状况：它可能被风刮倒，车轮可能打滑，或者在颠簸的路面侧翻；所以轮子的转角并不能精确的表示机器人走了多远，这个预测不是完美的。
GPS告诉我们一些状态信息，但是不直接的，有一些不确定性和不准确性。我们的预测是告诉我们机器人如何移动，但也是不直接的，有一些不确定性和不准确性。
如果我们能利用所有可用的信息，能给我们跟好的估计结果，这既是卡尔曼滤波的精髓。

How a Kalman filter sees your problem

继续刚才的问题，我们还是使用简单的状态，只有位置和速度的情况：
$$\vec{x}=\left[\begin{array}{c}p\\ v\end{array}\right]$$
我们不知道实际的位置和速度，有一系列可能的位置与速度的组合，但是其中的一些可能性更高：

上图是一个二维高斯分布的投影图片，卡尔曼滤波假设两个变量（位置和速度，在我们的case中）是随机高斯分布的。每个变量都有均值$\mu$(随机分布的中心)和方差${\sigma }^2$(表示不确定性)。

上图表示的是位置和速度是不相关的，相关性的概念在概率论里有讲。大概意思就是一个变量不会告诉你另一个变量的信息。

下面的例子中：位置和速度是相关的。一个指定位置的可能观测值取决于速度：

这种情况用于利用上旧的状态估计下一个状态，速度越大跑的越远。

这种关系在跟踪定位时非常重要，因为这会提供更多信息：一个测量值告诉我们其他测量值的可能范围。这就是卡尔曼滤波器的目标，我们希望从不确定的测量值中得到尽可能多的信息。

这种关系由协方差矩阵给出。简单来说，协方差矩阵的每个元素${\Sigma }_{ij}$都是第$i$个状态变量和第$j$个状态变量的相关程度。(所以协方差矩阵是对称的，交换$i$和$j$并不影响)协方差矩阵通常用“ $\Sigma$”表示，所以将其元素称为“${\Sigma }_{ij}$”。

Describing the problem with matrices

我们要建模需要在时间$k$上的Gaussian blob，需要两条信息，最优估计${\hat{\mathbf{x}}}_{\mathbf{k}}$（也称为均值$\mu$）和协方差矩阵${\mathbf{P}}_{\mathbf{k}}$。
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& =\left[\begin{array}{c}\text{position}\\ \text{velocity}\end{array}\right]\\ {\mathbf{P}}_k& =\left[\begin{array}{cc}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{vp}& {\Sigma }_{vv}\end{array}\right]\end{array}\text{\hspace{1em}(1)}$$
（这里只用到了位置和速度，但记得状态可以包括许多变量你想要的都可以呈现上去）

接下来我们需要在当前状态（current state）$k-1$ 时刻去预测在在 $k$ 时刻的下一个状态。记得我们并不知道哪个状态是真实的，但是预测方程并不关心。它根据所有可能状态，给出一个新的分布：

我们可以用矩阵 ${\mathbf{F}}_{\mathbf{k}}$ 表示该预测步骤：

把我们原始估计（original estimate）的每一个点移动到新的预测点，如果原始估计是对的话，这些预测点就是系统可能移动的地方。（注：这里的原始估计就是上图中蓝色的框框的区域，是$k-2$时刻估计出来的）
我们用基本的运动学方程来表示这个估计：
$$\begin{array}{rlr}{p}_k& =p_{k-1}+\Delta t& v_{k-1}\\ v_k& =& v_{k-1}\end{array}$$
用矩阵表示：

$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& =\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]{\hat{\mathbf{x}}}_{k-1}\\ & ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}\end{array}\text{\hspace{1em}(2)}$$
我们现在有了预测下一状态的预测矩阵，但是仍然不知道怎样更新协方差矩阵。

我们需要另一个方程。如果我们对分布中的每一个点乘以一个矩阵$\mathbf{A}$，那么协方差矩阵$\Sigma$这比较简单，我将直接给出：

$$\begin{array}{rl}Cov(x)& =\Sigma \\ Cov(\mathbf{A}x)& =\mathbf{A}\Sigma {\mathbf{A}}^T\end{array}\text{\hspace{1em}(3)}$$
结合（2）和（3）：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}\\ {\mathbf{P}}_k& ={\mathbf{F}}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T\end{array}\text{\hspace{1em}(4)}$$

External influence

我们没有利用到所有信息，外部世界的某些变化可能跟状态没有关系，但是会影响到系统。例如列车员推动气阀会使车的速度变化，同样我们的机器人会发送指令给轮子走或停。如果我们知道更多的关于真实世界的信息，我们可以将这些写成一个向量叫做$\overrightarrow{{\mathbf{u}}_k}$，加入到我们的预测作为一种修正。
例如我们知道加速度$a$表示控制指令，从基础的运动学公式：
$$\begin{array}{rlrl}{p}_k& =p_{k-1}+\Delta t& v_{k-1}+& \frac{1}{2}a{\Delta t}^2\\ v_k& =& v_{k-1}+& a\Delta t\end{array}$$
矩阵形式：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]a\\ & ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k\overrightarrow{{\mathbf{u}}_k}\end{array}\text{\hspace{1em}(5)}$$
${\mathbf{B}}_k$叫控制矩阵(control matrix),$\overrightarrow{{\mathbf{u}}_k}$叫控制向量(control vector)，如果没有外界控制影响可以忽略他们。

External uncertainty

如果状态只根据自身的属性变化，则没有问题。如果状态同时根据外部作用变化，只要我们知道外部作用力是啥，也没有问题。原文举例这里不解释了。

我们关于“世界”（我们没有keep track的）的不确定性建模，每一步预测都添加一点新的不确定性：

在原始估计中的每一个状态都能够转移到一定状态范围。因为高斯斑，可以说，在${\hat{\mathbf{x}}}_{k-1}$的每一个点，都移动到了一个协方差为${\mathbf{Q}}_k$的高斯斑中。换一种说法就是，我们将不确定的影响看作是协方差${\mathbf{Q}}_k$的噪声。

(注：增加了不确定性以后，原来的一个点转移到下一时刻就是根据方程算出来对应的某处，但是增加了不确定性以后，可能在新的区域绿色框框内的某处了)

这会产生一个新的高斯斑，有新的协方差（相同的期望）：

这个扩展的协方差通过加${\mathbf{Q}}_k$得到，所以完整的预测表达式：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k\overrightarrow{{\mathbf{u}}_k}\\ {\mathbf{P}}_k& ={\mathbf{F}}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{Q}}_k\end{array}$$
也就是说，新的最优估计是由上一时刻的最优估计进行预测，加上已知外部影响的修正得到的。

新的不确定性是由上一时刻的不确定性进行预测，加上一些环境中额外的不确定性。

这就简单了，我们有了系统当前状态的模糊估计(${\hat{\mathbf{x}}}_k,{\mathbf{P}}_k$)。接下来看看从传感器获取数据时发生了什么。

Refining the estimate with measurements(测量值矫正估计)

系统有一些传感器，间接给一些状态信息，换句话说就是：传感器在某一状态，给出一系列读数。

读数的单位和量程可能与状态的不同，我们用矩阵${\mathbf{H}}_k$对传感器建模。

我们能够得到传感器读数的分布：
$$\begin{array}{rl}{\vec{\mu }}_{\text{expected}}& ={\mathbf{H}}_k{\hat{\mathbf{x}}}_k\\ {\mathbf{\Sigma }}_{\text{expected}}& ={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T\end{array}$$
卡尔曼滤波适合于处理传感器的噪声。也就是说，传感器多少都会有些不可靠性，原始估计中的每一个状态的传感器读数都会是一个范围值。

从我们观测到的每一个读数，我们可能猜测系统在一个特定的状态。但是因为存在不确定性，观测到的数据中某些状态的可能性会比较高。（眼见不一定为实，某些数据偏高了）

称这个不确定性（传感器噪声）的协方差为${\mathbf{R}}_k$。该分布的期望和观测到的读数相同 ，称为$\overrightarrow{{\mathbf{z}}_k}$。

现在，我们得到两个高斯斑：一个表示系统状态预测；一个表示传感器读数。

接下来就是要协调预测值和传感器读数值。

所以哪个更接近真实的状态呢？我们认为观测的是错误的，预测值是我们想看到的。如果我们有两个概率，想知道两个概率都是真的（重叠）只要把他们乘起来（概率论的知识）。

重叠的部分就是最有可能的部分，就是根据已有的新的的最好估计。看起来像另一个高斯斑（gaussian blob）。

Combining Gaussians

（注：这节前面就是一些概率论的知识，两个高斯函数相乘的分布等等，不会的就补数学知识咯）
先假设一维的高斯分布相乘：$\begin{array}{r}\mathcal{N}(x,{\mu }_0,{\sigma }_0)\cdot \mathcal{N}(x,{\mu }_1,{\sigma }_1)\stackrel{?}{=}\mathcal{N}(x,\mu ’,\sigma ’)\end{array}$ 结果：
$$\begin{array}{rl}\mu ’& ={\mu }_0+\frac{{\sigma }_0^2({\mu }_1–{\mu }_0)}{{\sigma }_0^2+{\sigma }_1^2}\\ {\sigma ’}^2& ={\sigma }_0^2–\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2}\end{array}$$
简单分解出一个因子$\mathbf{k}$：
$$\begin{array}{r}\mathbf{k}=\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}\end{array}$$

$$\begin{array}{rlr}\mu ’& ={\mu }_0+& \mathbf{k}({\mu }_1–{\mu }_0)\\ {\sigma ’}^2& ={\sigma }_0^2–& \mathbf{k}{\sigma }_0^2\end{array}$$
我们把上式写成高维的形式：$\Sigma$表示高斯斑的协方差矩阵，$\vec{\mu }$表示每个轴的均值：
$$\begin{array}{r}\mathbf{K}={\Sigma }_0({\Sigma }_0+{\Sigma }_1{)}^{-1}\end{array}$$
$\mathbf{K}$称为卡尔曼增益矩阵。

Putting it all together

我们有两个分布：预估测量$({\mu }_0,{\Sigma }_0)=({\mathbf{H}}_k{\hat{\mathbf{x}}}_k,{\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T)$和观测测量$({\mu }_1,{\Sigma }_1)=(\overrightarrow{{\mathbf{z}}_k},{\mathbf{R}}_k)$，把他们弄到一起：
$$\begin{array}{rlrl}{\mathbf{H}}_k{\hat{\mathbf{x}}}_k’& ={\mathbf{H}}_k{\hat{\mathbf{x}}}_k& +& \mathbf{K}(\overrightarrow{{\mathbf{z}}_k}–{\mathbf{H}}_k{\hat{\mathbf{x}}}_k)\\ {\mathbf{H}}_k{\mathbf{P}}_k’{\mathbf{H}}_k^T& ={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T& –& \mathbf{K}{\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T\end{array}$$
卡尔曼增益是：
$$\begin{array}{r}\mathbf{K}={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{-1}\end{array}$$
把 ${\mathbf{H}}_k{\hat{\mathbf{x}}}_k’$ 的 ${\mathbf{H}}_k$ 去掉，把 ${\mathbf{H}}_k{\mathbf{P}}_k’{\mathbf{H}}_k^T$ 的左右两边都去掉，怎么去就是 线性代数的基本问题了，得到：
$$\begin{array}{rlrl}{\hat{\mathbf{x}}}_k’& ={\hat{\mathbf{x}}}_k& +& \mathbf{K}’(\overrightarrow{{\mathbf{z}}_k}–{\mathbf{H}}_k{\hat{\mathbf{x}}}_k)\\ {\mathbf{P}}_k’& ={\mathbf{P}}_k& –& \mathbf{K}’{\mathbf{H}}_k{\mathbf{P}}_k\end{array}$$

$$\begin{array}{r}\mathbf{K}’={\mathbf{P}}_k{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{-1}\end{array}$$
得到了完整的更新步骤。

${\hat{\mathbf{x}}}_k’$是校正后的最优估计，我们可以继续，将其与${\mathbf{P}}_k’$投入下一轮的预测、更新。

后记

后记是我在看这篇intro的时候学到的一些知识点。感觉对一些参数有更深刻的理解或者解开了一些疑惑吧。

1. 首先对于参数 $A$ ，就是上面写的 $F_k$ 是 $k-1$ 到 $k$ 的关系矩阵，实际过程中可能是个变化量，这个值跟现控里面的状态空间表达式那个有点类似吧（这个我个人感觉，还不太确定待求证）；$B$ 是和输入相关的矩阵；$H$ 是测量状态到测量值 $z_k$ 的变换矩阵，实际过程也可能是变换的，有时候假设不变。
2. 测量误差协方差 $R_k$ 通常都是经验值，我们也可以使用一些off-line sample measurements去计算，但是怎么算，概率论只学了一维的协方差计算，还没学习高维的，以后可能会学到。
3. 过程噪声协方差 $Q_k$ 通常更难确定，因为我们没有能力去直接观察我们estimating的过程，但是通常简单的过程模型就能产生不错的结果了，如果过于复杂就会给系统认为“injects”不确定性。
4. 通常我们就手动调参，理论上能获的更好的结果，如果需要先验地算出这两个参数，就需要系统辨识（system identification）的知识，我表示不会。
5. 通常在 $Q$ 和 $R$ 是固定的常数下，$P_k$ 和 $K_k$ 可以快速地收敛并保持不变，这种情况下我们就可以off-line地估计出这两个数。