卡尔曼滤波原理（五）序贯Kalman滤波

量测维数很高，而且能写成很多分量，每一个分量可以看成一个小量测，可以序贯进行量测更新

• 优点是：计算快，数字稳定性更好，我们知道矩阵求逆是和维数的三次方成正比，分成小矩阵求逆快（都分解成一维的，求逆就变为求导）
• 缺点是：如果量测相关且每一时刻变化，每一时刻要做 Cholesky分解。（其实很多时候量测噪声都是定常数矩阵，只要在启动的时候做一次分解就行）

函数模型
$$\{\begin{array}{l}{\mathbf{X}}_k={\mathbf{\Phi }}_{k/k-1}{\mathbf{X}}_{k-1}+{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}\\ {\mathbf{Z}}_k={\mathbf{H}}_k{\mathbf{X}}_k+{\mathbf{V}}_k\end{array}$$
其中
$$\begin{array}{l}\left[\begin{array}{c}{\mathbf{Z}}_k^{(1)}\\ {\mathbf{Z}}_k^{(2)}\\ ⋮\\ {\mathbf{Z}}_k^{(N)}\end{array}\right]=\left[\begin{array}{c}{\mathbf{H}}_k^{(1)}\\ {\mathbf{H}}_k^{(2)}\\ ⋮\\ {\mathbf{H}}_k^{(N)}\end{array}\right]{\mathbf{X}}_k+\left[\begin{array}{c}{\mathbf{V}}_k^{(1)}\\ {\mathbf{V}}_k^{(2)}\\ ⋮\\ {\mathbf{V}}_k^{(N)}\end{array}\right]\\ {\mathbf{R}}_k=\left[\begin{array}{cccc}{\mathbf{R}}_k^{(1)}& & & \\ & {\mathbf{R}}_k^{(2)}& & \\ & & \ddots & \\ & & & {\mathbf{R}}_k^{(N)}\end{array}\right]\end{array}$$
序贯滤波执行框图：

如果历元间量测噪声相关，量测噪声不是对角阵，它总可以作平方根分解（Cholesky分解）
$${\mathbf{R}}_k={\mathbf{L}}_k{\mathbf{L}}_k^{\mathrm{T}}$$
使用 ${\mathbf{L}}_k^{-1}$ 同时左乘量测方程两边, 得：
$${\mathbf{L}}_k^{-1}{\mathbf{Z}}_k={\mathbf{L}}_k^{-1}{\mathbf{H}}_k{\mathbf{X}}_k+{\mathbf{L}}_k^{-1}{\mathbf{V}}_k$$
得到新的量测方程：
$${\mathbf{Z}}_k^{\ast }={\mathbf{H}}_k^{\ast }{\mathbf{X}}_k+{\mathbf{V}}_k^{\ast }$$
其中：
$${\mathbf{Z}}_k^{\ast }={\mathbf{L}}_k^{-1}{\mathbf{Z}}_k,{\mathbf{H}}_k^{\ast }={\mathbf{L}}_k^{-1}{\mathbf{H}}_k,{\mathbf{V}}_k^{\ast }={\mathbf{L}}_k^{-1}{\mathbf{V}}_k$$
从而可以将量测方差阵对角化（单位阵！）：
$$\begin{array}{rl}{\mathbf{R}}_k^{\ast }& =\mathrm{E}\left[{\mathbf{V}}_k^{\ast }{\left({\mathbf{V}}_k^{\ast }\right)}^{\mathrm{T}}\right]=\mathrm{E}\left[\left({\mathbf{L}}_k^{-1}{\mathbf{V}}_k\right){\left({\mathbf{L}}_k^{-1}{\mathbf{V}}_k\right)}^{\mathrm{T}}\right]\\ & ={\mathbf{L}}_k^{-1}\mathrm{E}\left[{\mathbf{V}}_k{\mathbf{V}}_k^{\mathrm{T}}\right]{\left({\mathbf{L}}_k^{-1}\right)}^{\mathrm{T}}={\mathbf{L}}_k^{-1}{\mathbf{R}}_k{\left({\mathbf{L}}_k^{-1}\right)}^{\mathrm{T}}=\mathbf{I}\end{array}$$
满足序贯滤波模型 $\{\begin{array}{l}{\mathbf{X}}_k={\mathbf{\Phi }}_{klk-1}{\mathbf{X}}_{k-1}+{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}\\ {\mathbf{Z}}_k^{\ast }={\mathbf{H}}_k^{\ast }{\mathbf{X}}_k+{\mathbf{V}}_k^{\ast }\end{array}$