卡尔曼滤波教程

总览

“如果你不能把它解释地足够简洁，说明你还不够理解它。” —— 阿尔伯特·爱因斯坦

关于作者

我叫Alex Becker，我来自以色列。我是一名在无线领域有着超过20年经验的工程师。我工作的一部分便是和卡尔曼滤波器打交道，主要应用于目标跟踪。

我始终接受建设性的批评，我很乐意倾听你的评价和建议。如有需要请给我发邮件。

免责声明：本教程的所有数值示例所体现和涉及到的、包括但不限于设计模式、方法论、技术和参数等，与作者已知或从事的任何应用系统均无关。

关于卡尔曼滤波

许多现代系统均使用多个传感器的多个测量结果来估计隐藏（未知）状态。例如GPS接收机能够用来估计位置和速度，此处位置和速度就是隐藏状态，接收到的卫星信号时差就构成了一组测量。

对跟踪和控制系统而言，最大的挑战之一便是在存在不确定性的情况下获取一个对隐藏状态的精准的估计。例如GPS接收机会受各种外部干扰，如热噪声、大气效应、卫星轨道微小变化、接收机时间精度等。

卡尔曼滤波是最常用最重要的状态估计算法之一。卡尔曼滤波器能从不确定且非精确的测量中估计隐藏状态，同时还可以根据历史估计值对未来系统状态进行预测。

这种滤波算法以 Rudolf E. Kálmán (1930年5月19日 - 2016年7月2日) 的名字命名。在1960年，卡尔曼发表了著名的论文，描述了一个离散数据的线性滤波问题的递归解法。

如今卡尔曼滤波广泛应用于目标追踪、定位和导航系统、控制系统、计算机图形学等等许多领域。

为什么需要预测算法

在开始讲解卡尔曼滤波以前，我们先闹明白为什么需要一套跟踪和预测算法。

为说明这一点，我们拿一个目标追踪雷达来举例。

假设我们每5秒发起一次追踪，即以5秒为间隔，雷达发出一束波束照射目标。

雷达波到达目标并返回后，它开始估计目标当前的位置和速度。雷达同时还能估计（或预测）目标在下一个测量周期即5秒后的位置。

