卡尔曼滤波理解与实现

作者:XU Ruilin, WANG Xuejun, ZHU Ximin, WEN Shuhan, MAO Bo

编者序言

本文为离散卡尔曼滤波算法的一 一个简明教程,从算法思想、实现过程、理论推导和程序实现四个方面阐述和分析了卡尔曼滤波算法。

XU Ruilin完成本教程主要部分的编写,WANG Xuejun完成第3节的编写,ZHU Ximin完成2.2节的编写,WEN Shuhan完成2.3节的编写,MAO Bo完成全文整理、修订和排版。

1 问题引入

卡尔曼滤波(Kalman Filtering)及其一系列的优化和改进算法是目前在求解运动状态推算问题上最为普遍和高效的方法。鲁道夫·卡尔曼(Rudolf Emil Kalman)在NASA埃姆斯研究中心访问时,发现他的方法适用于解决阿波罗计划的轨迹预测问题。阿波罗飞船的导航电脑就是使用这种滤波器进行轨迹预测。

卡尔曼滤波尤其适用于动态系统,这种方法对于内存要求极低而运算速度快,且能够保持较好的计算精度,这使得这种方法非常适合解决实时问题和应用于嵌入式系统,也就是说,卡尔曼滤波天然的适用于解决舰艇指控系统的航迹推算问题。在接下来的内容里,我们将逐步领会卡尔曼滤波的这些绝佳特点。

不过,现在我们先从复杂的舰艇航迹推算问题中解脱出来,从一个更加熟悉和简单的问题中来理解这个滤波算法的思想、过程和算法。

假设有一辆无人车WALL-E,需要导引它从A点到达B点,共有两种手段(图1):

  • 利用WALL-E本身的GPS系统进行定位。
  • 利用开发者写下的WALL-E运动模型和算法来推断自身的位置。


    图1:定位的基本问题.png

显然,两种方法都有一定的误差。如果单独采用某一种方法进行定位,WALL-E在误差的影响下将无法到达B点。因此,需要将两种方法结合起来,得到一个更加精确的结果,这就是卡尔曼滤波要解决的问题。

2 问题求解

卡尔曼滤波方法如何看待我们的问题呢?在探究这个问题之前,我们先对问题进行抽象,并用数学语言来描述我们的问题。

2.1 问题抽象和符号系统

我们用矢量来描述WALL-E的运动状态,这个列矢量包括位置矢量和速度矢量两个分量。在WALL-E的问题上,我们现在不知道位置和速度的准确值,但是知道WALL-E的运动模型满足状态方程,定位的方法,也即观测WALL-E运动状态的方法满足观测方程. 当然,我们也知道,这两种方法都存在一定的误差,那么我们的问题就可以转化为一个优化问题——

在这一优化问题中,目标函数是要使预测(估计)误差最小,同时约束于估计方法和的条件下。在卡尔曼滤波中,我们的估计原则(也就是最小化估计误差的原则)是最小方差无偏估计[1],我们将通过后面的过程分析来说明这一点。

在我们正式开始引入公式分析卡尔曼滤波问题之前,我们还必须解决一个问题------把连续的线性系统离散化,也就是将连续时域问题转化为时间序列问题。当然,目前我们只讨论线性系统的情况,关于非线性系统问题,我们有扩展卡尔曼滤波(Extended Kalman Filtering, EKF)和无迹卡尔曼滤波(Unscented Kalman Filtering, UKF)两种方法来求解。

补充内容------连续线性时变系统的离散化
设连续线性时变系统的时域状态方程为

若采样周期为,则从时刻到时刻,有


令,,则离散化的状态方程为

通过对线性系统的离散化处理,我们现在可以考虑每一个时刻WALL-E的运动状态。接下来,我们将用来表示在时刻运动状态的最优估计值;用表示用时刻对时刻的状态预测值;用表示对时刻综合预测和观测两种方法的最优估计值。

2.2 过程详解

