【卡尔曼滤波第二期】一维无过程噪声的卡尔曼滤波

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

类似 α−β−(γ) 滤波器，卡尔曼滤波也使用 “测量，更新，预测” 三个步骤。α−β−γ 滤波器概述可参见另一博客-【卡尔曼滤波第一期】α−β−γ 滤波器概述。
与 α−β−(γ) 不同，卡尔曼滤波将测量值、当前状态估计和下一个状态预测均视为 具有正态分布的随机变量，这些随机变量由均值和方差进行描述。

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

前言

在另一博客-【卡尔曼滤波第一期】α−β−γ 滤波器概述中有一个多次称重黄金重量的案例，结果如下图（描述了真值-绿色线、测量值-蓝色线和估计值-红色线随测量次数的变化情况）。

对随机变量进行估计

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

对随机变量进行测量
测量误差是测量值（蓝线）和真值（绿线）之间的差。由于测量误差是随机的，我们可以用方差 σ2 来描述。对应的标准差 (σ) 就是测量不确定性。

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

记测量值的方差为 r。

测量误差的方差可以从测量设备厂商获取、根据数据计算、理论推导得出或通过校准过程得到。比如使用秤的时候，我们可以对一个重量已知的物体反复多次测量来对秤进行校准，并实地计算出测量值的标准差。秤的制造商也能够给出其测量不确定性的度量。

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

现在观察称重测量的 概率密度函数（PDF）。下图展示了对金条重量的十次测量。

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

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

状态预测

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

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

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

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

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

在第二个示例里，状态估计的不确定性可以如下外插：即位置预测的方差等于当前位置估计的方差加上当前速度估计的方差乘以时间间隔的平方。预测速度的方差等于当前速度的方差（模型假设速度始终不变）。

注意，对任何服从方差为 σ2 的正态分布的随机变量 x，kx 服从方差为 k2σ2 的正态分布，因此位置估计的方差更新时速度估计的方差所乘的项为时间间隔的平方。

上述状态估计的方差的外插方程称为 协方差外插方程，是第三个卡尔曼滤波方程。

状态更新

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

• 状态预测
• 测量值

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

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

说明：任何服从方差为 σ2 的正态分布的随机变量 x，kx 服从方差为 k2σ2 的正态分布。

我们就推导出了一个形式上和 α−β−(γ) 滤波器的状态更新方程十分接近的新方程。前文称为“更新量”的项对应的权重称为 卡尔曼增益 （记为 Kn）。

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

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

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

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

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

小节

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

滤波器输入为：

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

• 初始状态 ( x^0,0 )
• 初始状态方差 ( p0,0 )

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

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

• 测量值 ( zn )
• 测量方差 ( rn )

---

滤波器输出为：

• 状态估计 ( x^n,n )
• 状态估计方差 ( pn,n )

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

注1：上述方程没有考虑过程噪声。
注2：状态外插方程和协方差外插方程依赖具体系统的动力学模型。
注3：上表所描述的卡尔曼滤波方程是针对特定问题的特定形式。其一般形式在后续会以矩阵形式给出。目前我们意在理解卡尔曼滤波的基本原理和概念。

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

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

• 初始状态 ( x^0,0 )
• 初始状态方差 ( p0,0 )

初始化后进行预测。

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

• 测量值 ( zn )
• 测量方差 ( rn )

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

状态更新过程的输入为：

• 测量值 ( zn )
• 测量方差 ( rn )
• 状态预测 ( x^n,n−1 )
• 状态预测方差 ( pn,n−1 )

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

• 状态估计 ( x^n,n )
• 状态估计方差 ( pn,n )

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

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

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

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

卡尔曼增益理解

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

可见，卡尔曼增益 ( Kn ) 是测量的权重，(1−Kn) 这一项则为当前状态预测的权重。

• 卡尔曼增益在测量不确定性很高而预测不确定性很低的时候接近0，故当前状态预测的权重很高，测量权重则很低。
• 相反地，卡尔曼增益在测量不确定性很低而预测不确定性很高的时候接近1，故当前状态预测的权重很低，测量权重则很高。
• 如果测量和预测的不确定性相同，则卡尔曼增益等于0.5.

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

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

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

案例：估计大楼高度

假设我们想用一个不准的高度计测量大楼的高度。我们知道大楼高度不随时间推移而改变，至少在短时间内不变。

数值示例

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

结果分析
首先，我们需要确认卡尔曼滤波器收敛了。卡尔曼增益应该逐渐降低直到达到一个稳态。当卡尔曼增益很低时，充满噪声的测量值的权重同样很低。下图描述了卡尔曼增益在上述卡尔曼滤波器前100次迭代的变化情况。

能看到在前10次迭代中卡尔曼增益显著下降，并在大约50次迭代后进入稳态状态。

我们还想确认准度。准度描述了测量值与真值的接近情况。下表比较了前50次迭代的真值、测量值和估计值。

估计误差是真值（绿线）和卡尔曼滤波器估计值（红线）之间的差。可见我们的卡尔曼滤波器的估计误差在滤波器收敛的过程中同步收敛。

根据具体的应用场景需求可以定义准度判据。典型的准度判据有：

• 最大误差
• 平均误差
• 均方误差（RMSE）

另外一个重要的指标是 估计的不确定性。我们希望卡尔曼滤波的估计能尽量精确，因此，我们希望得到较低的估计不确定性。

假设对测量大楼高度这样一个场景，要求得到95%置信的结果。下表给出了上述卡尔曼滤波估计值加上95%置信区间后和真值的对比。

置信区间基于估计值的不确定性来绘制。上图中，置信区间叠加在了估计值上（红线）。95%的绿色样本点应该都落在95%置信区间内。

我们能看到，不确定性太高了（置信区间过于宽大），需要把测量不确定性降低一些。

下图描述了较低测量不确定性情况下的卡尔曼滤波输出：

尽管通过调低测量不确定性，我们成功降低了估计的不确定性，但许多绿色样本落在了95%的置信区间外。说明卡尔曼滤波过于相信测量值，对其准度过于乐观了。

上图中，50个采样点里仅有2个位于95%置信区间外，此时的滤波器性能满足需求。

参考