卡尔曼滤波原理_卡尔曼滤波原理及公式推导

摘要：

本文作者是北京信息科技大学2019级硕士研究生，在日常学习中研究了卡尔曼滤波的原理及其公式推导。本文首先介绍了卡尔曼滤波的概念和核心思想，再以小车运动状态为例进行方程的推导。

正文：

卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。卡尔曼滤波算法的核心思想是根据当前的仪器测量值与上一时刻的预测值以及误差计算得到当前的最优量，以此预测下一时刻的值。

本篇以小车的运动状态为例，进行卡尔曼滤波方程的推导。

如图所示，在理想状态下，小车不受任何外力作用时保持匀速直线运动。

图1

用运动学公式表达这一状态

但事实上小车并不是按照匀速直线运动，所以我们假设它有一个加速度为a

图2

此时的运动学表达为

接下来引入两种矩阵描述：

表示具有噪声的状态的矩阵：

和协方差矩阵：

把方程改写成矩阵形式：

根据协方差矩阵的性质

可得：

加入噪声：

得到具有噪声的预测方程：

给小车加上传感器可以得到位置和速度的测量值(这个值可能也不是准确的)

由于运动学公式的计算和测量值并都不能确定是完全准确的，所以我们使用卡尔曼滤波算法进行最优估计，首先把方程推广为一般形式，并将Xpredicted和Ymeasurement代入：

将方程进行简化：

至此我们已经得到了卡尔曼滤波的方程：

最后，将它推广为一般形式：

参考文献：

https://www.bilibili.com/video/av75068018?from=search&seid=8021704495196825336

https://www.cnblogs.com/jermmyhsu/p/8195588.html

https://baike.baidu.com/item/协方差矩阵