在估计WALL-E位置的问题上,假定我们已经知道它是匀速直线运动,WALL-E身上还携带有一个GPS传感器可以提供它的位置信息,WALL-E在前进过程中可能会遇到一些情况,比如停止前进或是受到风的影响。

加入我们已知的是WALL-E上一个时刻的最佳估计状态,即k-1时刻的位置和速度,要求的是下一时刻即k时刻的最佳估计状态,即k时刻的位置和速度,我们可以发现有两种方法可以得到它的k时刻的状态:

一种是通过WALL-E设定程序计算得到下一秒的状态,比如现在设定是匀速直线运动,那么下一秒的速度应该是恒定不变的,而位置则是在上一秒位置的基础上加上时间乘以速度即一秒内走过的路程,但是现实生活中并不是理想的,机器人会受到摩擦力、风力等的影响,当然也可能会有顽皮的小孩挡住他前进的道路,这些因素使得WALL-E在k时的真实状态与我们计算得到的数据有所不同。

另一种是通过WALL-E所携带的GPS来确定它的位置,因为GPS是测量出的就是WALL-E的实时状态,因此它比较准确。但是GPS测量k时刻的状态有两个问题,一是GPS只能测出WALL-E的位置,而测不出它的速度;二是GPS传感器测量的时候也会有仪器的误差,只能说它是比较准确的,比较接近真实值的。

那么接下来问题来了,我们如何得到k时刻WALL-E的真实状态呢?

我们将第一种方法得到的状态值称为预测值,第二种方法得到的状态值称为测量值,对汽车的最佳估计就是将这两部分信息结合起来,尽量的去逼近k时刻的真实值。

下面再深入一些思考,怎么将这两部分结合起来?

在初始时间k-1, 是WALL-E的最佳估计值,WALL-E其实可以是估计值附近的任何位置,并且这种不确定性由该概率密度函数描述。WALL-E最有可能在这个分布的平均值附近。在下一个时间,估计的不确定性增加,用一个更大的方差表示,这是因为在时间步骤k-1和k之间,WALL-E可能收到了风力的影响,或者脚可能向前滑了一点,因此,它可能已经行进了与模型预测的距离不同的距离。

WALL-E位置的另一个信息来源来自测量,方差表示误差测量的不确定性,真正的位置同样可以是平均值附近的任何位置。

预测值和测量值,对WALL-E的最佳估计是将这两部分信息结合起来,将两个概率函数相乘得到另一个高斯函数,该估计值的方差小于先前估计值,并且该概率密度函数的平均值为我们提供了WALL-E位置的最佳估计。

2.3 从控制论角度分析

    为了让卡尔曼滤波器具体可感,在进行公式推导之前,我们先进行原理的理解和模型的建立。
    假设现在有一个宇宙飞船,为了让这个宇宙飞船能够在宇宙空间中平稳运行,我们需要知道他的发动机内部温度,我们知道燃料进量等信息,但是我们无法把传感器直接加在发动机内部,因为会被融化,但是如果我们如果能够找到 目的状态“内部温度”和另一状态的关系,就能够通过建立联系来得知内部温度。因此我们把传感器加在发动机外部,测量发动机外部温度,我们称它为测量状态。
    如果说我们可以建立相对完善的数学模型来表达这个实际问题,我们就可以通过“燃料进量”等已知因素(在此我们称之为 控制状态)和测量状态“发动机外部温度”来 估测目的状态“内部温度”。
     建立模型的目的在于, 使目的状态 —— 内部温度的估计值 , 无限趋近收敛于真实的外部温度,由于我们的数学模型足够完善,只要我们让测量状态——外部温度的估计值收敛于真实测量值,目的状态自然也与之匹配。
图2:宇宙飞船模型框图.png
   但是,如果模型对于测量状态的估计值和测量值 有偏差怎么办呢,我们引入控制论的思想,把测量状态的真实值和测量值的差值 反馈回数学模型,对模型进行修正。
图3:加入反馈后的宇宙飞船模型.png
   接下来,我们将这个实际的例子抽象一下。
