Kalman filter principle (five) sequential Kalman filter

The measurement dimension is very high and can be written in many components. Each component can be regarded as a small measurement, which can be updated sequentially

• The advantages are: fast calculation and better numerical stability. We know that matrix inversion is proportional to the cube of the dimension, and it is divided into small matrices for fast inversion (all decomposed into one-dimensional, and inversion becomes derivative)
• The disadvantage is that if the measurements are related and change every moment, Cholesky decomposition must be done every moment. (In fact, in many cases, the measurement noise is a constant constant matrix, as long as it is decomposed once at startup)

函数模型
$$\{\begin{array}{l}{\mathbf{X}}_k={Phi}_{k/k-1}{X}_{k-1}+C_{k-1}{\mathbf{W}}_{k-1}\\ {\mathbf{Z}}_k={\mathbf{H}}_k{X}_k+{\mathbf{V}}_k\end{array}$$
其中
$$\begin{array}{l}\left[\begin{array}{c}{\mathbf{Z}}_k^{(1)}\\ Z_k^{(2)}\\ ⋮\\ Z_k^{(N)}\end{array}\right]=\left[\begin{array}{c}{\mathbf{H}}_k^{\left(1\right)}\\ H_k^{\left(2\right)}\\ ⋮\\ H_k^{\left(N\right)}\end{array}\right]{\mathbf{X}}_k+\left[\begin{array}{c}{\mathbf{V}}_k^{\left(1\right)}\\ V_k^{\left(2\right)}\\ ⋮\\ V_k^{\left(N\right)}\end{array}\right]\\ {\mathbf{R}}_k=\left[\begin{array}{cccc}{R}_k^{\left(1\right)}& & & \\ & R_k^{\left(2\right)}& & \\ & & \ddots & \\ & & & R_k^{\left(N\right)}\end{array}\right]\end{array}$$
Sequential filtering execution block diagram:

If the measurement noise is correlated between epochs, the measurement noise is not a diagonal matrix, it can always be decomposed by square root (Cholesky decomposition)
$$R_k={\mathbf{L}}_k{L}_k^{\mathrm{T}}$$
Use $L_k^{-1}$At the same time, multiply both sides of the measurement equation to the left, and get:
$$L_k^{-1}{Z}_k=L_k^{-1}{H}_k{X}_k+L_k^{-1}{V}_k$$
Get a new measurement equation:
$$Z_k^{\ast }={\mathbf{H}}_k^{\ast }{\mathbf{X}}_k+{\mathbf{V}}_k^{\ast }$$
其中：
$${\mathbf{Z}}_k^{\ast }={\mathbf{L}}_k^{-1}{Z}_k,H_k^{\ast }=L_k^{-1}{H}_k,V_k^{\ast }=L_k^{-1}{V}_k$$
The measurement variance matrix can thus be diagonalized (identity matrix!):
$$\begin{array}{rl}{\mathbf{R}}_k^{\ast }& =\mathrm{E}\left[{\mathbf{V}}_k^{\ast }{\left({\mathbf{V}}_k^{\ast }\right)}^{\mathrm{T}}\right]=E\left[\left({\mathbf{L}}_k^{-1}{\mathbf{V}}_k\right){\left({\mathbf{L}}_k^{-1}{V}_k\right)}^{\mathrm{T}}\right]\\ & ={\mathbf{L}}_k^{-1}\mathrm{E}\left[{\mathbf{V}}_k{V}_k^{\mathrm{T}}\right]{\left(L_k^{-1}\right)}^{\mathrm{T}}=L_k^{-1}{\mathbf{R}}_k{\left(L_k^{-1}\right)}^T=\mathbf{I}\end{array}$$
Satisfy the sequential filtering model $\{\begin{array}{l}{\mathbf{X}}_k={Phi}_{klk-}{X}_{k-1}+C_{k-1}{\mathbf{W}}_{k-1}\\ {\mathbf{Z}}_k^{\ast }={\mathbf{H}}_k^{\ast }{X}_k+V_k^{\ast }\end{array}$