卡尔曼滤波器从入门到放弃

最新推荐文章于 2025-01-06 16:02:36 发布

avenger_fang

最新推荐文章于 2025-01-06 16:02:36 发布

阅读量2.8k

点赞数 5

分类专栏： SLAM 文章标签：算法

原文链接：https://www.kalmanfilter.net/kalman1d.html

版权

SLAM 专栏收录该内容

13 篇文章

订阅专栏

EXAMPLE 5 – ESTIMATING THE HEIGHT OF A BUILDING

数值例子：

【一维卡尔曼滤波器的完整模型-带过程噪声】

LINEAR TIME-INVARIANT SYSTEMS

前言

本文翻译自以色列工程师Alex Becker的博客Kalman Filter Tutorial

卡尔曼滤波器是一个简单的话题。但是，许多书籍和教程并不容易理解。大多数都需要广泛的数学背景，并且缺乏实际的数值示例，本文的目的就是通过数值实例来推导和理解卡尔曼滤波器。并教你怎么设置对应的参数，为什么如此设置。

【个人总结】

【总结卡尔曼滤波器使用流程】

1.第一步，我们根据前一时刻的状态X和前一时刻的估计不确定性P，根据系统的模型，包括是否带有控制，系统的微分方程得到系统的外推方程，通过系统模型，来估计下一时刻的状态。

这包含的输入为前提条件：初始估计X(先验)和初始估计的不确定性P。初始估计的不确定维度和状态维度一致。反映出每一个状态的估计不确定性(方差)。

需要计算的：建立微分方程，获得外推方程的状态转移矩阵F和控制矩阵G

2.估计系统的过程噪声，过程噪声的设定，是选用离散形式还是连续形式，其值怎么确定？

3.根据状态转移矩阵和过程噪声，外推当前状态的估计不确定。

4.然后计算卡尔曼增益；

5.得到卡尔曼增益之后，就能够结合当前预测的估计和当前的观测值，得到加权后的估计(后验，和这个观测条件结果有关）。

6.得到后验之后，同时更新后验状态估计的不确定性P。

【从一维卡尔曼滤波器】

不带过程噪声的一维卡尔曼滤波器：

前面我们学习了卡尔曼滤波器五个方程的其中两个：

状态更新方程；
动态模型递推方程；

回顾前面的金条例子，我们多次测量求平均值。

在上图中，可以看到真值，估计值、测量值和测量数量。

测量值与真值之间的差值为测量误差，如蓝色和绿色曲线所示。因为测量误差是随机的，我们将其建模为0均值的高斯分别，方差为 $\sigma ^{2}$ ，即为测量的不确定性，此方差能够通过标定的方式获取或者供应商提供。

我们将测量不确定度表示为r。估计值（红线）和真实值（绿线）之间的差异是估计误差。正如你所看到的，随着我们进行额外的测量，估计误差变得越来越小，并且它向0收敛，而估计值向真实值收敛。我们不知道估计误差，但我们可以估计在估计中的不确定性。将估计不确定性表示为p。

下图显示了金条重量的十个测量值。

蓝色圆圈描述了测量值。
真实值由红色虚线描述。
绿线描述了测量的概率密度函数。
粗体绿色区域是标准差 (σ) 的测量值，即测量值位于该区域内的概率为 68.26%。

如您所见，10 个测量值中有 8 个足够接近真实值。它位于1 σ边界。

卡尔曼增益方程的直观推导

卡尔曼增益（表示为 $K_n$ ) 是赋予当前状态估计和测量值的权重。不像α-β-γ参数，为每个滤波器迭代动态计算卡尔曼增益。

在一维中，卡尔曼增益方程如下：

$0\leq K_n \leq1$

重写更新方程：

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

可以看到 $K_n$ 是测量值的权重， $(1-K_n)$ 为当前状态估计的权重，通过前面的数据预测当前时刻的状态。当测量不确定性很大，估计的不确定小的时候，卡尔曼增益接近0，因此我们给很大权重给状态估计，很小的权重给观测。若测量不确定性很小，估计的不确定性很大时，即卡尔曼增益接近1，这时给很大权重给观测，很小权重给预测。

