一维卡尔曼滤波

本章，我们推导一维情况下的卡尔曼滤波。本章的主要目标是在不引入复杂繁琐的数学工具的前提下，直观解释卡尔曼滤波的核心思想。

我们将一步步逐渐推导出卡尔曼滤波的方程。

本章我们先推导无过程噪声的卡尔曼滤波方程。在下一章，会将过程噪声考虑进去。

一维无过程噪声的卡尔曼滤波

如之前所言，卡尔曼滤波基于五个方程，我们已经见过了其中的两个：

状态更新方程
动态模型方程

本章我们推导其余三个卡尔曼滤波方程，并重新审视状态更新方程。

类似 \( \alpha -\beta -(\gamma) \) 滤波器，卡尔曼滤波也使用“测量，更新，预测”三个步骤。

与 \( \alpha -\beta -(\gamma) \) 不同，卡尔曼滤波将测量值、当前状态估计和下一个状态预测均视为具有正态分布的随机变量，这些随机变量由均值和方差进行描述。

下表给出了卡尔曼滤波的底层结构描述：

获取本书

Schematic description of the Kalman Filter algorithm

回忆第一个示例（金条称重），我们对金条进行了多次称重，并取平均值作为对重量的估计。

我们得到了下面的结果：

Measurements vs. True value vs. Estimates

上图描述了真值、测量值和估计值随测量次数的变化情况。

对随机变量进行估计

估计值（红线）和真值（绿线）之间的差就是估计误差。可见，随着测量次数的增加，估计误差不断下降，并最终收敛到0，同时估计值收敛到真值。实际中我们无法知道估计误差，但是我们可以计算估计值的不确定性。

记状态估计值的方差为 \( p \).

对随机变量进行测量

测量误差是测量值（蓝线）和真值（绿线）之间的差。由于测量误差是随机的，我们可以用方差 \( \sigma ^{2} \) 来描述。对应的标准差 \( \left( \sigma \right) \) 就是测量不确定性。

注：在其他文献中，测量不确定性也叫测量误差。

记测量值的方差为 \( r \).

测量误差的方差可以从测量设备厂商获取、根据数据计算、理论推导得出或通过校准过程得到。

比如使用秤的时候，我们可以对一个重量已知的物体反复多次测量来对秤进行校准，并实地计算出测量值的标准差。秤的制造商也能够给出其测量不确定性的度量。

对更高级的传感器，例如雷达，测量不确定性受很多因素影响，例如SNR（信噪比）、波束宽度、带宽、照射时长、时钟稳定性等等。每次雷达测量都有不同的信噪比、波束宽度和照射时长，因此雷达会自行计算每次测量的不确定性，并将其报告给用户。

现在观察称重测量的概率密度函数（PDF）。

下图展示了对金条重量的十次测量。

蓝色圆圈代表测量值。
红色虚线代表真值。
绿线代表测量的概率密度函数。
宽绿色区域是对应测量的一倍标准差 \( \sigma \) 区间，即有68.26%的概率测量值会落在这个区域内。

可以看到，10次测量中有7次落在了 \( 1 \sigma \) 区间内。

Measurements Probability Density Function

状态预测

在第一个示例里，即金条称重，系统动态模型是恒定的：

\[ \hat{x}_{n+1,n}= \hat{x}_{n,n} \]

在第二个示例里，即一维雷达追踪，我们用如下的运动方程将当前状态（目标位置和速度）外插至下一个状态：

\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \] \[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \]

即预测位置等于当前位置估计加上当前速度估计乘以时间间隔。预测的速度等于当前速度估计（模型假设速度始终不变）。

动态模型依系统不同而不同。

由于卡尔曼滤波将状态的估计视为一个随机变量，除过将状态估计本身外插外，我们还必须将其方差也外插。

在第一个示例里（金条称重），系统动态模型是常量，故状态估计的不确定性可以非常简单地外插：

\[ \hat{p}_{n+1,n}= \hat{p}_{n,n} \]

式中：

\( p \) 是金条重量估计值的方差。

在第二个示例里，状态估计的不确定性可以如下外插：

