Kalman filter算法介绍及Python实现
- 一、算法思路
- 1.1 Kalman filter简介
- 1.2 算法推导
- 二、Python复现
- 三、参考文章
一、算法思路
1.1 Kalman filter简介
卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
数据滤波是去除噪声还原真实数据的一种数据处理技术，Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。
1.2 算法推导
由于卡尔曼滤波是建立在三种状态下的预测模型，即现在我们现定义三种状态：
- 现实状态(1)，也就是物体真实的轨迹
- 测量状态(2)，也就是传感器返回的数值
- 预测状态(3)，根据能动方程推论状态
其中(1)(2)，大概就是位移随着时间的变化具有一定的线性关系，就是这种抽象关系，可以故且看作一个基于运动方程的线性表出：
为
时刻的状态向量，包括了相机位姿、路标坐标等信息，也可能有速度、朝向等信息（方便后续推断，将
路标并入到状态里）；
为状态转换方程，将
时刻的状态转换至
是控制输入矩阵，将运动测量值
的作用映射到状态向量上；
是预测的高斯噪声，其均值为0，协方差矩阵为
。
其中(3)，用一个在解释卡尔曼滤波时最常用的一维例子『小车追踪』，如下图所示：
由初中物理可得：
写成向量的形式就是：
这时候你把公式等价化：
就会发现动力学的预测公式就是这么个东西
代表由
预测所来的
的值，为了和下面传感器的返回数值
区分就用这个来代替
用『式子(1)-(3)』：
于是:
可以看到，经过预测更新，协方差矩阵P变大了。这是因为状态转换并不完美，而且运动测量值含有噪声，具有较大的不确定性。
预测更新实际上相当于“加法”：将当前状态转换到下一时刻（并增加一定不确定性），再把外界的干扰（运动测量值）叠加上去（又增加了一点不确定性）。
得到了测量值，那么我们就可以对状态向量进行测量更新了，对应的公式为:
可得：
这一串的公式推导真的很麻烦，具体可以看知乎大佬的『卡尔曼滤波：从入门到精通』
二、Python复现
我这里是用的Github上的一个开源项目，具体的应用和实例文本Notebook链接：Asll’s Heywhale
三、参考文章
[1] 知乎：卡尔曼滤波：从入门到精通 [2] Github：pykalman [3] CSDN：关于卡尔曼滤波和卡尔曼平滑关系的理解 [4] CSDN：一文图解卡尔曼滤波(Kalman Filter)