卡尔曼滤波器是最优的因为他最小化估计的不确定性。

一维的估计的不确定更新：

上式称为协方差更新方程。很明显的就是状态的不确定性在经过迭代之后是逐渐缩小的，因为 $(1-K_n)\leq 1$ ，若测量的不确定较大，卡尔曼增益就比较小，因而估计的协方差收敛的比较慢。当测量的不确定较小，那么卡尔曼增益就比较大，估计的不确定就会很快收敛到0。

估计的不确定外推

和状态外推一样，估计的不确定性外推是通过动态模型方程进行的。

如在第二个例子中，预测方程为

那么它的状态不确定外推就是

估计的不确定外推方程又称为协方差外推方程

滤波器的输入为：

Initialization

初始化只做一次，它提供两个参数：其一为初始的状态；其二为初始的不确定性。初始值的提供可以从别的系统而来。

Measurement

输出为：

System State Estimate
Estimate Uncertainty

以下为卡尔曼滤波器的五个方程：

EXAMPLE 5 – ESTIMATING THE HEIGHT OF A BUILDING

假设我们想使用一个非常不精确的高度计来估计建筑物的高度

数值例子：

真实建筑高度为50米。
高度计测量误差（标准偏差）为 5 米。
十个测量值分别为：48.54m、47.11m、55.01m、55.15m、49.89m、40.85m、46.72m、50.05m、51.27m、49.95m。

迭代0次：初始化：看一眼估算出建筑物高度估计为60米。 $\hat{x}_{0,0}=60$

现在我们将初始化估计不确定性。一个人的估计误差（标准差）约为 15 米：σ= 15. 因此方差为 225： $\sigma ^{^{2}} = 225$ ， $\hat{p}_{0,0}=225$ 。

预测：根据初始化值预测下一个状态。由于我们系统的动态模型是恒定的，即建筑物不会改变其高度。

$\hat{x}_{1,0}=\hat{x}_{0,0}=60$

$\hat{p}_{1,0}=\hat{p}_{0,0}=225$

第一次迭代

第一个测量是：z1= 48.54米，由于标准差 (σ) 的高度计测量误差为 5 $\sigma ^{^{2}} = 25$ ，因此测量的不确定度为 $r_1=25$

第二步更新： $k_1=\frac{p_{1,0}}{p_{1,0}+r_1}=\frac{225}{225+25}=0.9$ ，依次进行计算可得如下曲线图

估计值在 7 次测量后收敛到约 49.5 米。

下图比较了测量不确定度和估计不确定度。

在第一次滤波器迭代中，估计不确定度接近测量不确定度并迅速降低。10 次测量后，估计不确定度 (σ2) 为 2.47，即估计误差标准差为：σ=1.57米。

下图显示了卡尔曼增益：

卡尔曼增益正在减少，使测量权重越来越小。卡尔曼滤波器使用的初始值并不精确。因此，状态更新方程中的测量权重较高，估计不确定性较高。每次迭代，测量权重较低；因此，估计的不确定性较低。

【一维卡尔曼滤波器的完整模型-带过程噪声】

我们需要将过程噪声变量添加到协方差外推方程中。

过程噪声PROCESS NOISE

在现实世界中，系统动态模型存在不确定性。例如，当我们要估计电阻器的电阻值时，我们假设一个恒定的动态模型，即电阻在测量之间没有变化。但是，由于环境温度的波动，电阻会略有变化。用雷达跟踪弹道导弹时，动态模型的不确定性包括目标加速度的随机变化。由于可能的飞机机动，不确定性对于飞机来说更为显著。另一方面，当我们使用 GPS 接收器估计静态对象的位置时，动态模型的不确定性为零，因为静态对象不移动。动态模型的不确定性称为过程噪声。也称为模型噪声和系统噪声。