\[ p_{n+1,n}^{x}= p_{n,n}^{x} + \Delta t^{2} \cdot p_{n,n}^{v} \] \[ p_{n+1,n}^{v}= p_{n,n}^{v} \]

式中：

\( p^{x} \)	是位置估计的方差
\( p^{v} \)	是速度估计的方差

即位置预测的方差等于当前位置估计的方差加上当前速度估计的方差乘以时间间隔的平方。预测速度的方差等于当前速度的方差（模型假设速度始终不变）。

注意，对任何服从方差为 \( \sigma^{2} \) 的正态分布的随机变量 \( x \)，\( kx \) 服从方差为 \( k^{2}\sigma^{2} \) 的正态分布，因此位置估计的方差更新时速度估计的方差所乘的项为时间间隔的平方。关于这里更细节的解释可以在附录 - 方差期望的推导中找到。

上述状态估计的方差的外插方程称为协方差外插方程，是第三个卡尔曼滤波方程。为什么改叫协方差了？在多变量卡尔曼滤波的章节会进行解释。

状态更新

我们使用了两个随机变量来估计系统当前的状态：

状态预测
测量值

卡尔曼滤波器是一种最优滤波器，最优性体现在它能够把上述状态预测和测量值融合在一起，使得得到的当前状态估计不确定性最小。

当前状态估计值是状态预测和测量值的加权和：

\[ \hat{x}_{n,n} = w_{1}z_{n} + w_{2}\hat{x}_{n,n-1} \] \[ w_{1} + w_{2} = 1 \]

式中 \( w_{1} \) 和 \( w_{2} \) 分别是测量值和状态预测的权重。

\( \hat{x}_{n,n} \) 可以写作：

\[ \hat{x}_{n,n} = w_{1}z_{n} + (1 - w_{1})\hat{x}_{n,n-1} \]

对应地，方差如下给出：

\[ p_{n,n} = w_{1}^{2}r_{n} + (1 - w_{1})^{2}p_{n,n-1} \]

式中：

\( p_{n,n} \) 是最优估计 \( \hat{x}_{n,n} \) 的方差。

\( p_{n,n-1} \) 是状态预测 \( \hat{x}_{n,n-1} \) 的方差。

\( r_{n} \) 是测量值 \( z_{n} \) 的方差。

回忆前面提到的，任何服从方差为 \( \sigma^{2} \) 的正态分布的随机变量 \( x \)，\( kx \) 服从方差为 \( k^{2}\sigma^{2} \) 的正态分布。关于这里更细节的解释可以在附录 - 方差期望的推导中找到。

我们试图找到一个最优估计使其能最小化 \( p_{n,n} \). 为了找到能最小化 \( p_{n,n} \) 的 \( w_{1} \)，我们把 \( p_{n,n} \) 对 \( w_{1} \) 求导，并令导数为零。

\[ \frac{dp_{n,n}}{dw_{1}} = 2w_{1}r_{n} - 2(1 - w_{1})p_{n,n-1} = 0 \]

得到

\[ w_{1}r_{n} = p_{n,n-1} - w_{1}p_{n,n-1} \]

\[ w_{1}p_{n,n-1} + w_{1}r_{n} = p_{n,n-1} \]

\[ w_{1} = \frac{p_{n,n-1}}{p_{n,n-1} + r_{n}} \]

把上述结果代入当前状态估计 \( \hat{x}_{n,n} \) 的表达式：

\[ \hat{x}_{n,n} = w_{1}z_{n} + (1 - w_{1})\hat{x}_{n,n-1} = w_{1}z_{n} + \hat{x}_{n,n-1} - w_{1}\hat{x}_{n,n-1} = \hat{x}_{n,n-1} + w_{1}\left( z_{n} - \hat{x}_{n,n-1} \right) \]

\[ \hat{x}_{n,n} = \hat{x}_{n,n-1} + \frac{p_{n,n-1}}{p_{n,n-1} + r_{n}}\left( z_{n} - \hat{x}_{n,n-1} \right) \]

我们就推导出了一个形式上和 \( \alpha -\beta -(\gamma) \) 滤波器的状态更新方程十分接近的新方程。前文称为“更新量”的项对应的权重称为卡尔曼增益（记为 \( K_{n} \) ）。

