扩展卡尔曼滤波（EKF）

Extended Kalman Filter

有一个挺好的讲解+运用，但是只是仿真，里面说到了小车角度和运动预测等等的关系，能根据这篇文章设计运动方程和预测方程：扩展卡尔曼滤波EKF与多传感器融合_扩展卡尔曼滤波是如何融合多传感器的-CSDN博客

主要运用

泰勒展开+卡尔曼

主要思想

将非线性的运动方程，通过泰勒展开，近似为若干个线性方程的叠加， 并对每个方程进行滤波，最后再叠加。

基本步骤

1. 收到第一个测量值，对状态x进行初始化。
2. 预测未来
3. 修正当下

EKF向UKF过渡的必要性

EKF经过泰勒展开后，所使用的参数系统的实际参数已经不一样了，它只是一个近似。所以理论上EKF是不像线性系统中用KF时那样能够得到最优解的。如果继续展开，二阶、三阶、四阶的EKF也只是减少了误差，无法对任意非线性系统进行最优估计，且阶数变多带来的计算量会猛增。

EKF的这些缺陷是它的工作原理造成的，所以即使扩大阶数，也无法从根本上消除由于线性化造成的估计误差。

对于同一个非线性系统，EKF的展开式近似会对系统造成一个固定的偏差，若对某一步近似做多次尝试，再求平均，这个误差仍然消除不了。（类似于系统误差）

EKF_classic 制作

从EKF_demo改进而来，用自己喜欢的字母和注释，弄一个典型的EKF，保存在500_NUAA/520_research field/KF_class里面

符号定义：

T	采样率
T	时间序列
PQR	略
X	真值（X_i）（X_ideal）
X_	未滤波的值，理解成里程计or INS直接计算的值
X_ekf	EKF滤波后的值
Z	观测值
Z_hat	每一步的观测预测值

状态量为三维，模拟三维位置，观测量也是三维，构造非线性观测方程：