带上过程噪声得协方差外推方程为：

$p_{n+1,n}=p_{n,n}+q_n$ ，其中 $q_n$ 为过程噪声

那么卡尔曼的五个方程分别就变成了

示例 6 – 估计罐中液体的温度

我们想估计罐中液体的温度：

我们假设在稳定状态下，液体温度是恒定的。然而，真实液体温度的一些波动是可能的。我们可以通过以下等式来描述系统动态模型：

$x_n=T+\omega_n$ ， $\omega_n$ 为方差为q的随机噪声。

数值例子

让我们假设真实温度为 50 摄氏度。
我们假设模型是准确的。因此我们设置过程噪声方差（q) 到 0.0001。
测量误差（标准偏差）为 0.1 摄氏度。
每 5 秒进行一次测量。
测量值为：49.95℃, 49.967℃, 50.1℃, 50.106℃, 49.992℃, 49.819℃, 49.933℃, 50.007℃, 50.023℃ 和 49.99℃。

迭代0次

初始化：设置初始状态和初始估计不确定性

我们猜测温度为10℃，即 $\hat{x}_{0,0}=10$ ，我们猜测的不准确，因此设置我们的不确定度的误差

σ到 100，即初始不确定性Estimate Uncertainty为 $\sigma ^{2} = p_{0,0}=100^{2}=10000$

这个方差非常大。如果我们使用更有意义的值进行初始化，我们会获得更快的卡尔曼滤波器收敛。

预测：现在，我们将根据初始化值预测下一个状态。由于我们的模型具有恒定的动态，因此预测估计值等于当前估计值：

$\hat{x}_{1,0}=10$

外推估计的不确定性：

$p_{1,0}=p_{0,0}+q=10000.0001$

第1次迭代

第 1 步 - 测量--测量值： $z_1=49.95$ ，由于测量误差为0.1( $\sigma$ )，因此方差为 $\sigma ^{2}$ 为0.01；因此测量的不确定度为 $r_1=0.01$

第 1 步 - 更新-卡尔曼增益计算

$k_1=\frac{p_{1,0}}{p_{1,0}+r_1}=\frac{10000.0001}{10000.0001+0.01}=0.9999999$

可以看到卡尔曼增益非常接近1，也就是我们估计的误差远大于测量误差，因此估计的权重可以忽略不计，而测量权重几乎也为1。

$\hat{x}_{1,1}=\hat{x}_{1,0}+k_1(z_1-\hat{x}_{1,0})=10+0.999999(49.95-10)=49.95$

更新当前估计不确定性:

$p_{n,n}=(1-K_n)p_{n,n-1}=(1-0.999999)\cdot 10000.0001=0.01$

外推估计不确定性(方差）为

$p_{2,1}=p_{1,1}+q=0.01+0.0001=0.0101$

第2次迭代

第 1 步 - 测量--测量值： $z_2=49.967$ ,由于测量误差为0.1( $\sigma$ )，因此方差为 $\sigma ^{2}$ 为0.01；因此测量的不确定度为 $r_2=0.01$

第 2 步 - 更新

$k_2=\frac{p_{2,1}}{p_{2,1}+r_2}=\frac{0.0101}{0.0101+0.01}=0.5$

如此继续...，

卡尔曼增益正在下降，使测量权重越来越小。估计的不确定性迅速下降。10 次测量后，估计不确定度 (σ2) 为 0.0013，即估计误差标准差为：0.036℃。

示例 7 – 估计加热液体的温度

在这种情况下，系统的动态模型不是恒定的 - 液体以 0.1 的速率加热℃/s。

但是我们按照上述例子的模型，还是认为系统的动态模型是恒定的，即有：

我们假设模型是准确的。因此我们设置过程噪声方差（q) 到 0.0001。
测量误差（标准偏差）为 0.1℃.
每 5 秒进行一次测量。
系统的动态模型是恒定的。

数值例子：

