基于Python实现扩展卡尔曼滤波算法

1、卡尔曼滤波器定义式的变形
（1）新息过程
$$a_n=y_n-b_n({\hat{x}}_{n|n-1})$$
（2）状态空间
$$X_{n+1}=A_{n+1,n}{X}_n+w_n+{\epsilon }_n$$
$$y_n=B_n{X}_n+v_n$$
（3）表述形式
$${\hat{x}}_{n+1|n}=A_{n+1,n}{\hat{x}}_{n|n}+{\epsilon }_n$$
2、实现扩展卡尔曼滤波器的预备步骤
扩展卡尔曼滤波的基本思想是在没个时间点，围绕最近状态估计结果中的状态空间模型线性化
（1）阶段1 新矩阵的构建
An+1,n=∂an(X)∂X∣X=x^n∣nA_{n+1,n}=\frac{\partial a_n(X)}{\partial X}|_{X=\hat{x}_{n|n}}An+1,n​=∂X∂an​(X)​∣