图4:卡尔曼滤波器初步抽象模型.jpg
    我们将测量状态和目的状态之间的联系,用系数矩阵C表示,目的状态随着时间变化的规律我们用x对时间求导来表示(后续的例子中,我们将会解释另外一种目的状态与时间建立关系的方法——抽样),求导后的x受目的状态与控制状态的影响,我们将这种影响关系用系数矩阵AB表示。

    我们回到初衷——使模型的目的状态估计值和实际问题的目标状态值趋同,那么我们将估计值收敛于实际值之前的误差记为e,

以下,我们将进行e的运算推导

设:

则有实际目标变量的表达式:

数学模型中目标变量的表达式:

实际模型中测量变量的表达式:

数学模型中测量变量的表达式:

将目标变量的实际值和估计值相减:

将上述方程带入误差e的表达式,我们可得出误差e的解析解:


从推导结果中我们不难看出,估计值和实际值的误差随时间呈指数形式变化,当(F-KH)<1时,随着时间的推移,会无限趋近于零,也就是意味着估计值和实际值相吻合。这就是为什么卡尔曼滤波器可以完美预测出目标状态值的原理。

    当然,有人会说,如果不引入这个卡尔曼增益,只要A<1,同样会导致e收敛于0,这也就说明了卡尔曼滤波器的意义——更快减小预测的误差,得到最优值。

2.4 过程分析

在估计WALL-E位置的问题上,我们不知道位置和速度的准确值,但是我们可以给出一个估计区间(图5.a)。卡尔曼滤波假设所有的变量是随机的且符合高斯分布(正态分布)。每个变量有一个均值和一个方差 (图5.b)。而图5.c则表示速度和位置是相关的。

图5:估计的不确定性.png

假如我们已知上一个状态的位置值,现在要预测下一个状态的位置值。如果我们的速度值很高,我们移动的距离会远一点。相反,如果速度慢,WALL-E不会走的很远。这种关系在跟踪系统状态时很重要,它给了我们更多的信息:一个观测值告诉我们另一个观测值可能是什么样子。这就是卡尔曼滤波的目的------从所有不确定信息中提取有价值的信息。

2.4.1 利用矩阵描述问题

根据数理统计知识,我们知道这种两个观测值(随机变量)之间的关系可以通过一个协方差矩阵

描述(图6)。

图6:协方差矩阵对变量相关性的反映.png

我们假设系统状态的分布为高斯分布(正态分布),所以在时刻我们需要两个信息:最佳预估值及其协方差矩阵(如式(2)所示)。

下一步,我们需要通过时刻的状态来预测时刻的状态。请注意,我们不知道状态的准确值,但是我们的预测函数并不在乎,它仅仅是对时刻所有可能值的范围进行预测转移,然后得出一个k时刻新值的范围。在这个过程中,位置和速度的变化为

我们可以通过一个状态转移矩阵来描述这个转换关系

同理,我们更新协方差矩阵为
\boldsymbol{P_{k \mid k-1}}= Cov(\boldsymbol{F_{k \mid k-1}}\boldsymbol{x_p},\boldsymbol{F_{k \mid k-1}}\boldsymbol{x_v})=\boldsymbol{F_{k \mid k-1}}Cov(\boldsymbol{x_p},\boldsymbol{x_v})\boldsymbol{F_{k \mid k-1}^\top}=\boldsymbol{F_{k \mid k-1}}\boldsymbol{P_{k-1}}\boldsymbol{F_{k \mid k-1}^\top}

2.4.2 考虑系统控制情况时的状态描述

到目前为止,我们考虑的都是匀速运动的情况,也就是系统没有对WALL-E的运动状态进行控制的情况。那么,如果系统对WALL-E进行控制,例如发出一些指令启动或者制动轮子,对这些额外的信息,我们可以通过一个向量来描述这些信息,并将其添加到我们的预测方程里作为一个修正。假如我们通过发出的指令得到预期的加速度,运动状态方程就更新为

引入矩阵表示为