卡尔曼增益方程是第四个卡尔曼滤波方程。在一维情况下，卡尔曼增益方程具有如下的形式：

\[ K_{n}= \frac{预测值的方差}{预测值的方差+测量值的方差}= \frac{p_{n,n-1}}{p_{n,n-1}+r_{n}} \]

式中：

\( p_{n,n-1} \)	是状态预测的方差
\( r_{n} \)	是测量值的方差

卡尔曼增益是一个0到1之间的数：

\[ 0 \leq K_{n} \leq 1 \]

最后，我们还需要得到当前最优状态估计的方差。这在前文已经进行过推导：

\[ p_{n,n} = w^{2}_{1}r_{n} + \left( 1 - w_{1} \right)^{2}p_{n,n-1} \]

权重 \( w_{1} \) 就是卡尔曼增益：

\[ p_{n,n} = K^{2}_{n}r_{n} + \left( 1 - K_{n} \right)^{2}p_{n,n-1} \]

把 \( \left( 1 - K_{n} \right) \) 这一项展开：

\[ \left( 1 - K_{n} \right) = \left( 1 - \frac{p_{n,n-1}}{p_{n,n-1} + r_{n}} \right) = \left( \frac{p_{n,n-1} + r_{n} - p_{n,n-1}}{p_{n,n-1} + r_{n}} \right) = \left( \frac{r_{n}}{p_{n,n-1} + r_{n}} \right) \]

	注解
\( p_{n,n} = \left( \frac{p_{n,n-1}}{p_{n,n-1} + r_{n}} \right)^{2}r_{n} + \left( \frac{r_{n}}{p_{n,n-1} + r_{n}} \right)^{2}p_{n,n-1} \)	把展开后的 \( K_{n} \) 和 \( \left( 1 - K_{n} \right) \) 代入
\( p_{n,n} = \frac{p_{n,n-1}^{2}r_{n}}{\left(p_{n,n-1} + r_{n}\right)^{2}} + \frac{r_{n}^{2}p_{n,n-1}}{\left(p_{n,n-1} + r_{n}\right)^{2}} \)	展开平方
\( p_{n,n} = \frac{p_{n,n-1}r_{n}}{p_{n,n-1} + r_{n}} \left( \frac{p_{n,n-1}}{p_{n,n-1} + r_{n}} + \frac{r_{n}}{p_{n,n-1} + r_{n}} \right) \)	提出共有项
\( p_{n,n} = \left( 1 - K_{n} \right)p_{n,n-1}\left( K_{n} + \left( 1 - K_{n} \right) \right) \)	再代入 \( K_{n} \) 和 \( \left( 1 - K_{n} \right) \)
\( p_{n,n} = \left( 1 - K_{n} \right)p_{n,n-1} \)	\( K_{n} \) 抵消

\[ p_{n,n} = \left( 1 - K_{n} \right)p_{n,n-1} \]

上述方程更新了当前状态估计的方差。称为协方差更新方程。

从方程中就能看出，因为有 \( \left( 1-K_{n} \right) \leq 1 \)，状态估计的不确定性是始终在随着滤波器迭代而下降的。当测量不确定性很高的时候，卡尔曼增益很低，因此状态估计不确定性收敛的速度会较慢。相反当测量不确定性很低的时候卡尔曼增益则很高，故状态估计不确定性会快速收敛到0.

所以需要我们来确定到底需要测量和迭代多少次来获得一个足够确定的估计值。如果我们想测量大楼的高度，并且希望达到一倍 \( \sigma \) 为3cm的精度，我们需要反复进行测量，直到状态估计的方差 \( \sigma ^{2} \) 低于 \( 9~cm^{2} \).

汇总到一起

本节把前面提到的所有内容拼接成一个完整的算法。

滤波器输入为：

初始化

初始化进程只进行一次，负责提供两个参数：

初始状态 ( \( \hat{x}_{0,0} \) )
初始状态方差 ( \( p_{0,0} \) )

