算法介绍及实现——卡尔曼滤波

        本文主要介绍卡尔曼滤波的推导过程及建模步骤，是网站的学习笔记。本文主要是通过例子来引出卡尔曼滤波的建模思想及算法步骤。

---

参考网站：（这个网站讲得真的很详细很清楚，层层递进，逻辑清晰）Kalman Filter Tutorialhttps://www.kalmanfilter.net/

---

目录

一、引言

        1）引例1——运动状态建模

二、g-h(-k)滤波器

        1）引例2——黄金称重

        2）引例3——一维模式下的等速飞机追踪

        3）引例4——匀加速飞机轨迹预测

三、一维卡尔曼滤波器

        1）引入观测噪声（Measurement Error）

        2）引入过程噪声（Process Noise）

        3）引例5

四、多维卡尔曼滤波器

        1）多维情况下的状态外推方程

        2）多维情况下的协方差外推方程

        3）多维情况下的状态更新方程

        4）协方差更新方程

        5）卡尔曼增益方程

        6）引例6

        7）引例7

---

一、引言

        1）引例1——运动状态建模

        基于一个匀加速直线运动对物体的位置进行预测，设当前为X0（当前状态），X为预测位置。若是物体做理想的匀加速直线运动，则可使用下式进行预测：

 将其扩展到三维空间中有：

         上述的方程反应了输入数据（位移或坐标）对输出数据之间的关系，这样描述上一个系统状态和下一个系统状态之间关系的方程称为动态模型或状态空间模型。

        但是，实际情况不会是抽象分析中的理想环境，还包括各种各样的误差影响，若想在实际情况中获得高精度的预测，就必须要将噪声考虑其中。此处主要将噪声分为两类：

        测量噪声：是指在测量过程中的噪声，比如在上述引例中，需要对速度和时间进行测量，而对这些量的观测存在一个随机误差，测量中包含的误差称为测量噪声。（在建模过程中随机误差即测量噪声服从正态分布）

        过程噪声：由于外部环境的原因，使得物体并非做预想的严格的匀速直线运动（即由于外界环境影响，物体运动并非完全与运动方程对齐），这种动态模型的误差（不确定性）称为过程噪声

 

二、g-h(-k)滤波器

        1）引例2——黄金称重

        对黄金进行称重。假设现在有一块黄金，每日对其进行称重，称重过程（测量过程）只包含随机误差，不包含系统误差，那么每日的黄金重量可能如下：

         连续观测n天，我们选取前n天的平均值（每天的观测值记为Zi）作为第n天黄金重量的估计值。

         第一个下标表示对第n天的估计值，第二个下标表示基于多少天的数据，若基于前n-1天的观测值对第n天的数据进行估计则记为Xn,n-1：

         由于我们认定黄金的重量是恒定不变的，所以状态空间模型(基于当前状态值对下一状态的预测)为：

 由于预测时需要再重新累加求和，由此我们可以对式子进行改写：

 对上式证明如下：

 再顾及状态空间模型有：

 上述式子可以描述为：

        该方程被称为状态更新方程，其中的（Measurement-Predicted Value of the current state）被称为测量残差。其也表示了基于该时刻预测值和观测值获得该时刻的估计值的方法。其中1/n称为卡尔曼增益，在本例子中会随着迭代而改变。所以可写为如下方程： 

         ——数值示例（搬运于网站，看不清可直接跳转网页）The alpha - beta - gamma filter (kalmanfilter.net)https://www.kalmanfilter.net/alphabeta.html

 

······

        2）引例3——一维模式下的等速飞机追踪

        在一维对等速飞机进行追踪。有一雷达对飞机进行跟踪，如下图:

Xn表示时间n内飞机的飞行距离，飞机的速度可以用微分方式获得： 

         则其动态模型为：

 

上述方程组被称为过渡方程或预测方程（也称状态外推方程），也是卡尔曼五个方程之一，这个方程组完成由本状态向下一状态的预测。