\boldsymbol{x_{k \mid k-1}} =\boldsymbol{F_{k \mid k-1}}\hat{\boldsymbol{x_{k-1}}}+ \begin{bmatrix}\frac{{\Delta t}^2}{2} \\ \Delta t\end{bmatrix}\boldsymbol{a}\\ =\boldsymbol{F_{k \mid k-1}}\hat{\boldsymbol{x_{k-1}}}+\boldsymbol{B_{k-1}}\boldsymbol{u_{k-1}}

式中称为控制矩阵,称为控制向量(例如加速度)。当然,如果没有任何外界动力影响的系统,可以忽略这一部分。

2.4.3 不确定性的影响

我们增加另一个细节,假如我们的预测转换矩阵不是100%准确呢,会发生什么?如果状态只会根据系统自身特性演变,那样将不会有任何问题。如果所有外界作用力对系统的影响可以被计算得十分准确,那样也不会有任何问题。但是如果有些外力我们无法预测,例如我们在跟踪一个四轴飞行器,它会受到风力影响;或者在跟踪一个轮式机器人,轮子可能会打滑,地面上的突起会使它减速。我们无法跟踪这些因素,而这些不确定事件发生时,预测方程将会失灵。因此,我们将这些不确定性统一建模,在预测方程中增加一个不确定项。

通过这种方式,使得原始状态中的每一个点可以都会预测转换到一个范围,而不是某个确定的点(图7.a)。可以这样描述------中的每个点移动到一个符合方差的高斯分布里(图7.b)。换言之,我们把这些不确定因素描述为方差为的高斯噪声,并用表示。这样就会产生一个新的高斯分布,方差不同,但是均值相同(图7.c)。

图7:在高斯噪声下的状态预测.png

通过对的叠加扩展,得到完整的预测转换方程为

新的预测转换方程只是引入了已知的系统控制因素。新的不确定性可以通过之前的不确定性计算得到。到这里,我们得到了一个模糊的估计范围------通过和描述的范围。

2.4.4通过观测校正预测

我们之前的工作仍然是在使用运动模型一种方法来估计系统的状态,现在,我们要把另一种方法,也就是观测(本问题中为GPS定位)考虑进来,以进一步修正对运动状态的估计(图8)。

图8:预测对预测范围的约束.png

我们用矩阵来描述观测方法的作用,于是有

再加入观测噪声,观测方程为

图9:观测矩阵.png

从控制论的角度出发,我们定义新息(也即观测值与预测值的误差)为

当然我们也知道,观测本身也会存在误差,比如本问题中的GPS定位精度仅有10m. 因此,我们用矩阵来描述这种不确定性(图10图11.a)。

图10:观测误差.png

这时,我们新息的协方差为

\begin{aligned} \boldsymbol{S_k}=Cov(\boldsymbol{\tilde{x_k}})=Cov(\boldsymbol{z_k}-\boldsymbol{H_k}\boldsymbol{x_{k \mid k-1}})\\ =Cov(\boldsymbol{H_k}\boldsymbol{x_k}+\boldsymbol{v_k}-\boldsymbol{H_k}\boldsymbol{x_{k \mid k-1}})\\ =\boldsymbol{H_k}\boldsymbol{P_{k \mid k-1}}\boldsymbol{H_k^\top}+\boldsymbol{R_k} \end{aligned}

现在我们需要把两种方法得到的可能性融合起来(图11.b)。对于任何状态,有两个可能性:1. 传感器的观测值更接近系统真实状态;2. 模型推算的估计值更接近系统真实状态。如果有两个相互独立的获取系统状态的方式,并且我们想知道两者都准确的概率值,于是我们可以通过加权来解决更相信谁的问题(图11.c)。

图11:使用观测更新预测.png

我们现在知道,系统模型的状态预测与对系统的状态观测服从高斯分布,把这个问题抽象一下就是——

根据我们的一个估计准则------最小方差估计,那么这个问题可以转化为优化问题求解

求导数(差分)得

则,从而

