卡尔曼滤波与组合导航原理（五）序贯Kalman滤波

函数模型
{ X k = Φ k / k − 1 X k − 1 + Γ k − 1 W k − 1 Z k = H k X k + V k \left\{\begin{array}{l} \boldsymbol{X}_{k}=\boldsymbol{\Phi}_{k / k-1} \boldsymbol{X}_{k-1}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1} \\ \boldsymbol{Z}_{k}=\boldsymbol{H}_{k} \boldsymbol{X}_{k}+\boldsymbol{V}_{k} \end{array}\right. {Xk​=Φk/k−1​Xk−1​+Γk−1​Wk−1​Zk​=Hk​Xk​+Vk​​
其中
[ Z k ( 1 ) Z k ( 2 ) ⋮ Z k ( N ) ] = [ H k ( 1 ) H k ( 2 ) ⋮ H k ( N ) ] X k + [ V k ( 1 ) V k ( 2 ) ⋮ V k ( N ) ] R k = [ R k ( 1 ) R k ( 2 ) ⋱ R k ( N ) ] \begin{array}{l} {\left[\begin{array}{c} \boldsymbol{Z}_{k}^{(1)} \\ \boldsymbol{Z}_{k}^{(2)} \\ \vdots \\ \boldsymbol{Z}_{k}^{(N)} \end{array}\right]=\left[\begin{array}{c} \boldsymbol{H}_{k}^{(1)} \\ \boldsymbol{H}_{k}^{(2)} \\ \vdots \\ \boldsymbol{H}_{k}^{(N)} \end{array}\right] \boldsymbol{X}_{k}+\left[\begin{array}{c} \boldsymbol{V}_{k}^{(1)} \\ \boldsymbol{V}_{k}^{(2)} \\ \vdots \\ \boldsymbol{V}_{k}^{(N)} \end{array}\right]} \\ \boldsymbol{R}_{k}=\left[\begin{array}{cccc} \boldsymbol{R}_{k}^{(1)} & & & \\ & \boldsymbol{R}_{k}^{(2)} & & \\ & & \ddots & \\ & & & \boldsymbol{R}_{k}^{(N)} \end{array}\right] \end{array} ​Zk(1)​Zk(2)​⋮Zk(N)​​ ​= ​Hk(1)​Hk(2)​⋮Hk(N)​​ ​Xk​+ ​Vk(1)​Vk(2)​⋮Vk(N)​​ ​Rk​= ​Rk(1)​​Rk(2)​​⋱​Rk(N)​​ ​​
序贯滤波执行框图：

如果历元间量测噪声相关，量测噪声不是对角阵，它总可以作平方根分解（Cholesky分解）
R k = L k L k T \boldsymbol{R}_{k}=\boldsymbol{L}_{k} \boldsymbol{L}_{k}^{\mathrm{T}} Rk​=Lk​LkT​
使用 L k − 1 \boldsymbol{L}_{k}^{-1} Lk−1​ 同时左乘量测方程两边, 得：
L k − 1 Z k = L k − 1 H k X k + L k − 1 V k \boldsymbol{L}_{k}^{-1} \boldsymbol{Z}_{k}=\boldsymbol{L}_{k}^{-1} \boldsymbol{H}_{k} \boldsymbol{X}_{k}+\boldsymbol{L}_{k}^{-1} \boldsymbol{V}_{k} Lk−1​Zk​=Lk−1​Hk​Xk​+Lk−1​Vk​
得到新的量测方程：
Z k ∗ = H k ∗ X k + V k ∗ \boldsymbol{Z}_{k}^{*}=\boldsymbol{H}_{k}^{*} \boldsymbol{X}_{k}+\boldsymbol{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 \quad \boldsymbol{Z}_{k}^{*}=\boldsymbol{L}_{k}^{-1} \boldsymbol{Z}_{k}, \quad \boldsymbol{H}_{k}^{*}=\boldsymbol{L}_{k}^{-1} \boldsymbol{H}_{k}, \quad \boldsymbol{V}_{k}^{*}=\boldsymbol{L}_{k}^{-1} \boldsymbol{V}_{k} Zk∗​=Lk−1​Zk​,Hk∗​=Lk−1​Hk​,Vk∗​=Lk−1​Vk​
从而可以将量测方差阵对角化（单位阵！）：
R k ∗ = E [ V k ∗ ( V k ∗ ) T ] = E [ ( L k − 1 V k ) ( L k − 1 V k ) T ] = L k − 1 E [ V k V k T ] ( L k − 1 ) T = L k − 1 R k ( L k − 1 ) T = I \begin{aligned} \boldsymbol{R}_{k}^{*} & =\mathrm{E}\left[\boldsymbol{V}_{k}^{*}\left(\boldsymbol{V}_{k}^{*}\right)^{\mathrm{T}}\right]=\mathrm{E}\left[\left(\boldsymbol{L}_{k}^{-1} \boldsymbol{V}_{k}\right)\left(\boldsymbol{L}_{k}^{-1} \boldsymbol{V}_{k}\right)^{\mathrm{T}}\right] \\ & =\boldsymbol{L}_{k}^{-1} \mathrm{E}\left[\boldsymbol{V}_{k} \boldsymbol{V}_{k}^{\mathrm{T}}\right]\left(\boldsymbol{L}_{k}^{-1}\right)^{\mathrm{T}}=\boldsymbol{L}_{k}^{-1} \boldsymbol{R}_{k}\left(\boldsymbol{L}_{k}^{-1}\right)^{\mathrm{T}}=\boldsymbol{I} \end{aligned} Rk∗​​=E[Vk∗​(Vk∗​)T]=E[(Lk−1​Vk​)(Lk−1​Vk​)T]=Lk−1​E[Vk​VkT​](Lk−1​)T=Lk−1​Rk​(Lk−1​)T=I​
满足序贯滤波模型 { X k = Φ k l k − 1 X k − 1 + Γ k − 1 W k − 1 Z k ∗ = H k ∗ X k + V k ∗ \left\{\begin{array}{l}\boldsymbol{X}_{k}=\boldsymbol{\Phi}_{k l k-1} \boldsymbol{X}_{k-1}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1} \\ \boldsymbol{Z}_{k}^{*}=\boldsymbol{H}_{k}^{*} \boldsymbol{X}_{k}+\boldsymbol{V}_{k}^{*}\end{array}\right. {Xk​=Φklk−1​Xk−1​+Γk−1​Wk−1​Zk∗​=Hk∗​Xk​+Vk∗​​