初始化参数可以由其他的系统、过程（例如雷达的搜索模式）或基于经验和理论知识所得出的合理的猜测来获得。即使初始化参数不太准确，卡尔曼滤波器也能收敛到接近真值。

测量

每个滤波器采样周期都要进行测量。每次测量得到两个参数：

测量值 ( \( z_{n} \) )
测量方差 ( \( r_{n} \) )

滤波器输出为：

状态估计 ( \( \hat{x}_{n,n} \) )
状态估计方差 ( \( p_{n,n} \) )

下表总结了五个卡尔曼滤波方程：

	方程	方程名	文献中可能出现的其他名字
状态更新阶段	\( K_{n}= \frac{p_{n,n-1}}{p_{n,n-1}+r_{n}} \)	卡尔曼增益 Kalman Gain	权重方程 Weight Equation
	\( \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ K_{n} \left( z_{n}- \hat{x}_{n,n-1} \right) \)	状态更新 State Update	滤波方程 Filtering Equation
	\( p_{n,n}=~ \left( 1-K_{n} \right) p_{n,n-1} \)	协方差更新 Covariance Update	修正方程 Corrector Equation
状态预测阶段	\( \hat{x}_{n+1,n}= \hat{x}_{n,n} \) (恒定动态模型) \( \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \) \( \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \) (速度恒定动态模型)	状态外插 State Extrapolation	预测器方程 Predictor Equation 转移方程 Transition Equation 预测方程 Prediction Equation 动态模型 Dynamic Model 状态空间模型 State Space Model
状态预测阶段	\( p_{n+1,n}= p_{n,n} \) (恒定动态模型) \( p_{n+1,n}^{x}= p_{n,n}^{x} + \Delta t^{2} \cdot p_{n,n}^{v} \) \( p_{n+1,n}^{v}= p_{n,n}^{v} \) (速度恒定动态模型)	协方差外插 Covariance Extrapolation	预测器协方差方程 Predictor Covariance Equation

注1：上述方程没有考虑过程噪声。下一章我们将加入过程噪声。

注2：状态外插方程和协方差外插方程依赖具体系统的动力学模型。

注3：上表所描述的卡尔曼滤波方程是针对特定问题的特定形式。其一般形式在后续会以矩阵形式给出。目前我们意在理解卡尔曼滤波的基本原理和概念。

下图给出了卡尔曼滤波的详细框图描述。

Detailed description of the Kalman Filter algorithm

第0步：初始化

如上所述，初始化仅执行一次，提供两个参数：

初始状态 ( \( \hat{x}_{0,0} \) )
初始状态方差 ( \( p_{0,0} \) )

初始化后进行预测。

第1步：测量

测量过程也提供两个参数：

测量值 ( \( z_{n} \) )
测量方差 ( \( r_{n} \) )

第2步：状态更新

状态更新过程给出当前系统状态的估计。

状态更新过程的输入为：

测量值 ( \( z_{n} \) )
测量方差 ( \( r_{n} \) )
状态预测 ( \( \hat{x}_{n,n-1} \) )
状态预测方差 ( \( p_{n,n-1} \) )

基于这些输入，状态更新过程计算卡尔曼增益并产生输出：

状态估计 ( \( \hat{x}_{n,n} \) )
状态估计方差 ( \( p_{n,n} \) )

这些是卡尔曼滤波器的输出。

第3步：预测

预测过程使用系统动态模型将当前系统的状态估计及其方差外插到下一个时刻。

第一次迭代时，初始化参数被当作状态预测的值和方差。

预测的输出被当作下一个时刻的状态预测的值和方差。

卡尔曼增益理解

把状态更新方程等效变换一下：

\[ \hat{x}_{n,n} = \hat{x}_{n,n-1} + K_{n}\left( z_{n} - \hat{x}_{n,n-1} \right) = \left( 1 - K_{n} \right)\hat{x}_{n,n-1} + K_{n}z_{n} \]

可见，卡尔曼增益 ( \( K_{n} \) ) 是测量的权重，\( \left( 1 - K_{n} \right) \) 这一项则为当前状态预测的权重。

卡尔曼增益在测量不确定性很高而预测不确定性很低的时候接近0，故当前状态预测的权重很高，测量权重则很低。