当维度高于一维时,我们用矩阵来描述,有

这里的称为卡尔曼增益(Kalman Gain),也就是我们在解决更信任哪种方法时的偏向程度。

如果我们从两个独立的维度估计系统状态,那么根据系统模型的预测为

通过传感器的观测为

我们结合着两种方法得到

由可知,卡尔曼增益为

将约去(中也含有项),得

此时的卡尔曼增益实际为

我们最后再来验证一下估计的无偏性——

这里我们设时刻的真值为,由于

\begin{aligned} \mathbb{E}(\boldsymbol{\hat{x_k}}-\boldsymbol{x_k})&=\mathbb{E}[\boldsymbol{\hat{x_k}}+\boldsymbol{K}(\boldsymbol{z_k}-\boldsymbol{H_k}\boldsymbol{x_{k \mid k-1}})-\boldsymbol{x_k}]\\ &=\mathbb{E}[\boldsymbol{\hat{x_k}}+\boldsymbol{K}(\boldsymbol{H_k}\boldsymbol{x_k}+\boldsymbol{v_k}-\boldsymbol{H_k}\boldsymbol{x_{k \mid k-1}})-\boldsymbol{x_k}]\\ &=\mathbb{E}[(\boldsymbol{I}-\boldsymbol{K}\boldsymbol{H_k})(\boldsymbol{x_{k \mid k-1}}-\boldsymbol{x_k})+\boldsymbol{K}\boldsymbol{v_k}]\\ &=\mathbb{E}[(\boldsymbol{I}-\boldsymbol{K}\boldsymbol{H_k})(\boldsymbol{x_{k \mid k-1}}-\boldsymbol{x_k})]+\boldsymbol{K}\mathbb{E}(\boldsymbol{v_k})\\ &=\mathbb{E}[(\boldsymbol{I}-\boldsymbol{K}\boldsymbol{H_k})(\boldsymbol{x_{k \mid k-1}}-\boldsymbol{x_k})] \end{aligned}

由于(从初值而来的无偏传递性)可知,即卡尔曼滤波满足无偏估计准则。显然,其中要求系统噪声和观测噪声是不相关、零期望的白噪声,且是线性系统,初始时刻的状态估计是无偏的。当这些条件不能满足时,卡尔曼滤波的估计结果是有偏的。

2.5 过程整理

到这里,我们已经获得了卡尔曼滤波的全部要素。我们可以把整个过程总结为3个基本假设

假设一和都是零均值高斯白噪声,也即,

假设二与无关,也即

假设三系统初值的均值和方差已知,且与均不相关。

以及5个基本方程 方程一状态预测

方程二协方差预测

方程三卡尔曼增益

方程四状态更新

方程五协方差更新

3 数学推导

=E\left[ \left[ \left( I-KH \right) \left( x_k-x_{k|k-1} \right) \right] \left[ \left( I-KH \right) \left( x_k-x_{k|k-1} \right) \right] ^T-Kv_k\left[ \left( I-KH \right) \left( x_k-x_{k|k-1} \right) \right] ^T-\left[ \left( I-KH \right) \left( x_k-x_{k|k-1} \right) \right] \left[ Kv_k \right] ^T+\left( Kv_k \right) \left( Kv_k \right) ^T \right]


协方差矩阵的对角线元素就是方差。把矩阵P的对角线元素求和,定义为矩阵的迹[2][3]


最小均方差就是使得上式最小,对未知量K求导,令导函数等于0,就能找到K的值。


注意这个计算式,转换矩阵式常数,测量噪声协方差也是常数。因此的大小将与预测值的误差协方差有关。不妨进一步假设,上面式子中的矩阵维数都是1*1大小的,并假=1,不等于0。那么可以写成如下:

所以越大,那么就越大,权重将更加重视反馈,如果等于0,也就是预测值和真实值相等,那么,估计值就等于预测值(先验)。

     将计算出的这个K反代入Pk中,就能简化Pk,估计协方差矩阵Pk的:


