扩展卡尔曼滤波算法
输入过程：
[ y 1 , y 2 , . . . , y n ] [y_1,y_2,...,y_n] [y1,y2,...,yn]
已知参数：
非线性状态向量函数= a n ( x n ) a_n(x_n) an(xn)
非线性测量向量函数= b n ( x n ) b_n(x_n) bn(xn)
过程噪声向量的协方差矩阵= Q w , n Q_{w,n} Qw,n
测量噪声向量的协方差矩阵= Q v , n Q_{v,n} Qv,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 ] G_n = P_{n,n-1} B_n ^T[B_nP_{n,n-1}B_n^T + Q_{v,n}] Gn=Pn,n−1BnT[BnPn,n−1BnT+Qv,n]
a n = y n − b n ( x ^ n ∣ n − 1 ) a_n = y_n-b_n(\hat x_{n|n-1}) an=yn−bn(x^n∣n−1)
x ^ n ∣ n = x ^ n ∣ n − 1 + G n a n \hat x_{n|n}=\hat x_{n|n-1} + G_n a_n x^n∣n=x^n∣n−1+Gnan
x ^ n + 1 ∣ n = a n ( x ^ n ∣ n ) \hat x_{n+1|n} = a_n(\hat x_{n|n}) x^n+1∣n=an(x^n∣n)
P n ∣ n = P n ∣ n − 1 − G n B n P n ∣ n + 1 P_{n|n} = P_{n|n-1}-G_nB_nP_{n|n+1} Pn∣n=Pn∣n−1−GnBnPn∣n+1
P n + 1 ∣ n = A n + 1 , n P n ∣ n A n + 1 , n T + Q w , n P_{n+1|n} = A_{n+1,n}P_{n|n}A_{n+1,n}^T+Q_{w,n} Pn+1∣n=An+1,nPn∣nAn+1,nT+Qw,n