测量值为：

50.45℃, 50.967℃, 51.6℃, 52.106℃, 52.492℃, 52.819℃, 53.433℃, 54.007℃, 54.523℃和 54.99℃.

过程和上例类似，依次求解即可。

但卡尔曼滤波器未能提供可靠的估计。卡尔曼滤波器估计存在滞后误差。

滞后误差有两个原因：

动态模型不适合这种情况。
我们选择了非常低的过程噪声( q= 0.0001 )而真实的温度波动要大得多。

解决方法：

（1）如果我们知道液体温度可以线性变化，我们可以定义一个考虑液体温度可能线性变化的新模型。但是，如果不能对温度变化进行建模，则这种方法没有作用；

（2）另一方面，由于我们的模型没有很好的定义，我们可以通过增加过程噪声来调整过程模型的可靠性( q) ，如下面这个例子。

示例 8 – 估计加热液体的温度Ⅱ

此示例与上一个示例类似，只有一处更改。由于我们的过程没有明确定义，我们增加了过程的不确定性( q)从 0.0001 到 0.15。

估计值遵循测量值。没有滞后误差，由于过程不确定性高，测量权重远高于估计权重。因此，卡尔曼增益很高，收敛到 0.94。

总结：

最好的卡尔曼滤波器实现将涉及一个非常接近现实的模型，几乎没有留下过程噪声的空间。然而，精确的模型并不总是可用的——例如，飞机飞行员可能决定执行改变预测飞机轨迹的突然机动。

多维卡尔曼滤波器

如空中有一个飞机，其位置的状态量为三维 $\left [ x,y,z \right ]$ ,若加上速度和加速度，则

$\left [ x,y,z,\dot{x},\dot{y},\dot{z},\ddot{x},\ddot{y},\ddot{z} \right ]$

假设一个恒定加速度动态模型，我们可以描述外推的飞机状态为：

通常的做法是用矩阵形式的单个方程来描述多维过程。

矩阵相关知识回顾：

卡尔曼滤波器的五个方程：

状态外推方程：根据当前状态的知识预测下一个系统状态。它从当前（时间步长）推断状态向量nn）到未来（时间步长n + 1n+1），状态外推方程描述了动态系统的模型。它也被称为：预测方程、

下图提供了状态外推方程的示意图描述

状态变量可以表示我们希望知道的系统属性，如移动的飞机具有三个属性：位置、速度和加速度。

哪些属性是状态变量，哪些属性是系统的输入？

运动机械系统具有位置、速度、加速度和阻力等属性。
作用在系统上的力应该被认为是一个外力函数，即控制状态向量的系统输入（在恒定加速度情况下的位置和速度）

如：作用在弹簧上的力，可以表示为 $F(t)$ 就能够作为系统的输入 $u(t)$ ，弹簧的位移 $x(t)$ 可以作为系统的状态。

示例 - 飞机 - 无控制输入

我们定义了飞机的状态外推方程，假设一个恒定的加速度模型，状态向量 X描述在笛卡尔坐标系中估计的飞机位置、速度和加速度。

则状态外推方程为

写成矩阵的形式就是：

示例 - 带有控制输入的飞机

例子与上一个类似，不同的是传感器连接着飞机的控制器，就具有额外的信息获取飞机的加速度。

示例 - 自由下落的物体

状态量为高度和速度，

虽然我们没有传感器测量加速度，但是我们知道自由落体的加速度为重力加速度

LINEAR TIME-INVARIANT SYSTEMS

线性系统是方程组，其中变量从不相互相乘，而只是与常数相乘，然后相加。

时不变系统的系统函数不是时间的直接函数。

线性系统建模：

状态外推方程的推导：

我们需要对动态系统进行建模。换句话说，要弄清楚动态系统的状态空间表示。以下两个方程是 LTI 系统的状态空间表示。

为了得到状态转移矩阵，和控制输入矩阵，需要求解状态空间的微分方程。步骤如下图所示。