因此递推公式中每一步的K就计算出来了,同时每一步的估计协方差也能计算出来。但K的公式中好像又多了一个我们还未曾计算出来的东西 他称之为预测值和真实值之间误差的协方差矩阵。它的递推计算如下:

=E\left[ F_{k|k-1}\left( x_{k-1}-\hat{x}_{k-1} \right) \left( x_{k-1}-\hat{x}_{k-1} \right) ^TF_{k|k-1}^{T}+F_{k|k-1}\left( x_{k-1}-\hat{x}_{k-1} \right) w_{k}^{T}+w_k\left( x_{k-1}-\hat{x}_{k-1} \right) ^TF_{k|k-1}^{T}+w_kw_{k}^{T} \right]

    由此也得到了$P_{k|k-1}$的递推公式。因此我们只需设定最初的$P_k$就能不断递推下去。

4 仿真分析

现在我们使用卡尔曼滤波来解决一个实际问题。在这个仿真试验中,我们提供了一个基于Python 3.7的脚本程序。接下来我们将结合这个脚本程序来解释如何使用代码复现离散卡尔曼滤波算法。而实际上,不论采用何种编程语言,其复现思路都是一致的,无非就是将数学描述转换为代码描述。

   # Python 3.7 编译环境
   # 导入第三方库numpy和matplotlib
   import numpy as np # 用于进行数学计算
   import matplotlib.pyplot as plt # 用于绘图

   plt.rcParams['font.sans-serif']=['Kaiti'] # 用来正常显示中文标签
   plt.rcParams['axes.unicode_minus']=False # 用来正常显示负号

   # 类就是一个模板,模板里可以包含多个函数,函数里实现一些功能
   # 对象则是根据模板创建的实例,通过实例,对象可以执行类中的函数
   class KalmanFilter(object):
      # python的面向对象编程中的__init__()方法,用以初始化新创建对象的状态
      # self为实例本身,F,B,H,Q,R,P,x0都是实例self的属性
      def __init__(self, F = None, B = None, H = None, Q = None, R = None, \
      P = None, x0 = None): # 所有矩阵置空
         
         # 异常处理,抛出一个异常:“设置适当的系统状态.”
         # 也就是说,F矩阵和H矩阵不能为空,这是理所当然的
         # F矩阵由我们的运动模型来确定,H矩阵由我们的观测方法来确定
         if(F is None or H is None):
            raise ValueError("Set proper system dynamics.")
         
         # F.shape用于返回矩阵F的尺度,是一个二维列表
         # F.shape[0]为行长度,F.shape[1]为列长度
         # 在MATLAB中,我们可以用函数size()实现这一功能
         self.n = F.shape[1] 
         self.m = H.shape[1]
         self.F = F # 将F的值赋给对象self.F
         self.H = H # 将H的值赋给对象self.H
         # 若未指定,则定义控制矩阵为0矩阵
         self.B = 0 if B is None else B
         # 若未定义3个协方差矩阵P、Q、R,则定义为与F矩阵维度一致的单位矩阵
         # 否则按原定义传入
         self.Q = np.eye(self.n) if Q is None else Q
         self.R = np.eye(self.n) if R is None else R
         self.P = np.eye(self.n) if P is None else P
         # 若未定义初值x0,则定义为0向量
         self.x = np.zeros((self.n, 1)) if x0 is None else x0
      
      # 预测模块,也就是使用系统运动模型进行估计
      def predict(self, u = 0):
         # 实现公式x(k|k-1)=F(k-1)x(k-1)+B(k-1)u(k-1)
         # np.dot(F,x)用于实现矩阵乘法
         self.x = np.dot(self.F, self.x) + np.dot(self.B, u)
         # 实现公式P(k|k-1)=F(k-1)P(k-1)F(k-1)^T+Q(k-1)
         # 在Python中,求A得转置的语法为A.T
         # 在MATLAB中为A'
         self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q
         return self.x
      
      # 状态更新模块,也就是使用观测校正预测
      def update(self, z):
         # 新息公式y(k)=z(k)-H(k)x(k|k-1)
         y = z - np.dot(self.H, self.x)
         # 新息的协方差S(k)=H(k)P(k|k-1)H(k)^T+R(k)
         S = self.R + np.dot(self.H, np.dot(self.P, self.H.T))
         # 卡尔曼增益K=P(k|k-1)H(k)^TS(k)^-1
         # linalg.inv(S)用于求S矩阵的逆
         K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
         # 状态更新,实现x(k)=x(k|k-1)+Ky(k)
         self.x = self.x + np.dot(K, y)
         # 定义单位阵
         I = np.eye(self.n)
         # 协方差更新,这里对式(35)进行了移项处理
         # P(k)=[I-KH(k)]P(k|k-1)
         self.P = np.dot(np.dot(I - np.dot(K, self.H), self.P), \
         (I - np.dot(K, self.H)).T) + np.dot(np.dot(K, self.R), K.T)

   # 卡尔曼滤波仿真
   def example():
      # 离散化处理,dt为采用周期
      dt = 1.0/60
      # 以下均为赋初值的过程
      F = np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]])
      H = np.array([1, 0, 0]).reshape(1, 3)
      Q = np.array([[0.05, 0.05, 0.0], [0.05, 0.05, 0.0], [0.0, 0.0, 0.0]])
      R = np.array([0.5]).reshape(1, 1)
      x = np.linspace(-10, 10, 100)
      # 加入噪声的观测
      measurements = - (x**2 + 2*x - 2)  + np.random.normal(0, 2, 100)
      # 将无噪声的观测作为真值
      actual = - (x**2 + 2*x - 2)
      
      # 动用我们之前定义的类:卡尔曼滤波器
      # 预测
      kf = KalmanFilter(F = F, H = H, Q = Q, R = R)
      predictions = []
      error = []
    
      #使用观测更新预测
      for z in measurements:
         predictions.append(np.dot(H, kf.predict())[0])
         kf.update(z)
      
      # 求估计误差
      for i in range(len(actual)):
         error.append(actual[i]-predictions[i])
    
      # 绘图
      plt.plot(range(len(actual)),actual, label='真值')
      plt.plot(range(len(predictions)), np.array(predictions), label='最优估计值')
      plt.legend()
      plt.show()
    
      # 作一个零向量用于对照
      zeros = []
      for j in range(len(actual)):
         zeros.append(0)
    
      plt.plot(range(len(error)),error,label='估计误差')
      plt.plot(range(len(error)),zeros)
      plt.legend()
      plt.show()

   # 主程序运行
   if __name__ == '__main__':
      example()      
