【卡尔曼滤波】 Kalman Filter 原理详解与公式推导

前言

笔者最近在学习卡尔曼滤波，因个人水平实在有限，花了许久才理解卡尔曼滤波的思想。故现以通俗易懂的语言，总结我对卡尔曼滤波原理的理解

参考资料：本笔记基于Blog
中的实例与图解，并推荐大家阅读这篇文章

引例：什么是卡尔曼滤波？

现在，你开发了一个可以在森林里到处跑的小机器人。
这个机器人带有GPS，精度大约10m
森林里有很多沟壑和悬崖，如果机器人走错了一步，就有可能掉下悬崖。因此我们需要基于已有信息，推测出比GPS更精确的位置

状态矩阵

ps:这个状态只是这个小机器人的一系列基本属性，在这里是位置与速度。在其他场景也可以是机器人的重量、温度等

已知信息：
状态信息：通过估计得到的机器人的位置、速度。电机的预期转速以及机器人的运动方向等
测量信息：GPS测得的位置

但是，两种信息都有各自的不确定性
状态信息可能受到阻力、地面不平等影响；而GPS测量的信息则因其精度不足而有误差（测量误差）。
故我们既不能完全相信状态信息，也不能完全相信预测信息

需要注意，两种信息的不确定性也分大小。若GPS精度特别低，则预测信息的不确定性会更大。若运动过程中受到的阻力等干扰过多，则状态方程也难以很好估计机器人状态

卡尔曼滤波的引入：
卡尔曼滤波可以帮助我们在不确定性中融合信息，提取精确信息
简单来说：卡尔曼滤波算法以确定性为权重/即确定性高的信息权重更大，更相信；确定性低的信息权重较小，更不相信。
他通过两种信息的权重，加权融合信息，得到最优估计

图解卡尔曼滤波

引入

我们引入二维状态变量的概率密度分布图，来形象的表现系统二维状态(如小机器人的位置与速度)的可能性的分布。

概率密度分布图

越亮的区域表示越大的可能性

二维状态变量的均值与方差
两个状态变量均服从高斯分布，故每个变量都有一个均值 μ，共同表示随机分布的中心(最可能的状态)；以及一个方差𝜎^2，协方差表示不确定性(中心向两侧的偏移量)

二维状态变量的相关性
事实上，二维状态变量常常是存在相关性的。就以位置与速度而言，若当下速度很大，则下此位置与这次位置相比，大概率偏差很大。二维状态变量的相关性以协方差的形式体现
以图表示

从宏观上理解卡尔曼融合

概述
卡尔曼滤波基于已知的状态信息（由状态方程预测）和观测信息（如传感器测量），
分别得到先验状态矩阵（靠状态信息的运动状态方程预测）和观测状态矩阵(传感器测量到的直接或间接状态值)
这两者的重叠区域，即融合后的后验状态矩阵。
我们通过密度连乘计算后验矩阵密度分布分析，可以提取出其中心（即均值或最优估计）和边长（即协方差）。
最优估计即后验状态矩阵的中心

先验状态矩阵与观测状态矩阵

后验状态矩阵(最优估计矩阵)

分析后验状态矩阵
卡尔曼滤波融合先验状态矩阵和观测状态矩阵后得到的后验状态矩阵，也即融合得到最优估计

从微观上理解状态矩阵

先验状态矩阵

无外部控制的状态转移(基于状态转移方程

我们用状态转移矩阵Fk来表示状态的转移，并预测这个过程

实际上，状态转移矩阵Fk使得中心(均值)与边长(协方差)均进行变换，体现为状态矩阵的矩阵变换
有外部控制的状态转移

噪声的参与
噪声通常服从均值为 0、方差为 Q 的高斯分布。
因此，噪声不会改变状态矩阵的均值，只会影响方差。即不改变先验状态矩阵的中心，只改变状态矩阵的边长。
因此，在状态转移方程中，无需考虑噪声的影响。但在协方差转移方程中，需加入噪声协方差 Q，以表示噪声增加了系统的不确定性和离散性。

观测状态矩阵

我们有时不直接测量状态值。
如陀螺仪中观测量为角速度，而状态量为角度，前者为后者的低一阶物理量，因此观测矩阵需要将测量值映射为状态值。

观测状态矩阵映射为同物理意义的状态矩阵。实际上也是中心(均值)与边长(协方差)的映射。
注意，观测噪声(传感器精度)同样会使得观测状态矩阵的不确定性和离散性增加。

卡尔曼滤波公式理解

在这里我们只较为浅显的理解卡尔曼滤波的公式，若想了解详细的公式推导，可跳跃至b站DR_CAN
卡尔曼滤波公式

参数说明

预测阶段（Prediction）

更新阶段（update）

公式理解

我们现在来一一对比卡尔曼滤波图解，来理解卡尔曼滤波公式

预测阶段（Prediction）

更新阶段（update）

卡尔曼增益系数与推导过程

我们先以一维高斯分布来分析，具有方差 这里写图片描述 和 μ 的高斯曲线可以用下式表示：

而卡尔曼滤波就是将两个服从高斯分布的概率密度函数(预测概率密度、测量概率密度)相乘融合，并求融合高斯分布的均值(最优估计)与方差

化简可得融合高斯分布的均值与方差：

定义卡尔曼增益系数K
实际上这个K还不是真正的卡尔曼增益系数，卡尔曼增益系数其实包含了Hk-的逆矩阵的作用，也即测量值到预测值的映射作用

改写为高维矩阵形式

公式整合

卡尔曼滤波器框架