EKF（拓展卡尔曼滤波）学习笔记：

一些参考：

（三十九）通俗易懂理解——卡尔曼滤波与扩展卡尔曼滤波 - 知乎 (zhihu.com)

(50 封私信 / 42 条消息) 如何通俗并尽可能详细地解释卡尔曼滤波？ - 知乎 (zhihu.com)

视觉slam14讲-高博

先预测，在更新，得到融合以后的数据值

在任何含有不确定因素的动态系统里使用卡尔曼滤波，有效利用多个粗糙数据之间的关系

一个测量值告诉我们另一个测量值可能是什么样子。这就是卡尔曼滤波的目的

 用于衡量变量之间的相关性

（1）建模：高斯模型衡量每一个想要知道的状态

（2）通过上一时刻状态来更新新的时刻的状态，考虑状态转移函数，外力作用，同时考虑系统的不确定性

 

（3） 与传感器的数据融合（观测数据），

 通过矩阵H,将单位进行统一，观测数据与测量数据

（4）两个高斯分布的融合，相乘

 得到系统卡尔曼增益。

举例说明，以视觉slam14讲为例

通用slam系统方程：

 

 x代表某一时刻的所有状态，代表该状态下所有的观测

概率论角度来说：似然由观测方程给出，先验由过去所有状态给出，对于KF，由上一时刻状态给出

先验的计算方程如下：

线性状态下的KF方程：

本时刻的先验：

上一时刻的后验：

 不是单纯的数值，而是一个状态分布函数，是一个概率函数，程序的更新，只需要维护一个状态变量，对其进行更新即可。

线性高斯系统：

 

 （1）预测

通过运动方程确定状态的先验分布，有

 有上一时刻的状态，加系统输入得到系统的先验分布。

 另一方面，由观测方程，我们可以计算在某个状态下，应该产生怎样的观测数据：

 后验概率，系统先验乘以系统观测

仅对比概率分布的指数部分，可得更新后的均值和方差分别为：

 总结如下：

 非线性状态下的EKF（拓展卡尔曼滤波）

我们需要把所有非线性公式通过泰勒展开进行线性化

对于预测方程：

 对于观测方程进行线性化有：

 注意，slam中EKF是离散形式存在的，x(k-1)是变量，x(k-1).hat是变量值，计算概率时，带入进去。