图12:仿真结果.png

图12所示,在算法经过启动阶段后,预测值与实际值的变化值符合我们的最优估计原则。但是这种最优依赖于理想的噪声环境条件,在实际的应用场景下,我们需要根据场景具体情况调整我们的算法。读者如有余力,可以尝试在此基础上使用其他程序设计语言复现卡尔曼滤波算法。

我们也提供了更加简洁的面向过程的Python复现源代码和仿真结果(图13)。

   # Python 3.7 编译环境
   import numpy as np
   import matplotlib.pyplot as plt

   delta_t = 0.1                               # 每秒钟采一次样
   end_t = 7                                   # 时间长度
   time_t = end_t * 10                         # 采样次数
   t = np.arange(0, end_t, delta_t)            # 设置时间数组

   u = 1                                       # 定义外界对系统的作用 加速度
   x = 1 / 2 * u * t ** 2                      # 实际真实位置

   v_var = 1                                   # 测量噪声的方差
   # 创建高斯噪声,精确到小数点后两位
   v_noise = np.round(np.random.normal(0, v_var, time_t), 2)

   X = np.mat([[0], [0]])                      # 定义预测优化值的初始状态
   v = np.mat(v_noise)                         # 定义测量噪声
   z = x + v                                   # 定义测量值(假设测量值=实际状态值+噪声)
   A = np.mat([[1, delta_t], [0, 1]])          # 定义状态转移矩阵
   B = [[1 / 2 * (delta_t ** 2)], [delta_t]]   # 定义输入控制矩阵
   P = np.mat([[1, 0], [0, 1]])                # 定义初始状态协方差矩阵
   Q = np.mat([[0.001, 0], [0, 0.001]])        # 定义状态转移(预测噪声)协方差矩阵
   H = np.mat([1, 0])                          # 定义观测矩阵
   R = np.mat([1])                             # 定义观测噪声协方差
   X_mat = np.zeros(time_t)                    # 初始化记录系统预测优化值的列表

   for i in range(time_t):
      # 预测
      X_predict = A * X + np.dot(B, u)        # 估算状态变量
      P_predict = A * P * A.T + Q             # 估算状态误差协方差
      # 校正
      K = P_predict * H.T / (H * P_predict * H.T + R)     # 更新卡尔曼增益
      X = X_predict + K * (z[0, i] - H * X_predict)       # 更新预测优化值
      P = (np.eye(2) - K * H) * P_predict                 # 更新状态误差协方差
      # 记录系统的预测优化值
      X_mat[i] = X[0, 0]

   plt.rcParams['font.sans-serif'] = ['kaiti']    # 设置正常显示中文
   plt.plot(x, "b", label='实际状态值')            # 设置曲线数值
   plt.plot(X_mat, "g", label='预测优化值')
   plt.plot(z.T, "r--", label='测量值')
   plt.xlabel("时间")                             # 设置X轴的名字
   plt.ylabel("位移")                             # 设置Y轴的名字
   plt.legend()                                   # 设置图例
   plt.show()                                     # 显示图表
