扩展卡尔曼滤波算法

输入过程：
$[y_1,y_2,...,y_n]$
已知参数：
非线性状态向量函数=$a_n(x_n)$
非线性测量向量函数=$b_n(x_n)$
过程噪声向量的协方差矩阵=$Q_{w,n}$
测量噪声向量的协方差矩阵=$Q_{v,n}$
计算：n=1,2,3…
$G_n=P_{n,n-1}{B}_n^T[B_n{P}_{n,n-1}{B}_n^T+Q_{v,n}]$
$a_n=y_n-b_n({\hat{x}}_{n|n-1})$
${\hat{x}}_{n|n}={\hat{x}}_{n|n-1}+G_n{a}_n$
${\hat{x}}_{n+1|n}=a_n({\hat{x}}_{n|n})$
$P_{n|n}=P_{n|n-1}-G_n{B}_n{P}_{n|n+1}$
$P_{n+1|n}=A_{n+1,n}{P}_{n|n}{A}_{n+1,n}^T+Q_{w,n}$