输入过程：
 $[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}$