目标未来的位置可以通过牛顿运动方程得到：
$$x=x_0+v_0\Delta t+\frac{1}{2}a\Delta t^2$$其中：
$x$ 是目标位置
$x_0$ 是初始目标位置
$v_0$ 是初始目标速度
$a$ 是目标加速度
$\Delta t$ 是采样周期（本例中为5秒）
处理三维空间中的运动时，牛顿运动方程可以以方程组形式给出：
$$\{\begin{array}{l}x=x_0+v_{x0}\Delta t+\frac{1}{2}{a}_x\Delta t^2\\ y=y_0+v_{y0}\Delta t+\frac{1}{2}{a}_y\Delta t^2\\ z=z_0+v_{z0}\Delta t+\frac{1}{2}{a}_z\Delta t^2\end{array}$$
这组目标的参数 $\left[x,y,z,v_x,v_y,v_z,a_x,a_y,a_z\right]$ 称为 系统状态。当前状态作为预测算法的输入，预测算法的输出则是目标未来的状态，即目标在下一个采样点所具有的参数。

上面的牛顿运动方程组称为 动态模型 或 状态空间模型。动态模型描述了预测算法的输入和输出之间的关系。

显然，如果目标当前的状态和动态模型是已知的，预测目标后续的状态就可以很简单地实现。

实际中，雷达的测量并不完全准确，它所包含的随机噪声或不确定性会影响对目标状态的预测的精准度。误差大小取决于多方面因素，例如雷达校准、波束宽度、回波的信噪比等。雷达测量的随机噪声或不确定性称为 测量噪声。

另外，由于存在风扰、飞行员干预等因素，目标的运动并不总能依照运动方程。运动方程预测和实际目标的运动轨迹之间的差异称为 过程噪声。

由于测量噪声和过程噪声的存在，估计的目标位置有可能和目标的真实位置大相径庭。这种情况下，雷达的波束完全可能指向错误的方位，从而跟丢目标。

为了提升雷达跟踪的精准度，一套能对抗测量和模型的不确定性的预测算法就至关重要。

最常用的跟踪和预测算法就是 卡尔曼滤波。

KALMAN滤波简介

必要的背景知识

在开始之前，我想先解释几个基本用语，例如方差、标准差、正态分布、估计、准度、精度、均值、期望，和随机变量。

本教程的读者应该大部分已经对统计学比较熟悉。但在本教程的开始我承诺过会提供一些理解卡尔曼滤波器所需的背景知识。因此如果你对统计学已经比较熟悉，可以跳过这一章内容直接开始下一章。

均值和期望

均值 Mean 和 期望 Expectation 是密切相关但有所不同的概念。

举个例子，给你五枚硬币 - 两个5分硬币和三个10分硬币，我们可以轻松地计算出这些硬币面值的均值，即求它们面值的平均数。

$$V_{mean}=\frac{1}{N}\sum _{n=1}^N{V}_n=\frac{1}{5}\left(5+5+10+10+10\right)=8cents$$
因为本系统中的状态（硬币面值）直接写在了硬币上，并且就是真实值，不存在任何不确定性，因此我们可以使用全量样本（全部五个硬币）的真实值来计算平均值，此时的平均值并不是期望。

假设给同一个人的体重进行五次测量，得到五个不同的测量值：79.8kg，80kg，80.1kg，79.8kg和80.2kg。将这个人看作一个系统，他/她的体重就是该系统的状态。

五次测量值之所以各不相同是因为体重秤存在随机测量误差。其实想一想就能知道，我们无法得知体重的真值，因为它是一个所谓隐藏状态（译注：原文为Hidden State，意为客观存在但是无法直接读取的能观状态）。但是我们可以通过对体重秤的多次测量值求平均来对这个真值进行估计。

$$W=\frac{1}{N}\sum _{n=1}^N{W}_n=\frac{1}{5}\left(79.8+80+80.1+79.8+80.2\right)=79.98kg$$
上述估计值，就是体重的期望。

所谓期望，可以直观理解为对一个隐藏状态进行足够多次测量以后它所应表现出来的值（译注：这一段原文作者为了简单起见叙述并不是很好，简单理解为，我们无法知道随机变量的真值，只能进行测量，测量得到的样本含有随机性，服从某个分布，我们可以依照分布对样本求概率加权平均，当样本数趋于无穷大时的概率加权平均值的极限就是该随机变量真值的期望。即平均值与样本对应，期望与随机变量对应，样本是我们能看到的，真值我们看不到，二者之间的桥梁就是样本数趋于无穷大）。

均值通常使用希腊字母 μ 来表示。

字母 E 通常表示期望。

方差和标准差

方差 Variance是对数据样本在其均值附近散布情况进行的度量，标准差 Standard Deviation是方差的平方根。

标准差通常用希腊字母 $\sigma$. 对应地，方差即为 ${\sigma }^2$.

现在假设我们想比较两个高中篮球队的身高分布情况，下表给出了两支球队球员的身高，以及每支球队身高的均值。

        球员 1	球员 2	球员 3	球员 4	球员 5	身高均值
球队 A	1.89m	2.1m	1.75m	1.98m	1.85m	1.914m
球队 B	1.94m	1.9m	1.97m	1.89m	1.87m	1.914m

可见，两支球队的身高均值是相同的。我们再来看它们的方差。

由于方差度量的是数据样本的散布情况，我们需要计算每个样本到均值的离散程度。这可以通过将均值从每个样本中减去来实现。

身高记为 $x$，身高的均值记为 $\mu$. 样本到均值的差即为：

$$x_n-\mu =x_n-1.914m$$
下表列出每个球员身高到平均值的差异。

        球员 1	球员 2	球员 3	球员 4	球员 5
球队 A	-0.024m	0.186m	-0.164m	0.066m	-0.064m
球队 B	0.026m	-0.014m	0.056m	-0.024m	-0.044m

有些值是负数，而数据散布的度量应是无符号的。可以对这些值求平方来消除负数（译注：求绝对值也能消除负数，但绝对值不可导，并且散布度量一般希望强调散布更远的样本，平方能够自加权）：

$${\left(x_n-\mu \right)}^2={\left(x_n-1.914m\right)}^2$$
下表列出每个球员身高到平均值的差异平方。

        球员 1	    球员 2	    球员 3	    球员 4	    球员 5
球队 A	0.000576m2	0.034596m2	0.026896m2	0.004356m2	0.004096m2
球队 B	0.000676m2	0.000196m2	0.003136m2	0.000576m2	0.001936m2

接下来对这些平方值再求平均来得到方差：

$${\sigma }^2=\frac{1}{N}\sum _{n=1}^N{\left(x_n-\mu \right)}^2$$
对球队A，身高方差为：

$${\sigma }_A^2=\frac{1}{N}\sum _{n=1}^N{\left(x_n-\mu \right)}^2=\frac{1}{5}\left(0.000576+0.034596+0.026896+0.004356+0.004096\right)=0.014m^2$$
对球队B，身高方差为：

$${\sigma }_B^2=\frac{1}{N}\sum _{n=1}^N{\left(x_n-\mu \right)}^2=\frac{1}{5}\left(0.000676+0.000196+0.003136+0.000576+0.001936\right)=0.0013m^2$$
可见，尽管两支球队的身高均值是相同的，球队A的身高散布要比球队B大。因此球队A的球员身高多样性要比球队B高。一支球队有多个不同的位置，例如控球后卫、得分后卫和中锋等，球队B身高分布过于集中，因而不便应对不同的位置带来的差异化要求。

本例中方差的量纲是米平方。通常用标准差更方便，因为标准差是方差的平方根，因此标准差的量纲和样本一致。

$$\sigma =\sqrt[]{\frac{1}{N}\sum _{n=1}^N{\left(x_n-\mu \right)}^2}$$
球队A球员身高的标准差是0.12m.

球队A球员身高的标准差是0.036m.

现在，再假设我们想求取全国所有高中篮球队队员身高的均值和方差。这是个海量工作量的任务 - 需要收集每一所高中的每一个球员的身高数据。

另一方面，我们也可以通过收集部分样本（一个量足够大的样本集），根据这个样本集对全国所有高中篮球队队员身高的均值和方差进行估计。

一个有100个随机选择出的球员的身高数据的样本集已经可以提供足够准确的估计了。

但是，从部分样本对真实方差进行估计的方程和从全量样本进行方差计算的方程略有不同。求和后的归一化系数是$N-1$而并非 $N$：

$${\sigma }^2=\frac{1}{N-1}\sum _{n=1}^N{\left(x_n-\mu \right)}^2$$
这个 $\frac{1}{N-1}$系数称为贝塞尔校正。

在visiondummy 和 Wikipedia上能看到这个矫正的数学证明和推导。

正态分布

自然界许多现象都遵循 正态分布 Normal Distribution。正态分布又称为 高斯分布 Gaussian Distribution （纪念著名数学家卡尔·弗莱德利希·高斯），其表达式如下：

$$f\left(x;\mu ,{\sigma }^2\right)=\frac{1}{\sqrt[]{2\pi {\sigma }^2}}{e}^{\frac{-{\left(x-\mu \right)}^2}{2{\sigma }^2}}$$
其函数图像（高斯曲线）又被称为正态分布的 概率密度函数（PDF）。

下表描述了三个不同城市A、B和C里披萨外卖送达时间的概率密度函数。

在A城，平均送达时间为30分钟，标准差为5分钟。
在B城，平均送达时间为40分钟，标准差为5分钟。
在C城，平均送达时间为30分钟，标准差为10分钟。
可以看到A城和B城的高斯曲线的形状是一样的，但是其中心对称轴位置不同。这表明在A城点披萨比在B城要平均少等待10分钟，但两座城市里送达时间的散布是差不多的。

还可以看到A城和C城的高斯曲线中心对称轴位置相同，但是形状不同。因此尽管平均送达时间一样，但散布不同。

下表列出了正态分布的曲线围成面积的比例情况：

A城里68.26%的送达时间落在 $\mu \pm \sigma$ 区间内（25-35分钟）
A城里95.44%的送达时间落在 $\mu \pm 2\sigma$ 区间内（20-40分钟）
A城里99.74%的送达时间落在 $\mu \pm 3\sigma$ 区间内（15-45分钟）
通常情况下，测量误差是正态分布的。卡尔曼滤波器假设测量误差具有正态分布。

随机变量

一个 随机变量 Random Variable 可以用来描述系统的隐藏状态。随机变量是一个集合，包含对应的随机试验所有可能的结果取值。

随机变量可以是连续的也可以是离散的：

• 连续随机变量可以在一个给定范围内取任何值，例如电池充电时间或者马拉松比赛时间。
• 离散随机变量是可数的，例如网站访问人数或者班里学生的数量。

随机变量使用概率密度函数描述，概率密度函数具有一个特性，称为 矩 Moment，矩是随机变量幂的期望。我们重点关注两种矩：

• $k$ 阶原点矩，是随机变量 $k$ 次幂的期望：$E\left(X^k\right)$.
• $k$ 阶中心距，是随机变量关于均值的散布的 $k$ 次幂的期望：$E\left({\left(X-{\mu }_X\right)}^k\right)$.

本教程中，随机变量由下述两种矩进行描述：

• 一阶原点矩 $E\left(X\right)$ - 测量结果的均值。
• 二阶中心矩 $E\left({\left(X-{\mu }_X\right)}^2\right)$ - 测量结果的方差。

估计，准度和精度

所谓 估计 Estimation 是对系统的隐藏状态的估计。例如某飞行器的真实位置对观测者而言是不可见的，我们可以用传感器，例如雷达，来估计飞行器的位置。通过使用多个传感器并且使用先进的估计和跟踪算法（例如卡尔曼滤波）能够大幅提升对飞行器位置估计的效果。每次这样的测量和计算都是一次估计。

准度 Accuracy 描述测量值与真值的接近情况。

精度 Precision 描述一系列测量值相对同一个真值的偏差分布情况。

准度和精度是衡量一个估计的最基础和重要的指标。下图展示了准度和精度的区别：

高精度系统的测量值具有很低的方差（即低不确定性），而低精度系统具有高方差（即高不确定性）。随机测量噪声造成了这些不确定性。

低准度系统又被称为 有偏 Biased 系统，源自它们的测量结果往往受系统性误差（偏差）的影响。

散布对测量的影响可以通过对测量结果求平均或进行平滑来降低。比如，用温度计测量的室内温度会包含随机测量噪声，我们可以测量多次然后取平均。因为误差是随机的，有些测量会比真值大，有些测量会比真值小，二者在求平均中抵消后平均结果会更接近真值。进行这样的测量越多，求得的估计值就越准确。

另一方面，一个有偏的温度计的测量结果经过上述估计后，仍然是有偏的。

本教程中所有的示例均假定测量系统是 无偏 Unbiased 的。

小结

下图从统计学角度展示了测量的过程。

一次测量是对一个随机变量的取样，该随机变量由一个 概率密度函数 (PDF)来描述。

多次测量的平均值就是该随机变量的 期望。

均值和真值之间的差是该测量系统的 偏差 或者 系统性误差，它构成了测量系统的 准度。

测量值的散布程度是该测量系统的 测量噪声，又叫 随机测量误差 或 测量不确定性，它构成了测量系统的 精度。

$\alpha -\beta -\gamma$滤波器

示例 1 – 给金条称重

现在介绍第一个简单示例。本例对一个静态系统的状态进行估计。所谓静态系统，是指在合理时间范围内系统状态不会自发改变的系统。例如一座塔便是一个静态系统，高度便是其状态之一，它不随时间改变而变化。

本例中，我们估计一根金条的重量。假定我们用来称金条的秤是无偏的，即称重结果没有系统性偏差，但是有随机噪声。

金条就是我们所关心的系统，金条的重量就是该系统的状态。该系统的动态模型是恒定的，因为我们假定金条的重量（在短时间内）不会发生变化。

为了估计出该系统的状态（金条重量），我们可以对其进行多次称重，然后取多次测量结果的平均值。

在时刻 $n$，估计值 ${\hat{x}}_{n,n}$ 便是所有之前测量的平均值：

$${\hat{x}}_{n,n}=\frac{1}{n}\left(z_1+z_2+\dots +z_{n-1}+z_n\right)=\frac{1}{n}\sum _{i=1}^n\left(z_i\right)$$
注解：
$x$ 是金条重量的真值
$z_n$ 是 $n$ 时刻对金条重量的测量值
${\hat{x}}_{n,n}$ 是在 $n$ 时刻，使用了 $n$ 时刻的测量值 $z_n$，对 $x$ 的估计值
${\hat{x}}_{n+1,n}$ 是在 $n$ 时刻对未来状态（$n+1$ 时刻）的预测，记为 ${\hat{x}}_{n+1,n}$ ，或者说外插
${\hat{x}}_{n-1,n-1}$ 是在 $n-1$ 时刻，使用了 $n-1$ 时刻的测量值 $z_{n-1}$，对 $x$ 的估计值
${\hat{x}}_{n,n-1}$ 是一个先验估计 - 在 $n-1$ 时刻对 $n$ 时刻的系统状态所进行的预测（译注：对第n个时刻而言， ${\hat{x}}_{n,n-1}$ 是先验估计， ${\hat{x}}_{n+1,n}$ 是预测）
注：在本教程中，变量上的尖号符号（或者叫hat）代表这是一个对该变量的估计值。
由于金条的重量不随时间改变而改变，系统动态模型在本例中是静态的（恒定），因此有 ${\hat{x}}_{n+1,n}={\hat{x}}_{n,n}$ .

上面求平均的表达式虽然在数学层面是正确的，但是它不具备可实现性。这是因为根据平均值的定义，为了估计 ${\hat{x}}_{n,n}$ ，我们需要存储下所有的历史测量值，这对内存开销巨大。并且每次获得了新的测量值后都需要完全重新从第一次测量开始计算，这对CPU算力也消耗巨大。

现实一点的考虑是，最好只需存储上一时刻的估计值 ${\hat{x}}_{n-1,n-1}$ ，并在新的测量完成后更新它即可。下图描述了这个思路：

根据当前的测量和先验估计，估计当前的状态。
根据当前的状态估计以及系统动态模型，进行下一时刻的预测。

把上述求平均的表达式在数学上等效变换一下，可以得到：

表达式	注解
${\hat{x}}_{n,n}=\frac{1}{n}{\sum }_{i=1}^n\left(z_i\right)$	求平均公式：$n$ 个测量的和再除以 $n$
$=\frac{1}{n}\left({\sum }_{i=1}^{n-1}\left(z_i\right)+z_n\right)$	前 $n-1$ 个测量的和再加上最近一次的测量值整体除以 $n$
$=\frac{1}{n}{\sum }_{i=1}^{n-1}\left(z_i\right)+\frac{1}{n}{z}_n$	$\frac{1}{n}$ 乘进去展开
$=\frac{1}{n}\frac{n-1}{n-1}{\sum }_{i=1}^{n-1}\left(z_i\right)+\frac{1}{n}{z}_n$	给求和项同时乘以并除以 $n-1$
$=\frac{n-1}{n}\frac{1}{n-1}{\sum }_{i=1}^{n-1}\left(z_i\right)+\frac{1}{n}{z}_n$	调整顺序, 橘黄色的项就是上一时刻的估计值
$=\frac{n-1}{n}{\hat{x}}_{n-1,n-1}+\frac{1}{n}{z}_n$	把求和项用上一时刻的估计值替换掉
$={\hat{x}}_{n-1,n-1}-\frac{1}{n}{\hat{x}}_{n-1,n-1}+\frac{1}{n}{z}_n$	把 (\hat{x}_{n-1,n-1}) 乘进 $\frac{n-1}{n}$ 的分子，并拆项
$={\hat{x}}_{n-1,n-1}+\frac{1}{n}\left(z_n-{\hat{x}}_{n-1,n-1}\right)$	提出 $\frac{1}{n}$，${\hat{x}}_{n-1,n-1}$ 就是在 $n-1$ 时刻使用 $n-1$ 时刻的测量值对 $x$ 的状态估计。

接下来使用 $n-1$ 时刻的估计值 ${\hat{x}}_{n-1,n-1}$ 计算 ${\hat{x}}_{n,n-1}$ （对 $n$ 时刻 $x$ 的预测）。即把 ${\hat{x}}_{n-1,n-1}$ 外插至 $n$ 时刻。

由于系统模型是静态的，当前时刻对 $x$ 的预测就等于上一时刻对 $x$ 的估计：${\hat{x}}_{n,n-1}={\hat{x}}_{n-1,n-1}$.

基于上述推导，对当前时刻状态 ${\hat{x}}_{n,n}$ 的估计可以写成：

$${\hat{x}}_{n,n}={\hat{x}}_{n,n-1}+\frac{1}{n}\left(z_n-{\hat{x}}_{n,n-1}\right)$$
上式即为卡尔曼滤波的五个方程之一。称为状态更新方程。其意为：

系数 $\frac{1}{n}$ 是本例特定的。后面会具体谈到这个系数的重要性，但此刻可以先指出，在卡尔曼滤波的语境中，这个系数被称作卡尔曼增益，符号为 $K_n$. 其具有下标 $n$ 意味着卡尔曼增益随着每次迭代都会改变。

$K_n$ 的提出是Rudolf Kalman重要的贡献之一。

在进展到卡尔曼滤波之前，我们先不用 $K_n$，而是用希腊字母 ${\alpha }_n$ 来表示这个系数。

所以状态更新方程可以写作：

$${\hat{x}}_{n,n}={\hat{x}}_{n,n-1}+{\alpha }_n\left(z_n-{\hat{x}}_{n,n-1}\right)$$
$\left(z_n-{\hat{x}}_{n,n-1}\right)$ 这一项被称为“测量残差”，也叫更新量。更新量包含新的信息。

本例中，随着 $n$ 的增加，$\frac{1}{n}$ 会下降。在一开始，因为没有足够的信息，第一次估计完全是基于第一次的测量值的（$\frac{1}{n}{|}_{n=1}=1$）。随着迭代进行，每次后续测量的权重都在下降，并且会逐渐变得可以忽略不计。

继续讲示例。在进行第一次测量之前，我们可以根据金条上的钢印来猜测（或粗略估计）金条的重量，这是 初始估计，是算法的第一个估计值。

卡尔曼滤波需要一个初始估计作为初始值，这个值可以非常粗略。

估计算法

下图描述了本例中所使用的算法。

现在，可以开始具体测量和估计的过程了。

数值示例

0. 第0次迭代

初始化
金条重量的初始估计是1000g，这个估计仅在滤波器初始化时使用一次，后续迭代不再需要这个值。

$${\hat{x}}_{0,0}=1000g$$

预测
金条的重量不会改变，系统模型是静态的，下一个时刻的预测就等于此时的初始化估计值。

$${\hat{x}}_{1,0}={\hat{x}}_{0,0}=1000g$$

1. 第1次迭代

第1步
用秤称重：

$$z_1=996g$$

第2步
计算增益。本例中 ${\alpha }_n=\frac{1}{n}$，故：

$${\alpha }_1=\frac{1}{1}=1$$

用状态更新方程计算当前的估计值：

$${\hat{x}}_{1,1}={\hat{x}}_{1,0}+{\alpha }_1\left(z_1-{\hat{x}}_{1,0}\right)=1000+1\left(996-1000\right)=996g$$

注：初始估计可以是任何值，由于 ${\alpha }_1=1$，初始值在第1次迭代的时候将被抵消。
第3步
系统模型是静态的，金条的重量不应该改变，下一时刻的预测应该等于此时的估计：

$${\hat{x}}_{2,1}={\hat{x}}_{1,1}=996g$$

2. 第2次迭代

在一个采样周期过后，上一时刻的预测值成为了这一时刻的先验估计。

$${\hat{x}}_{2,1}=996g$$

第1步
做第二次称重：

$$z_2=994g$$

第2步
计算增益：

$${\alpha }_2=\frac{1}{2}$$

计算估计值：

$${\hat{x}}_{2,2}={\hat{x}}_{2,1}+{\alpha }_2\left(z_2-{\hat{x}}_{2,1}\right)=996+\frac{1}{2}\left(994-996\right)=995g$$

第3步
$${\hat{x}}_{3,2}={\hat{x}}_{2,2}=995g$$

3. 第3次迭代

$$z_3=1021g{\alpha }_3=\frac{1}{3}$$ $${\hat{x}}_{3,3}=995+\frac{1}{3}\left(1021-995\right)=1003.67g$$ $${\hat{x}}_{4,3}=1003.67g$$

4. 第4次迭代
   $$z_4=1000g{\alpha }_4=\frac{1}{4}$$ $${\hat{x}}_{4,4}=1003.67+\frac{1}{4}\left(1000-1003.67\right)=1002.75g$$ $${\hat{x}}_{5,4}=1002.75g$$

5. 第5次迭代
   $$z_5=1002g{\alpha }_5=\frac{1}{5}$$ $${\hat{x}}_{5,5}=1002.75+\frac{1}{5}\left(1002-1002.75\right)=1002.6g$$ $${\hat{x}}_{6,5}=1002.6g$$

6. 第6次迭代
   $$z_6=1010g{\alpha }_6=\frac{1}{6}$$ $${\hat{x}}_{6,6}=1002.6+\frac{1}{6}\left(1010-1002.6\right)=1003.83$$ $${\hat{x}}_{7,6}=1003.83g$$

7. 第7次迭代
   $$z_7=983g{\alpha }_7=\frac{1}{7}$$ $${\hat{x}}_{7,7}=1003.83+\frac{1}{7}\left(983-1003.83\right)=1000.86g$$ $${\hat{x}}_{8,7}=1000.86g$$

8. 第8次迭代
   $$z_8=971g{\alpha }_8=\frac{1}{8}$$ $${\hat{x}}_{8,8}=1000.86+\frac{1}{8}\left(971-1000.86\right)=997.125g$$ $${\hat{x}}_{9,8}=997.125g$$

9. 第9次迭代
   $$z_9=993g{\alpha }_9=\frac{1}{9}$$ $${\hat{x}}_{9,9}=997.125+\frac{1}{9}\left(993-997.125\right)=996.67g$$ $${\hat{x}}_{10,9}=996.67g$$

10. 第10次迭代
    $$z_{10}=1023g{\alpha }_{10}=\frac{1}{10}$$ $${\hat{x}}_{10,10}=996.67+\frac{1}{10}\left(1023-996.67\right)=999.3g$$ $${\hat{x}}_{11,10}=999.3g$$

至此先告一段落。增益随着每次测量而减小，故后面的测量对估计值的贡献总小于前面的测量。我们已经很接近真实的金条重量了（1000g）。如果做更多次的称重，我们会和真值更加接近。

下表汇总并比较了上面的测量值、估计值以及真值。

变量	数值
$n$	1 2 3 4 5 6 7 8 9 10
${\alpha }_n$	$1$ $\frac{1}{2}$ $\frac{1}{3}$ $\frac{1}{4}$ $\frac{1}{5}$ $\frac{1}{6}$ $\frac{1}{7}$ $\frac{1}{8}$ $\frac{1}{9}$ $\frac{1}{10}$
$z_n$	996 994 1021 1000 1002 1010 983 971 993 1023
${\hat{x}}_{n,n}$	996 995 1003.67 1002.75 1002.6 1003.83 1000.86 997.125 996.67 999.3
${\hat{x}}_{n+1,n}$	996 995 1003.67 1002.75 1002.6 1003.83 1000.86 997.125 996.67 999.3

结果分析

下图中列出了测量值、估计值以及真值。

示例小结

本例中，我们设计了一个针对静态系统的简单估计算法。我们还推导出了五个卡尔曼滤波方程之一的状态更新方程。下一章中我们会重新审视这个状态更新方程。

示例 2 - 跟踪直线匀速运动的飞行器

是时候考虑一下状态随时间变化的动态系统了。本例中，我们尝试用 $\alpha -\beta$ 滤波器 对一个直线匀速飞行中的飞行器进行跟踪。

假设一个只有一个维度的世界中，有这样一个飞行器在往远离雷达的方向飞行（或者靠近雷达）。因为是一维空间，飞行器到雷达的角度是恒定的，其高度也是恒定的。

$x_n$ 表示 $n$ 时刻飞行器的距离。飞行器速度可以近似用距离差分法得到 - 计算距离随时间的变化率。

因此，速度是距离的导数：

$$\dot{x}=v=\frac{dx}{dt}$$
雷达向目标的方向以固定频率发射跟踪波束，两次跟踪测量之间的时间间隔为 $\Delta t$.

则匀速运动的动力学模型可以由下面的运动方程给出：

$$x_{n+1}=x_n+\Delta t{\dot{x}}_n$$ $${\dot{x}}_{n+1}={\dot{x}}_n$$
根据这些方程，下一个采样周期时的飞行器距离等于当前飞行器距离加上目标速度乘以采样间隔时间。由于我们假设飞行器的速度不变，下一时刻的速度等于当前时刻的速度。

上述方程称为 状态外插方程 (也叫 转移方程 或 预测方程)。

在上个示例中我们已经用过状态外插方程了，只不过上个例子中这个方程是个恒等式，即下一时刻的状态等于当前时刻的状态。

状态外插方程依赖系统动态模型，因此不同的示例中这个方程也不同。

这个方程有一个以矩阵形式给出的更加一般的形式，后续会讲到。

上述方程的形式是本例特有的。

注：我们已经学到了两个卡尔曼滤波方程： 状态更新方程、 状态外插方程

现在我们来把状态更新方程改一改。

$\alpha -\beta$ 滤波器

假设雷达的跟踪间隔 ( $\Delta t$ ) 为5秒，假设 $n-1$ 时刻飞行器的距离为30,000m，其速度为40m/s.

使用上述状态外插方程，我们能够预测 $n$ 时刻的目标位置为：

$${\hat{x}}_{n,n-1}={\hat{x}}_{n-1,n-1}+\Delta t{\hat{\dot{x}}}_{n-1,n-1}=30000+5\times 40=30200m$$

预测目标 $n$ 时刻的速度为：

$${\hat{\dot{x}}}_{n,n-1}={\hat{\dot{x}}}_{n-1,n-1}=40m/s$$

然而 $n$ 时刻雷达测量的目标距离 ( $z_n$ ) 为30,110m而非30,200m。预测和实际测量的距离之间相差了90m. 这个差有可能是两个原因导致的：

1. 雷达测量不够精准
2. 飞行器速度变化了。新的速度是 $\frac{30,110-30,000}{5}=22m/s$
   哪个原因是正确的呢？

我们把速度的状态更新方程写下来：

$${\hat{\dot{x}}}_{n,n}={\hat{\dot{x}}}_{n,n-1}+\beta \left(\frac{z_n-{\hat{x}}_{n,n-1}}{\Delta t}\right)$$
系数 $\beta$ 的值和雷达的测量精度等级有关。假设雷达的 $1\sigma$ 精度是20m，那么90m的误差大概率是飞行器速度改变了，我们应该把 $\beta$ 的值调高一些。如果 $\beta$ 设为0.9，那么此时估计的速度就应该是：

$${\hat{\dot{x}}}_{n,n}={\hat{\dot{x}}}_{n,n-1}+\beta \left(\frac{z_n-{\hat{x}}_{n,n-1}}{\Delta t}\right)=40+0.9\left(\frac{30110-30200}{5}\right)=23.8m/s$$

而另一方面，假如雷达的 $1\sigma$ 精度是150m，那90m的误差大概率是雷达测得不准，我们应该把 $\beta$ 的值降低一些。如果 $\beta$ 的值降为0.1，那么估计的速度就应该是：

$${\hat{\dot{x}}}_{n,n}={\hat{\dot{x}}}_{n,n-1}+\beta \left(\frac{z_n-{\hat{x}}_{n,n-1}}{\Delta t}\right)=40+0.1\left(\frac{30110-30200}{5}\right)=38.2m/s$$

如果是飞行器真实速度从40m/s降到了22m/s，可以看到在10个测量周期之后（把上面的公式以 $\beta$ = 0.1 带入10次），飞行器速度的估计也逐渐降到了22m/s. 而如果是因为雷达测量不准，则后续测量的位置将会大致均匀散布在真实位置前后，整体上计算出来的平均速度会保持在40m/s左右不变。

飞行器位置的状态更新方程与上一个例子里的方程类似：

$${\hat{x}}_{n,n}={\hat{x}}_{n,n-1}+\alpha \left(z_n-{\hat{x}}_{n,n-1}\right)$$
和上例不同的是，上例里 $\alpha$ 系数每周期都在重新计算 ( ${\alpha }_n=\frac{1}{n}$ )，而本例中 $\alpha$ 则是恒定的。

$\alpha$ 系数的大小和雷达精度有关。对高精度雷达，应该选用高的 $\alpha$，以给测量值分配更高的权重。如果 $\alpha =1$，则估计的飞行器距离会等于测量值。

$${\hat{x}}_{n,n}={\hat{x}}_{n,n-1}+1\left(z_n-{\hat{x}}_{n,n-1}\right)=z_n$$

如果 $\alpha =0$，则测量值完全起不到任何作用：

$${\hat{x}}_{n,n}={\hat{x}}_{n,n-1}+0\left(z_n-{\hat{x}}_{n,n-1}\right)={\hat{x}}_{n,n-1}$$

于是我们推导出了雷达跟踪问题的状态更新方程。这又叫$\alpha -\beta$ 跟踪更新方程 或 $\alpha -\beta$ 跟踪滤波方程。

位置的状态更新方程为：
$${\hat{x}}_{n,n}={\hat{x}}_{n,n-1}+\alpha \left(z_n-{\hat{x}}_{n,n-1}\right)$$

速度的状态更新方程为：
$${\hat{\dot{x}}}_{n,n}={\hat{\dot{x}}}_{n,n-1}+\beta \left(\frac{z_n-{\hat{x}}_{n,n-1}}{\Delta t}\right)$$
注：在其他的书中，$\alpha -\beta$ 滤波器又叫 g-h 滤波器，字母g代替了希腊字母 $\alpha$，字母h代替了希腊字母 $\beta$.
注：本例中，我们是根据雷达距离测量来间接估计的飞行器速度的 ( $\dot{x}=\frac{\Delta x}{\Delta t}$ )，实际上现代雷达可以直接用多普勒效应测量径向速度。但目前我们的目的是解释卡尔曼滤波而非雷达工作原理，所以为了简单起见，我们的示例中将全部用距离观测来间接测量速度。

估计算法

下图描述了本例所使用的估计算法。

与上一个示例不同，增益（$\alpha$ 和 $\beta$）的值在本例里是给定的。在卡尔曼滤波里，$\alpha$ 和 $\beta$ 会被卡尔曼增益代替，并且每个采样周期会重新计算，后面会讲到。

现在来看一个数值示例。

数值示例

考虑一个在一维世界里正在向雷达靠近（或远离）的飞行器。

$\alpha -\beta$ 滤波器的参数为：

$\alpha =0.2$.
$\beta =0.1$.
测量周期是5秒。

注：本例中，为了便于绘图，我们假设雷达是个低精度雷达，追踪的飞行器是架低速飞行器。现实中的雷达精度一般都更高，飞行器也更快。

0. 第0次迭代

初始化
$n=0$ 时刻的初始条件给定为：

$${\hat{x}}_{0,0}=30000m$$ $${\hat{\dot{x}}}_{0,0}=40m/s$$

注：跟踪问题的初始化（或者说如何获得初始状态）是个十分重要的工作，后面会讲到。目前我们的目标是理解 $\alpha -\beta$ 滤波器的基本原理，所以我们假设初始值已经合理获取了。
预测
初始值需要外插至第一个测量周期：

$${\hat{x}}_{n+1,n}={\hat{x}}_{n,n}+\Delta t{\hat{\dot{x}}}_{n,n}\to {\hat{x}}_{1,0}={\hat{x}}_{0,0}+\Delta t{\hat{\dot{x}}}_{0,0}=30000+5\times 40=30200m$$ $${\hat{\dot{x}}}_{n+1,n}={\hat{\dot{x}}}_{n,n}\to {\hat{\dot{x}}}_{1,0}={\hat{\dot{x}}}_{0,0}=40m/s$$

1. 第1次迭代

第一个周期 ( $n=1$ )，初始值就是先验估计：

$${\hat{x}}_{n,n-1}={\hat{x}}_{1,0}=30200m$$ $${\hat{\dot{x}}}_{n,n-1}={\hat{\dot{x}}}_{1,0}=40m/s$$

第1步
雷达进行一次测距：

$$z_1=30171m$$

第2步
用状态更新方程计算当前的状态估计：

$${\hat{x}}_{1,1}={\hat{x}}_{1,0}+\alpha \left(z_1-{\hat{x}}_{1,0}\right)=30200+0.2\left(30171-30200\right)=30194.2m$$ $${\hat{\dot{x}}}_{1,1}={\hat{\dot{x}}}_{1,0}+\beta \left(\frac{z_1-{\hat{x}}_{1,0}}{\Delta t}\right)=40+0.1\left(\frac{30171-30200}{5}\right)=39.42m/s$$

第3步
用状态外插方程预测下一周期的状态：

$${\hat{x}}_{2,1}={\hat{x}}_{1,1}+\Delta t{\hat{\dot{x}}}_{1,1}=30194.2+5\times 39.42=30391.3m$$ $${\hat{\dot{x}}}_{2,1}={\hat{\dot{x}}}_{1,1}=39.42m/s$$

2. 第2次迭代

一个采样周期过后，状态预测变为先验估计：

$${\hat{x}}_{2,1}=30391.3m$$ $${\hat{\dot{x}}}_{2,1}=39.42m/s$$

第1步
雷达进行一次测距：

$$z_2=30353m$$

第2步
用状态更新方程计算当前的状态估计：

$${\hat{x}}_{2,2}={\hat{x}}_{2,1}+\alpha \left(z_2-{\hat{x}}_{2,1}\right)=30391.3+0.2\left(30353-30391.3\right)=30383.64m$$ $${\hat{\dot{x}}}_{2,2}={\hat{\dot{x}}}_{2,1}+\beta \left(\frac{z_2-{\hat{x}}_{2,1}}{\Delta t}\right)=39.42+0.1\left(\frac{30353-30391.3}{5}\right)=38.65m/s$$

第3步
用状态外插方程预测下一周期的状态：

$${\hat{x}}_{3,2}={\hat{x}}_{2,2}+\Delta t{\hat{\dot{x}}}_{2,2}=30383.64+5\times 38.65=30576.9m$$ $${\hat{\dot{x}}}_{3,2}={\hat{\dot{x}}}_{2,2}=38.65m/s$$

3. 第3次迭代

$$z_3=30756m$$ $${\hat{x}}_{3,3}=30576.9+0.2\left(30756-30576.9\right)=30612.73m$$ $${\hat{\dot{x}}}_{3,3}=38.65+0.1\left(\frac{30756-30576.9}{5}\right)=42.2m/s$$ $${\hat{x}}_{4,3}=30612.73+5\times 42.2=30823.9m$$ $${\hat{\dot{x}}}_{4,3}=42.2m/s$$

4. 第4次迭代

$$z_4=30799m$$ $${\hat{x}}_{4,4}=30823.9+0.2\left(30799-30823.9\right)=30818.93m$$ $${\hat{\dot{x}}}_{4,4}=42.2+0.1\left(\frac{30799-30823.9}{5}\right)=41.7m/s$$ $${\hat{x}}_{5,4}=30818.93+5\times 41.7=31027.6m$$ $${\hat{\dot{x}}}_{5,4}=41.7m/s$$

5. 第5次迭代

$$z_5=31018m$$ $${\hat{x}}_{5,5}=31027.6+0.2\left(31018-31027.6\right)=31025.7m$$ $${\hat{\dot{x}}}_{5,5}=41.7+0.1\left(\frac{31018-31027.6}{5}\right)=41.55m/s$$ $${\hat{x}}_{6,5}=31025.7+5\times 41.55=31233.4m$$ $${\hat{\dot{x}}}_{6,5}=41.55m/s$$

6. 第6次迭代

$$z_6=31278m$$ $${\hat{x}}_{6,6}=31233.4+0.2\left(31278-31233.4\right)=31242.3m$$ $${\hat{\dot{x}}}_{6,6}=41.55+0.1\left(\frac{31278-31233.4}{5}\right)=42.44m/s$$ $${\hat{x}}_{7,6}=31242.3+5\times 42.44=31454.5m$$ $${\hat{\dot{x}}}_{7,6}=42.44m/s$$

7. 第7次迭代

$$z_7=31276m$$ $${\hat{x}}_{7,7}=31454.5+0.2\left(31276-31454.5\right)=31418.8m$$ $${\hat{\dot{x}}}_{7,7}=42.44+0.1\left(\frac{31276-31454.5}{5}\right)=38.9m/s$$ $${\hat{x}}_{8,7}=31418.8+5\times 38.9=31613.15m$$ $${\hat{\dot{x}}}_{8,7}=38.9m/s$$

8. 第8次迭代

$$z_8=31379m$$ $${\hat{x}}_{8,8}=31613.15+0.2\left(31379-31613.15\right)=31566.3m$$ $${\hat{\dot{x}}}_{8,8}=38.9+0.1\left(\frac{31379-31613.15}{5}\right)=34.2m/s$$ $${\hat{x}}_{9,8}=31566.3+5\times 34.2=31737.24m$$ $${\hat{\dot{x}}}_{9,8}=34.2m/s$$

8. 第9次迭代

$$z_9=31748m$$ $${\hat{x}}_{9,9}=31737.24+0.2\left(31748-31737.24\right)=31739.4m$$ $${\hat{\dot{x}}}_{9,9}=34.2+0.1\left(\frac{31748-31737.24}{5}\right)=34.4m/s$$ $${\hat{x}}_{10,9}=31739.4+5\times 34.4=31911.4m$$ $${\hat{\dot{x}}}_{10,9}=34.4m/s$$

10. 第10次迭代

$$z_{10}=32175m$$ $${\hat{x}}_{10,10}=31911.4+0.2\left(32175-31911.4\right)=31964.1m$$ $${\hat{\dot{x}}}_{10,10}=34.4+0.1\left(\frac{32175-31911.4}{5}\right)=39.67m/s$$ $${\hat{x}}_{11,10}=31964.1+5\times 39.67=32162.45m$$ $${\hat{\dot{x}}}_{11,10}=39.67m/s$$

下表汇总了测量值和估计值。

变量	数值
$n$	1 2 3 4 5 6 7 8 9 10
$z_n$	30171 30353 30756 30799 31018 31278 31276 31379 31748 32175
${\hat{x}}_{n,n}$	30194.2 30383.64 30612.73 30818.93 31025.7 31242.3 31418.8 31566.3 31739.4 31964.1
${\dot{\hat{x}}}_{n,n}$	39.42 38.65 42.2 41.7 41.55 42.44 38.9 34.2 34.4 39.67
${\hat{x}}_{n+1,n}$	30391.3 30576.9 30823.9 31027.6 31233.4 31454.5 31613.15 31737.24 31911.4 32162.45
${\dot{\hat{x}}}_{n+1,n}$	39.42 38.65 42.2 41.7 41.55 42.44 38.9 34.2 34.4 39.67

结果分析

下图中列出了测量值、估计值以及真值。

我们的估计算法对测量值有平滑效果，并且能收敛到真值。

用较大的 $\alpha$ 和 $\beta$
下图描述了 $\alpha =0.8$、$\beta =0.5$ 时的真值、测量值和估计值。

滤波器的平滑效果降低了很多。当前估计值非常接近测量值，预测的误差也相对较高。

那么我们应该一直选取较低的 $\alpha$ 和 $\beta$ 吗？

答案是否定的。$\alpha$ 和 $\beta$ 的值应该依测量精度而定。如果测量设备精度很高，例如激光雷达，较高的 $\alpha$ 和 $\beta$ 会比较好，会更信赖测量值，对于本例则滤波器对飞行器速度的变化响应更快；如果测量设备精度很低，应该用较低的 $\alpha$ 和 $\beta$，会更信赖预测值，对于本例则滤波器对测量不确定性（误差）的平滑效果会更好，但滤波器对飞行器速度的变化响应就会慢得多。

示例小结

我们推导了 $\alpha -\beta$ 滤波器的状态更新方程，并且还学到了状态外插方程。我们用 $\alpha -\beta$ 滤波器设计了一个一维动态系统估计算法，并用它数值求解了一个匀速运动目标的状态估计问题。