相反地，卡尔曼增益在测量不确定性很低而预测不确定性很高的时候接近1，故当前状态预测的权重很低，测量权重则很高。

如果测量和预测的不确定性相同，则卡尔曼增益等于0.5.

在估计新的状态的时候，卡尔曼增益定义了测量和状态预测的权重，表明有多少新的测量值应该进入最后的估计值。

高卡尔曼增益

测量不确定性相对预测不确定性很低的时候卡尔曼增益很高（接近1），故新的估计会很接近测量值。下图描述了高卡尔曼增益在飞行器跟踪问题中的效果。

低卡尔曼增益

测量不确定性相对预测不确定性很高的时候卡尔曼增益很低（接近0），故新的估计会很接近预测值。下图描述了低卡尔曼增益在飞行器跟踪问题中的效果。

至此我们已经了解了卡尔曼滤波的算法，并且可以开始看第一个数值示例了。

示例 5 - 估计大楼高度

假设我们想用一个不准的高度计测量大楼的高度。

我们知道大楼高度不随时间推移而改变，至少在短时间内不变。

获取本书

数值示例

大楼高度的真值是50米。
高度计误差（标准差）是5米。
十次测量的值分别是：49.03m, 48.44m, 55.21m, 49.98m, 50.6m, 52.61m, 45.87m, 42.64m, 48.26m, 55.84m.

第0次迭代

初始化

大楼高度的初始估计可以简单目测一下。

高度的初始估计假设是：

\[ \hat{x}_{0,0}=60m \]

随后我们可以初始化估计值的方差了。人眼的估计误差（标准差）大约是15米：\( \sigma =15 \)，故方差是255：\( \sigma ^{2}=225 \).

\[ p_{0,0} = 225 \]

预测

现在我们基于初始给出的两个值来预测下一次测量的状态。

由于系统模型恒定，大楼高度不会变化：

\[ \hat{x}_{1,0}=\hat{x}_{0,0}= 60m \]

状态预测方差的外插也不变：

\[ p_{1,0}= p_{0,0}=225 \]

第一次迭代

第1步 - 测量

第1次测量的结果是：\( z_{1}=49.03m \)

由于高度计的标准差是5，方差就是25，故测量方差是 \( r_{1}=25 \).

第2步 - 更新

计算卡尔曼增益：

\[ K_{1}= \frac{p_{1,0}}{p_{1,0}+r_{1}}= \frac{225}{225+25}=0.9 \]

估计当前状态：

\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ K_{1} \left( z_{1}- \hat{x}_{1,0} \right) =60+0.9 \left( 49.03-60 \right) =50.13m \]

更新状态估计方差：

\[ p_{1,1}=~ \left( 1-K_{1} \right) p_{1,0}= \left( 1-0.9 \right) 225=22.5 \]

第3步 - 预测

由于系统模型恒定，大楼高度不变：

\[ \hat{x}_{2,1}=\hat{x}_{1,1}= 50.13m \]

状态预测方差的外插依旧不变：

\[ p_{2,1}= p_{1,1}=22.5 \]

第2次迭代

一个单位时延后，上一时刻的状态预测成为了当前时刻的先验估计：

\[ \hat{x}_{2,1}=50.13m \]

上一时刻状态预测的方差称为了当前时刻的先验估计方差：

\[ p_{2,1}= 22.5 \]

第1步 - 测量

第2次测量的结果是：\( z_{2} = 48.44m \)

测量方差是：\( r_{2}=25 \)

第2步 - 更新

计算卡尔曼增益：

\[ K_{2}= \frac{p_{2,1}}{p_{2,1}+r_{2}}= \frac{22.5}{22.5+25}=0.47 \]

估计当前状态：

\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ K_{2} \left( z_{2}- x_{2,1} \right) =50.13+0.47 \left( 48.44-50.13 \right) =49.33m \]

更新状态估计方差：

\[ p_{2,2}=~ \left( 1-K_{2} \right) p_{2,1}= \left( 1-0.47 \right) 22.5=11.84 \]

第3步 - 预测

由于系统模型恒定，大楼高度不变：

\[ \hat{x}_{3,2}=\hat{x}_{2,2}= 49.33m \]