图13:另一组仿真结果.png

5 结语

到这里,我们已经了解了卡尔曼滤波(离散情况)的思想、过程和实现方法。这一滤波算法的两大特点——

  • 线性递推

  • 最小方差无偏估计

分别通过(1)用上一时刻的最优估计值和当前时刻的观测值来估计本时刻的最优估计值(2)使状态估计的偏差为0且方差最小 体现出来。由于只用存储上一时刻的预测和本时刻的观测,使得卡尔曼滤波算法占用空间很小,且可以保证较高的准确性(图12),因此适用于嵌入式系统和解决实时问题。当然,这种准确性建立在我们对噪声(误差)的假设上。在实际应用中,我们将对其进行优化调整来解决问题,但上述的两大思想是不变的。

6 参考文献

[1] Bzarg,Bot图说卡尔曼滤波,一份通俗易懂的教程[EBOL].[2018-07-17].https://zhuanlan.zhihu.com/p/39912633?utm_source=wechat_session&utm_medium=social&utm_oi=834493203137826816
[2] 云羽落,如何通俗并尽可能详细地解释卡尔曼滤波?[EBOL].[2019-10-04].https://www.zhihu.com/question/23971601


  1. 统计学上, 最小方差无偏估计(Minimum-Variance Unbiased Estimator, MVUE)是一个对于所有无偏估计中,拥有最小方差的无偏估计。若无论真实参数值是多少,最小方差无偏估计(MVUE)都比其他不偏估计有更小或至多相等的方差,则称此估计为一致最小方差无偏估计(Uniformly Minimum-variance Unbiased Estimator, UMVUE)。 ↩

  2. 定义:在线性代数中,n乘n方阵“A”的迹,是指“A”的主对角线各元素的总和(从左上方至右下方的对角线),例如: 其中代表在 i 行 j 列中的数值。同样的,元素的迹是其特征值的总和,使其不变量根据选择的基本准则而定。 ↩

  3. 矩阵的迹的性质:迹是一种线性算子。亦即,对于任两个方阵A、B和标量r,会有下列关系: 1.主对角线不会在转置矩阵中被换掉,所以所有的矩阵和其转置矩阵都会有相同的迹。2. 3. 4. 5. 6. 7. ↩

你可能感兴趣的:(卡尔曼滤波理解与实现)