示例：等速移动体

由于没有外力施加到身体上，系统没有输入： $u(t)=0$

我们得到了微分形式的第一个方程。

系统的输出 $y(t)$ 为物体的位移 $p(t)$ ,

高阶动态系统建模

许多动态系统模型由高阶微分方程描述。微分方程的阶是微分方程中最高导数的个数。为了解决一个高阶方程，我们应该通过定义新变量并将它们代入最高阶项来将其简化为一阶微分方程.

一个普通的n阶线性微分方程可以表达成n个一阶微分方程。如

控制方程完全表征了系统的动态状态。降低方程的阶数：将最高阶移至左边。

定义一个新变量：

然后就可以写成：

示例：恒加速度运动体

使用牛顿第二定律就可以获得其控制方程：

$m\ddot{p}=F$

这是一个二阶微分方程，通过上述方式，将其转为一阶多项式微分方程

系统的输出为物体的位移

求解微分方程

我们需要确定状态外推方程的形式为：

线性时不变系统在不带外部输入的微分方程可以表示为一阶微分方程：

$\dot{x}=Ax$

我们的目标是获取状态转移矩阵F

若是单维系统：

两边积分即可获得：

同理，对于多维情况有：

示例续：等速运动体

具有输入变量的动态系统

对于零阶保持采样，假设输入是分段常数，状态空间方程

的通解形式为：

如果输入量为0时，则方程的解就和上面描述的一样。

例子续：恒加速移动物体：

例续：带阻尼的弹簧系统

其微分方程可以写为

矩阵指数的计算并不容易，因为 A 的高次方不是零。使用计算机软件来解方程。

协方差外推方程：

我们在一维的卡尔曼滤波章节中已经推导了协方差的外推方程，扩展到多维时，协方差的矩阵外推方程为：

多了一个状态转移矩阵的计算。

在没有过程噪声时估计不确定的推导：

在有过程噪声时估计不确定的推导：

我们已经看到，过程噪声方差对卡尔曼滤波器的性能有重要影响。太小的过程噪声Q导致滞后误差；如果Q值太大，卡尔曼滤波器将跟随测量值并产生估计噪声。

过程噪声在不同状态变量之间可以是独立的。在这种情况下，过程噪声协方差矩阵Q是一个对角矩阵。过程噪声也可能是相关的。

我们将看到一个恒速模型示例。该模型假设加速度为零（a = 0）。然而，加速度的随机变化σ2将导致速度和位置的变化。在这种情况下，过程噪声在状态变量之间是相互关联的。

离散噪声模型

离散噪声模型假设噪声在每个时间段内是不同的，但在时间段之间是恒定的。

对于恒速模型：

我们使用加速度随机噪声 $\sigma^2_a$ 来表示

使用状态转移矩阵进行投影

若一个动态模型没有控制输入，我们可以通过投影加速度的随机噪声 $\sigma^2_a$ 使用状态转移矩阵。

使用控制矩阵进行投影

如果动态模型包括控制输入，我们可以计算Q矩阵更快。我们可以预测加速度的随机方差 $\sigma^2_a$ 在我们使用状态转移矩阵的动态模型上。G为控制矩阵

连续噪声模型

如何选择噪声模型？

什么时候Δt 非常小，可以使用离散噪声模型，当Δt较大时最好使用连续噪声模型。我建议尝试这两种模型，并检查哪一个模型在卡尔曼滤波器上表现更好。过程噪声方差选择正确的值。您可以使用随机统计公式计算它，也可以根据您的工程实践选择一个合理的值。

雷达世界中， $\sigma^2_a$ 取决于目标特征和模型的完整性。对于机动目标，如飞机， $\sigma^2_a$ 应该相当大。对于非机动目标，例如火箭，可以使用较小的 $\sigma^2_a$ 。模型完整性也是选择过程噪声方差的一个因素，如果模型包括空气阻力等环境影响，则过程噪声随机性的程度较小，反之亦然。