序贯滤波(sequential Kalman filtering) 一种将高维数据量测更新降低为多个低维数量测更新的方法，有效降低矩阵的求逆计算量(通过把矩阵对角化，将对角拆开分开计算) 特别的对于如下方法求解求解状态增益误差阵即： P k − 1 = P k / k − 1 − 1 + H k T R k − 1 H k = P k / k − 1 − 1 + [ ( H k ( 1 ) ) T . . . ( H k ( N ) ) T ] [ R k ( 1 ) . . . 0 . . . R k ( k ) . . . 0 . . . R k N ] [ H k ( 1 ) . . . H k ( N ) ] = P k − 1 + ( H k ( 1 ) ) T R k ( 1 ) H k ( 1 ) + . . . + ( H k ( N ) ) T R k ( N ) H k ( N ) P_k^{-1}=P_{k/k-1}^{-1}+H_k^TR_k^{-1}H_k\\ =P^{-1}_{k/k-1}+\left[\begin{matrix} (H_k^{(1)})^T&...&(H_k^{(N)})^T\\ \end{matrix}\right]\left[\begin{matrix} R_k^{(1)} &...&0\\ ...&R_k^{(k)}&...\\ 0&...&R_k^{N} \end{matrix}\right]\left[\begin{matrix} H_k^{(1)}\\ ...\\ H_k^{(N)} \end{matrix}\right] \\ =P_k^{-1}+(H_k^{(1)})^TR_k^{(1)}H_k^{(1)}+...+(H_k^{(N)})^TR_k^{(N)}H_k^{(N)} Pk−1=Pk/k−1−1+HkTRk−1Hk=Pk/k−1−1+[(Hk(1))T...(Hk(N))T]⎣⎢⎡Rk(1)...0...Rk(k)...0...RkN⎦⎥⎤⎣⎢⎡Hk(1)...Hk(N)⎦⎥⎤=Pk−1+(Hk(1))TRk(1)Hk(1)+...+(Hk(N))TRk(N)Hk(N) 使用Cholesky分解得到三角矩阵 因为 R k R_k Rk是正定矩阵: R k = L k L k T R_k=L_kL^{T}_k Rk=LkLkT 构造新的测量方程： L k − 1 Z k = L k − 1 H k X k + L k − 1 V k L_k^{-1}Z_k=L_k^{-1}H_kX_k+L_k^{-1}V_k Lk−1Zk=Lk−1HkXk+Lk−1Vk 等价于： Z k ∗ = H k ∗ X k + V k ∗ Z^*_k=H^*_kX_k+V_k^* Zk∗=Hk∗Xk+Vk∗ Z k ∗ = L k − 1 Z k , H k ∗ = L k − 1 H k , V k ∗ = L k − 1 V k Z^*_k=L_k^{-1}Z_k,H^*_k=L_k^{-1}H_k,V_k^*=L_k^{-1}V_k Zk∗=Lk−1Zk,Hk∗=Lk−1Hk,Vk∗=Lk−1Vk 一般kalman滤波过程 序贯滤波过程
利用N次最小二乘估计，进行N次递推最小二乘法， 替换内容： X k / k − 1 → X k ( 0 ) , P k / k − 1 → P k ( 0 ) X_{k/k-1}\rightarrow X_k^{(0)},P_{k/k-1}\rightarrow P_{k}^{(0)} Xk/k−1→Xk(0),Pk/k−1→Pk(0) { K k ( 1 ) = P k ( 0 ) ( H k ( 0 ) ) T [ H k ( N ) P k ( 0 ) ( H k ( 1 ) ) T + R k ( 1 ) ] − 1 X ^ k ( 1 ) = ( I − K k H k ( 1 ) ) X ^ k ( 0 ) + K k Z k P k ( 1 ) = ( I − K k ( 1 ) H k ( 1 ) ) P k ( 0 ) . . . { K k ( N ) = P k ( N − 1 ) ( H k ( N − 1 ) ) T [ H k ( N ) P k ( N − 1 ) ( H k ( N ) ) T + R k ( N ) ] − 1 X ^ k ( N ) = ( I − K k H k ( N ) ) X ^ k ( N − 1 ) + K k Z k P k ( N ) = ( I − K k ( N ) H k ( N ) ) P k ( N − 1 ) \begin{cases} K_k^{(1)}=P_k^{(0)}(H_k^{(0)})^T[H_k^{(N)}P_k^{(0)}(H_k^{(1)})^T+R_k^{(1)}]^{-1}\\ \hat X_k^{(1)}=(I-K^{}_kH^{(1)}_k)\hat X_{k}^{(0)}+K_kZ_k\\ P_k^{(1)}=(I-K^{(1)}_kH^{(1)}_k)P_k^{(0)}\\ \end{cases} \\ ...\\ \begin{cases} K_k^{(N)}=P_k^{(N-1)}(H_k^{(N-1)})^T[H_k^{(N)}P_k^{(N-1)}(H_k^{(N)})^T+R_k^{(N)}]^{-1}\\ \hat X_k^{(N)}=(I-K^{}_kH^{(N)}_k)\hat X_{k}^{(N-1)}+K_kZ_k\\ P_k^{(N)}=(I-K^{(N)}_kH^{(N)}_k)P_k^{(N-1)}\\ \end{cases} ⎩⎪⎨⎪⎧Kk(1)=Pk(0)(Hk(0))T[Hk(N)Pk(0)(Hk(1))T+Rk(1)]−1X^k(1)=(I−KkHk(1))X^k(0)+KkZkPk(1)=(I−Kk(1)Hk(1))Pk(0)...⎩⎪⎨⎪⎧Kk(N)=Pk(N−1)(Hk(N−1))T[Hk(N)Pk(N−1)(Hk(N))T+Rk(N)]−1X^k(N)=(I−KkHk(N))X^k(N−1)+KkZkPk(N)=(I−Kk(N)Hk(N))Pk(N−1) X ^ k ( N ) → X ^ k , P k ( N ) → P k \hat X_k^{(N)}\rightarrow \hat X_k,P_{k}^{(N)}\rightarrow P_{k} X^k(N)→X^k,Pk(N)→Pk