（回顾：到现在已经学了两个卡尔曼方程。第一个方程为状态更新方程，他是基于上一个状态对当前状态的预测值和当前状态的观测值实现对当前状态值的估计；第二个方程是预测方程，他是基于当前状态和观测值实现对下一状态的预测。）

        示例：

         上述说明，在速度不变的情况下，本次飞机雷达测距为30000m，在时间间隔5s后飞机雷达测距应该为30200m（预测值）。此番工作就是基于当前状态对下一状态的预测。

        但是，5s后飞机雷达测距为30110m，和预测值存在90m的误差，那为什么会产生误差呢。有两个可能：要么是雷达测量不准；或者是飞机速度发生了改变。那究竟是哪一种可能呢？

         写出速度的状态更新方程：

         我对其解释为：

 关于对卡尔曼增益的确定：

         雷达误差是一种随机误差服从正态分布，若误差在雷达测量的标准差外，则可能是由速度变化引起的，反之。卡尔曼增益取决于其雷达的测量精度，若精度越高，则说明残差就越重要，卡尔曼增益就应该越大；精度越低，说明测量的权重不高，没有那么重要，所以残差就没那么重要，卡尔曼增益就应该越小。

        如果飞机速度确实从40m / s变为22m / s，我们在10个循环后看到这一点（运行上面的等式10次β= 0.1）。如果间隙是由测量误差引起的，则连续测量将在预测位置的前面或后面。因此，平均而言，目标速度不会改变。

        然后写出距离的状态更新公式。

         此处的参数卡尔曼增益表示雷达的测量精度，是一个常数。当其为1时，说明测量极其精确，观测值即为估计值；当其为0是，说明测量毫无意义。

        因此，对于飞机的位置和速度，我们都得到一个状态更新公式：

         该方程组构成了雷达跟踪器的状态更新方程。它们也被称为α−β（也称g-h）跟踪更新公式或α−β跟踪过滤方程.

        算例：The alpha - beta - gamma filter (kalmanfilter.net)

        python实现算例：kalman_g_h_filter.py （文末获取）

        3）引例4——匀加速飞机轨迹预测

        若用上式（引例3的模型）进行“先匀速再加速”的轨迹预测，效果如下：

 

        能很明显的发现实际的测量值和估计值之间存在这很大的差距，因为使用匀速运动的模型来预测匀加速运动，所以会导致预测会产生误差，这差距被称为滞后误差或归为系统误差。

        现在我们来讨论匀加速运动模型的建模。

        基于上述所言，我们现在讨论对匀加速直线运动的飞机的跟踪建模。因运动中引入了加速度，所以状态外推方程（预测方程）即为：

         对应即可得到其状态更新公式，回顾上述引例，在黄金称重例子中，我们只有一个状态即黄金的重量，所以状态更新公式和外推（预测）公式相应只有一个；在飞机的匀速运动引例中，飞机的状态包括位移x和速度v，所以其状态更新公式和外推（预测）公式相应的都有两个；依次类推，在匀加速运动中又引入了状态——加速度a，所以在本模型中，外推（预测）公式和状态更新公式都有三个，如下：

         方程组中的1式不用解释，2式在引例3中引入速度更新公式时做出了解释。对3式来说，理解如下：

演算示例：

        选取一个场景：一架飞机以50m / s的恒定速度移动15秒，然后以8m / s的恒定加速度加速再延长 35 秒。参数如下：

 

 

 。。。

结果图如下：

 