状态预测方差的外插依旧不变：

\[ p_{3,2}= p_{2,2}=11.84 \]

第3-10次迭代

后续迭代的计算结果汇总在下面的表里：

\( n \)	\( z_{n} \)	当前状态估计 ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) )	预测 ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) )
3	\( 55.21m \)	\[ K_{3}= \frac{11.84}{11.84+25}=0.32 \] \[ \hat{x}_{3,3}=~ 49.33+0.32 \left( 55.21 -49.33 \right) =51.22m \] \[ p_{3,3}= \left( 1-0.32 \right) 11.84=8.04 \]	\[ \hat{x}_{4,3}= \hat{x}_{3,3}=51.22m \] \[ p_{4,3}= p_{3,3}=8.04 \]
4	\( 49.98m \)	\[ K_{4}= \frac{8.04}{8.04+25}=0.24 \] \[ \hat{x}_{4,4}= 51.22+0.24 \left( 49.98 -51.22 \right) =50.92m \] \[ p_{4,4}= \left( 1-0.24 \right) 8.04=6.08 \]	\[ \hat{x}_{5,4}= \hat{x}_{4,4}=50.92m \] \[ p_{5,4}= p_{4,4}=6.08 \]
5	\( 50.6m \)	\[ K_{5}= \frac{6.08}{6.08+25}=0.2 \] \[ \hat{x}_{5,5}= 50.92+0.2 \left( 50.6 -50.92 \right) =50.855m \] \[ p_{5,5}= \left( 1-0.2 \right) 6.08=4.89 \]	\[ \hat{x}_{6,5}= \hat{x}_{5,5}=50.855m \] \[ p_{6,5}= p_{5,5}=4.89 \]
6	\( 52.61m \)	\[ K_{6}= \frac{4.89}{4.89+25}=0.16 \] \[ \hat{x}_{6,6}=~ 50.855+0.16 \left( 52.61 -50.855 \right) =51.14m \] \[ p_{6,6}= \left( 1-0.16 \right) 4.89=4.09 \]	\[ \hat{x}_{7,6}= \hat{x}_{6,6}=51.14m \] \[ p_{7,6}= p_{6,6}=4.09 \]
7	\( 45.87m \)	\[ K_{7}= \frac{4.09}{4.09+25}=0.14 \] \[ \hat{x}_{7,7}= 51.14+0.14 \left( 45.87 -51.14 \right) =50.4m \] \[ p_{7,7}= \left( 1-0.14 \right) 4.09=3.52 \]	\[ \hat{x}_{8,7}= \hat{x}_{7,7}=50.4m \] \[ p_{8,7}= p_{7,7}=3.52 \]
8	\( 42.64m \)	\[ K_{8}= \frac{3.52}{3.52+25}=0.12 \] \[ \hat{x}_{8,8}= 50.4+0.12 \left( 42.64 -50.4 \right) =49.44m \] \[ p_{8,8}= \left( 1-0.12 \right) 3.52=3.08 \]	\[ \hat{x}_{9,8}= \hat{x}_{8,8}=49.44m \] \[ p_{9,8}= p_{8,8}=3.08 \]
9	\( 48.26m \)	\[ K_{9}= \frac{3.08}{3.08+25}=0.11 \] \[ \hat{x}_{9,9}= 49.44 + 0.11 \left( 48.26 - 49.44 \right) = 49.31m \] \[ p_{9,9}= \left( 1-0.11 \right) 3.08=2.74 \]	\[ \hat{x}_{10,9}= \hat{x}_{9,9} = 49.31m \] \[ p_{10,9}= p_{9,9}=2.74 \]
10	\( 55.84m \)	\[ K_{10}= \frac{2.74}{2.74+25}=0.1 \] \[ \hat{x}_{10,10}= 49.31 + 0.1 \left( 55.84 - 49.31 \right) = 49.96m \] \[ p_{10,10}= \left( 1-0.1 \right) 2.74=2.47 \]	\[ \hat{x}_{11,10}= \hat{x}_{10,10} = 49.96m \] \[ p_{11,10}= p_{10,10} = 2.47 \]