python实现算例：kalman_g_h_k_filter.py  (文末获取）

对g-h-(k)滤波器的总结。

        建模步骤总结：

        ——step1首先基于启动数据，利用状态外推方程（预测方程）获得下一状态预测值。

        ——step2基于预测值和观测值，使用状态更新方程获得当前状态的估计值。

        ——step3完成当前状态的估计之后，基于当前状态估计值使用预测方程获得对下一状态的预测值。

        ——step4重复步骤2和步骤3。

        在建模过程中，g-h-k滤波器的参数选择至关重要。

g_h_k滤波器优化论文：

 （文末获取）

三、一维卡尔曼滤波器

        1）引入观测噪声（Measurement Error）

        回顾引例2，在对黄金的称重过程中，其结果如下：

我们可以明显的发现测量值和真实值之间存在很大的差距，我们将这种差距称为测量误差，也就是测量噪声。我们将测量误差用测量不确定度来表示，记为r。在本例中，我们可以通过称的商家获得（商家一般都会有自己仪器的测量误差）或推导获得。假设方差为m*m，那么标准差为m，根据随机误差服从正态分布可得下图：

         由正态分布理论可知，若真值属于观测值左右一个标准差范围内（l-m，l+m），则认为观测是合理的，上图10次观测中，有8次符合条件。所以，测量的不确定都r即为方差m*m。

        现在推导卡尔曼增益方程，在原来的引例中，参数取的是1/n，在卡尔曼滤波过程中，卡尔曼增益是迭代计算的，如下式即为卡尔曼增益方程：

        其中，r为测量不确定度，p为外推估计的不确定度（用于衡量估计值与真实值的误差，可理解为外推估计的权重）。r为方差大于零，所以卡尔曼增益范围为0至1。

        综上状态更新方程可变为：

         外推估计的不确定（p）的更新

        上式被称为协方差更新方程，每次滤波器迭代时，1-K是小于1的，所以估计的不确定性在减小。当测量不确定度（r，等于方差）很大时，K值很小，1-K就更接近1，所以滤波器收敛速度慢，反之。 

        现在，我们来讨论估计不确度的外推。首先我们梳理一下，当上一状态完成外推估计（预测）后，获得外推估计不确定度Pn,n-1，进而计算出卡尔曼增益K，再根据卡尔曼增益获得当前估计的不确定度Pn,n。而我们当前状态仍然需要对下一状态进行外推估计（预测），所以我们需要计算出Pn+1,n。

        以引例3匀速飞行飞机为例，状态包括位移和速度，其状态外推方程为:

 基于外推方程可以得到估计的不确定度计算公式：

        （关于协方差外推公式的理解：如下图） 

        估计不确定度的外推方程称为协方差外推方程。至此五个卡尔曼方程介绍结束。

在此，我们将所学的五个卡尔曼方程汇总起来：

首先是状态更新方程：实现对预测值和观测值的加权拟合（滤波）。

 

第二个是状态外推（预测）方程：基于当前状态估计值对下一个状态进行估计。

（以引例3为例）

 

第三是在引入观测噪声后，计算卡尔曼增益的卡尔曼增益方程。

 

第四是对估计不确定度的更新方程，即协方差更新方程。

 

第五是将估计不确定度外推到下一状态的协方差外推方程，（以引例3为例）

 

        例子可见官网。

        2）引入过程噪声（Process Noise）

        综上所述，我们讨论了含有观测噪声的情况下的卡尔曼滤波建模方法，并做了算例分析。但正如最先所讲，不仅包括观测噪声，还包括过程噪声，动态模型（状态外推方程）的不确定性称为过程噪声，只有都考虑到才是完整的卡尔曼滤波模型。

        从理论上讲，过程噪声造成的影响是使得估计的不确定度（p）变大，所以在协方差外推方程中，需要加入过程噪声的影响，因为观测噪声和过程噪声显然不相关，所以直接加上。

        如下所列为恒定状态下：

         对于引例3而言，对于位移外推的不确定度如下：

顾及过程噪声： 

上述中的q为过程噪声的方差。

         对协方差外推方程进行更新后，即为完整的卡尔曼滤波模型。

        3）引例5

迭代结果：

        从迭代结果可知，估计数值和观测值和真值都存在很大的差距，这种差距被称为滞后误差。上述我们在利用匀速的模型来分析加速飞行的飞机时结果也出现过滞后误差。所以，分析其原因，是状态方程和实际变化不符所造成的影响，也即模型误差。

        在本例中，我们可以很容易的将状态外推方程改正，只需在后面加上0.1乘以时间即可，但是在具体的实际问题中，我们可能并不知道正确的空间模型是什么，不能得到严格正确的状态外推方程，只知道当前模型存在滞后误差。此时，就可以通过上文介绍的过程噪声对模型进行改正，将过程噪声的值变大，反映着状态方程的不确定性程度越高。

        上述引例中，将过程噪声方差从0.0001改为0.15。在进行卡尔曼滤波迭代计算，结果如下：

 

四、多维卡尔曼滤波器

        1）多维情况下的状态外推方程

        首先介绍多维情况下的状态外推方程：

        2）多维情况下的协方差外推方程

        多维情况下的协方差外推方程：

 

        3）多维情况下的状态更新方程

        状态更新方程：

 

        4）协方差更新方程

        协方差更新方程：

 给出推导：

简化版更新方程：

 

        5）卡尔曼增益方程

卡尔曼增益方程：

卡尔曼结构示意图：

        6）引例6

            汽车定位例子。Examples (kalmanfilter.net)https://www.kalmanfilter.net/multiExamples.html

        python实现算例：kalman_vehicle_location.py

        7）引例7

               火箭高度示例。

Examples (kalmanfilter.net)https://www.kalmanfilter.net/multiExamples.html

        python实现算例：rocket_altitude.py

 浅薄之见，敬请指正，共勉！

---

资源获取（笔记、代码、论文）：

关注公众号 小堂同学（公众号微信号：gh_d99af1e66e91） 回复：kalman

---