卡尔曼滤波原理（四）噪声相关或有色条件下的Kalman滤波

一、系统噪声与量测噪声相关的Kalman滤波

1、模型

函数模型
$$\{\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}\mathrm{E}\left[{\mathbf{W}}_k\right]=0,\mathrm{E}\left[{\mathbf{W}}_k{\mathbf{W}}_j^{\mathrm{T}}\right]={\mathbf{Q}}_k{\delta }_{kj}\\ \mathrm{E}\left[{\mathbf{V}}_k\right]=0,\mathrm{E}\left[{\mathbf{V}}_k{\mathbf{V}}_l^{\mathrm{T}}\right]={\mathbf{R}}_k{\delta }_{kj}\\ \mathrm{E}\left[{\mathbf{W}}_k{\mathbf{V}}_j^{\mathrm{T}}\right]={\mathbf{S}}_k{\delta }_{kj}\end{array}$$

2、通过改写状态方程去相关

多了相关矩阵 ${\mathbf{S}}_k{\delta }_{kj}$，想假设相关就必须有这个相关矩阵。

表示这个矩阵挺困难的，所以实际处理时，即使有一定相关，也直接把它当成不相关处理。

从理论上分析，它们相关，可以通过对状态方程进行改写，处理成不相关形式：
$$\begin{array}{rl}{\mathbf{X}}_k& ={\mathbf{\Phi }}_{k/k-1}{\mathbf{X}}_{k-1}+{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}+{\mathbf{J}}_{k-1}\left({\mathbf{Z}}_{k-1}-{\mathbf{H}}_{k-1}{\mathbf{X}}_{k-1}-{\mathbf{V}}_{k-1}\right)\\ & =\left({\mathbf{\Phi }}_{k/k-1}-{\mathbf{J}}_{k-1}{\mathbf{H}}_{k-1}\right){\mathbf{X}}_{k-1}+{\mathbf{J}}_{k-1}{\mathbf{Z}}_{k-1}+\left({\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}-{\mathbf{J}}_{k-1}{\mathbf{V}}_{k-1}\right)\\ & ={\mathbf{\Phi }}_{k/k-1}^{\ast }{\mathbf{X}}_{k-1}+{\mathbf{J}}_{k-1}{\mathbf{Z}}_{k-1}+{\mathbf{W}}_{k-1}^{\ast }\end{array}$$
绿色部分实质上等于 $0$ ，$J_{k-1}$ 是一个待定系数矩阵。由此得到一个新的状态方程，此状态方程含有输入 ${\mathbf{J}}_{k-1}{\mathbf{Z}}_{k-1}$ ，即把上一时刻的量测当做已知的输入。现在的问题是：系数矩阵 $J_{k-1}$ 取什么值合适？

我们是希望通过对状态方程进行改写，处理成不相关形式，也就是说要找到一个系数矩阵 $J_{k-1}$ 使 $\mathrm{E}\left[{\mathbf{W}}_k^{\ast }{\mathbf{V}}_j^{\mathrm{T}}\right]$ 等于 $0$ ，即：
$$\begin{array}{l}\mathrm{E}\left[{\mathbf{W}}_k^{\ast }\right]=\mathrm{E}\left[{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}-{\mathbf{J}}_{k-1}{\mathbf{V}}_{k-1}\right]=0\\ \mathrm{E}\left[{\mathbf{W}}_k^{\ast }{\left({\mathbf{W}}_j^{\ast }\right)}^{\mathrm{T}}\right]=\left({\mathbf{\Gamma }}_k{\mathbf{Q}}_k{\mathbf{\Gamma }}_k^{\mathrm{T}}+{\mathbf{J}}_k{\mathbf{R}}_k{\mathbf{J}}_k^{\mathrm{T}}-{\mathbf{\Gamma }}_k{\mathbf{S}}_k{\mathbf{J}}_k^{\mathrm{T}}-{\mathbf{J}}_k{\mathbf{S}}_k^{\mathrm{T}}{\mathbf{\Gamma }}_k^{\mathrm{T}}\right){\mathbf{\delta }}_{kj}\\ \mathrm{E}\left[{\mathbf{W}}_k^{\ast }{\mathbf{V}}_j^{\mathrm{T}}\right]=\left({\mathbf{\Gamma }}_k{\mathbf{S}}_k-{\mathbf{J}}_k{\mathbf{R}}_k\right){\delta }_{kj}⟹{\mathbf{J}}_k={\mathbf{\Gamma }}_k{\mathbf{S}}_k{\mathbf{R}}_k^{-1}\end{array}$$
把系数矩阵 $J_{k-1}$ 带回，得到新的系统噪声方差阵：
$$\begin{array}{rl}{\mathbf{Q}}_k^{\ast }& =\mathrm{E}\left[{\mathbf{W}}_k^{\ast }{\left({\mathbf{W}}_j^{\ast }\right)}^{\mathrm{T}}\right]=\left({\mathbf{\Gamma }}_k{\mathbf{Q}}_k{\mathbf{\Gamma }}_k^{\mathrm{T}}-{\mathbf{J}}_k{\mathbf{S}}_k^{\mathrm{T}}{\mathbf{\Gamma }}_k^{\mathrm{T}}\right){\delta }_{kj}\\ & =\left({\mathbf{\Gamma }}_k{\mathbf{Q}}_k{\mathbf{\Gamma }}_k^{\mathrm{T}}-{\mathbf{J}}_k{\mathbf{R}}_k{\mathbf{R}}_k^{-1}{\mathbf{S}}_k^{\mathrm{T}}{\mathbf{\Gamma }}_k^{\mathrm{T}}\right){\mathbf{\delta }}_{kj}=\left({\mathbf{\Gamma }}_k{\mathbf{Q}}_k{\mathbf{\Gamma }}_k^{\mathrm{T}}-{\mathbf{J}}_k{\mathbf{R}}_k{\mathbf{J}}_k^{\mathrm{T}}\right){\delta }_{kj}\end{array}$$

3、噪声相关条件下Kalman滤波公式汇总

新函数模型
$$\{\begin{array}{l}{\mathbf{X}}_k={\mathbf{\Phi }}_{k/k-1}^{\ast }{\mathbf{X}}_{k-1}+{\mathbf{J}}_{k-1}{\mathbf{Z}}_{k-1}+{\mathbf{W}}_{k-1}^{\ast }\\ {\mathbf{Z}}_k={\mathbf{H}}_k{\mathbf{X}}_k+{\mathbf{V}}_k\end{array}$$
其中：
$$\begin{array}{l}{\mathbf{J}}_{k-1}={\mathbf{\Gamma }}_{k-1}{\mathbf{S}}_{k-1}{\mathbf{R}}_{k-1}^{-1}\\ {\mathbf{\Phi }}_{k/k-1}^{\ast }={\mathbf{\Phi }}_{k/k-1}-{\mathbf{J}}_{k-1}{\mathbf{H}}_{k-1}\\ {\mathbf{Q}}_{k-1}^{\ast }={\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}-{\mathbf{J}}_{k-1}{\mathbf{R}}_{k-1}{\mathbf{J}}_{k-1}^{\mathrm{T}}\end{array}$$
新随机模型
$$\{\begin{array}{lc}\mathrm{E}\left[{\mathbf{W}}_k^{\ast }\right]=0,& \mathrm{E}\left[{\mathbf{W}}_k^{\ast }{\left({\mathbf{W}}_j^{\ast }\right)}^{\mathrm{T}}\right]={\mathbf{Q}}_k^{\ast }{\delta }_{kj}\\ \mathrm{E}\left[{\mathbf{V}}_k\right]=0,& \mathrm{E}\left[{\mathbf{V}}_k{\mathbf{V}}_j^{\mathrm{T}}\right]={\mathbf{R}}_k{\delta }_{kj}\\ \mathrm{E}\left[{\mathbf{W}}_k^{\ast }{\mathbf{V}}_j^{\mathrm{T}}\right]=0& \end{array}$$
滤波公式
$$\{\begin{array}{l}{\hat{\mathbf{X}}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}^{\ast }{\hat{\mathbf{X}}}_{k-1}+{\mathbf{J}}_{k-1}{\mathbf{Z}}_{k-1}\\ {\mathbf{P}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}^{\ast }{\mathbf{P}}_{k-1}{\left({\mathbf{\Phi }}_{k/k-1}^{\ast }\right)}^{\mathrm{T}}+{\mathbf{Q}}_{k-1}^{\ast }\\ {\mathbf{K}}_{kk}={\mathbf{P}}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right)}^{-1}\\ {\hat{\mathbf{X}}}_k={\hat{\mathbf{X}}}_{k/k-1}+{\mathbf{K}}_k\left({\mathbf{Z}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k/k-1}\right)\\ {\mathbf{P}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k/k-1}\end{array}$$

二、系统噪声有色时的Kalman滤波

有色噪声含义非常广，并不是任何有色噪声都可以处理，能建模成状态方程形式的有色噪声才能处理。

1、模型

函数模型
$$\{\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}$$
将噪声分成白噪声 $W_{w,k}$ 和有色噪声 $W_{c,k}$ ：
$${\mathbf{\Gamma }}_k{\mathbf{W}}_k=\left[\begin{array}{ll}{\mathbf{\Gamma }}_{w,k}& {\mathbf{\Gamma }}_{c,k}\end{array}\right][\begin{array}{l}{\mathbf{W}}_{w,k}\\ {\mathbf{W}}_{c,k}\end{array}]$$
当然，有色噪声和白噪声之间不相关：
$$\mathrm{E}\left[{\mathbf{W}}_{w,k}{\mathbf{W}}_{w,j}^{\mathrm{T}}\right]={\mathbf{Q}}_{w,k}{\delta }_{kj},\mathrm{E}\left[{\mathbf{W}}_{w,k}{\mathbf{W}}_{c,j}^{\mathrm{T}}\right]=0$$

2、将有色噪声建模成状态方程形式

当前时刻的有色噪声 $W_{c,k}$ 等于上一时刻的有色噪声 $W_{c,k-1}$ ，乘噪声的状态转移矩阵 ${\Pi }_{k/k-1}$ ，再加上一个新的系统白噪声 ${\zeta }_{k-1}$
$${\mathbf{W}}_{c,k}={\mathbf{\Pi }}_{k/k-1}{\mathbf{W}}_{c,k-1}+{\mathbf{\zeta }}_{k-1}$$
状态增广新模型：
$$\{\begin{array}{l}\left[\begin{array}{c}{\mathbf{X}}_k\\ {\mathbf{W}}_{c,k}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{\Phi }}_{k/k-1}& {\mathbf{\Gamma }}_{c,k-1}\\ 0& {\mathbf{\Pi }}_{k/k-1}\end{array}\right]\left[\begin{array}{c}{\mathbf{X}}_{k-1}\\ {\mathbf{W}}_{c,k-1}\end{array}\right]+\left[\begin{array}{cc}{\mathbf{\Gamma }}_{w,k-1}& 0\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}{\mathbf{W}}_{w,k-1}\\ {\mathbf{\zeta }}_{k-1}\end{array}\right]\\ {\mathbf{Z}}_k=\left[\begin{array}{ll}{\mathbf{H}}_k& 0\end{array}\right]\left[\begin{array}{c}{\mathbf{X}}_k\\ {\mathbf{W}}_{c,k}\end{array}\right]+{\mathbf{V}}_k\end{array}$$

3、系统噪声有色时的Kalman滤波公式汇总

新模型
$$\{\begin{array}{l}\left[\begin{array}{c}{\mathbf{X}}_k\\ {\mathbf{W}}_{c,k}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{\Phi }}_{k/k-1}& {\mathbf{\Gamma }}_{c,k-1}\\ 0& {\mathbf{\Pi }}_{k/k-1}\end{array}\right]\left[\begin{array}{c}{\mathbf{X}}_{k-1}\\ {\mathbf{W}}_{c,k-1}\end{array}\right]+\left[\begin{array}{cc}{\mathbf{\Gamma }}_{w,k-1}& 0\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}{\mathbf{W}}_{w,k-1}\\ {\zeta }_{k-1}\end{array}\right]\\ {\mathbf{Z}}_k=\left[\begin{array}{ll}{\mathbf{H}}_k& 0\end{array}\right]\left[\begin{array}{c}{\mathbf{X}}_k\\ {\mathbf{W}}_{c,k}\end{array}\right]+{\mathbf{V}}_k\end{array}$$
若简记：
$${\mathbf{X}}_k^a=\left[\begin{array}{c}{\mathbf{X}}_k\\ {\mathbf{W}}_{c,k}\end{array}\right],{\mathbf{\Phi }}_{k/k-1}^a=\left[\begin{array}{cc}{\mathbf{\Phi }}_{k/k-1}& {\mathbf{\Gamma }}_{c,k-1}\\ 0& {\mathbf{\Pi }}_{k/k-1}\end{array}\right],{\mathbf{\Gamma }}_k^a=\left[\begin{array}{cc}{\mathbf{\Gamma }}_{w,k}& 0\\ 0& \mathbf{I}\end{array}\right],{\mathbf{W}}_k^a=\left[\begin{array}{c}{\mathbf{W}}_{w,k}\\ {\mathbf{\zeta }}_k\end{array}\right],{\mathbf{H}}_k^a=\left[\begin{array}{ll}{\mathbf{H}}_k& 0\end{array}\right]$$
则有：
$$\{\begin{array}{l}{\mathbf{X}}_k^a={\mathbf{\Phi }}_{k/k-1}^a{\mathbf{X}}_{k-1}^a+{\mathbf{\Gamma }}_{k-1}^a{\mathbf{W}}_{k-1}^a\\ {\mathbf{Z}}_k={\mathbf{H}}_k^a{\mathbf{X}}_k^a+{\mathbf{V}}_k\end{array}$$
其中：
$$\{\begin{array}{l}\mathrm{E}\left[{\mathbf{W}}_k^a\right]=0,\mathrm{E}\left[{\mathbf{W}}_k^a{\left({\mathbf{W}}_j^a\right)}^{\mathrm{T}}\right]={\mathbf{Q}}_k^a{\delta }_{kj}=\mathrm{diag}\left({\mathbf{Q}}_{w,k}{\mathbf{Q}}_{c,k}\right){\delta }_{kj}\\ \mathrm{E}\left[{\mathbf{V}}_k\right]=0,\mathrm{E}\left[{\mathbf{V}}_k{\mathbf{V}}_j^{\mathrm{T}}\right]={\mathbf{R}}_k{\mathbf{\delta }}_{kj}\\ \mathrm{E}\left[{\mathbf{W}}_j^a{\mathbf{V}}_k^{\mathrm{T}}\right]=0\end{array}$$
后面直接用Kalman滤波方程就可以了

三、量测噪声有色时的Kalman滤波方程

GNSS量测噪声，考虑当前和上一历元有相关性，可以当做有色噪声处理

函数模型
$$\{\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}$$

1、状态增广法

将量测噪声分成有色的 $V_{c,k}$ 和无色的 $V_{w,k}$ 两部分，$V_{c,k}$ 表示成状态方程：
$$\begin{array}{l}{\mathbf{V}}_k=\left[\begin{array}{l}{\mathbf{\Theta }}_{w,k}{\mathbf{\Theta }}_{c,k}\end{array}\right]\left[\begin{array}{l}{\mathbf{V}}_{w,k}\\ {\mathbf{V}}_{c,k}\end{array}\right]\\ \mathrm{E}\left[{\mathbf{V}}_{w,k}{\mathbf{V}}_{w,j}^{\mathrm{T}}\right]={\mathbf{R}}_{w,k}{\delta }_{kj},\mathrm{E}\left[{\mathbf{V}}_{w,k}{\mathbf{V}}_{c,j}^{\mathrm{T}}\right]=0\\ {\mathbf{V}}_{c,k}={\mathbf{\Psi }}_{c,k/k-1}{\mathbf{V}}_{c,k-1}+{\mathbf{\varsigma }}_{k-1}\mathrm{E}\left[{\mathbf{\varsigma }}_k\right]=0,\mathrm{E}\left[{\mathbf{\varsigma }}_k{\mathbf{\varsigma }}_j^{\mathrm{T}}\right]={\mathbf{R}}_{c,k}{\mathbf{\delta }}_{kj}\end{array}$$
状态增广：
$$\left[\begin{array}{c}{\mathbf{X}}_k\\ {\mathbf{V}}_{c,k-1}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{\Phi }}_{k/k-1}& 0\\ 0& {\mathbf{\Psi }}_{c,k-1/k-2}\end{array}\right]\left[\begin{array}{c}{\mathbf{X}}_{k-1}\\ {\mathbf{V}}_{c,k-2}\end{array}\right]+\left[\begin{array}{cc}{\mathbf{\Gamma }}_{k-1}& 0\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}{\mathbf{W}}_{k-1}\\ {\mathbf{\varsigma }}_{k-2}\end{array}\right]$$
新的量测方程为：
$$\begin{array}{rl}{\mathbf{Z}}_k& ={\mathbf{H}}_k{\mathbf{X}}_k+{\mathbf{\Theta }}_{w,k}{\mathbf{V}}_{w,k}+{\mathbf{\Theta }}_{c,k}\left({\mathbf{\Psi }}_{c,k/k-1}{\mathbf{V}}_{c,k-1}+{\varsigma }_{k-1}\right)\\ & =\left[\begin{array}{ll}{\mathbf{H}}_k& {\mathbf{\Theta }}_{c,k}{\mathbf{\Psi }}_{c,k/k-1}\end{array}\right]\left[\begin{array}{c}{\mathbf{X}}_k\\ {\mathbf{V}}_{c,k-1}\end{array}\right]+\left({\mathbf{\Theta }}_{w,k}{\mathbf{V}}_{w,k}+{\mathbf{\Theta }}_{c,k}{\varsigma }_{k-1}\right)\end{array}$$
若记：
$$\begin{gathered} {\mathbf{X}}_k^a=\left[\begin{array}{c}{\mathbf{X}}_k\\ {\mathbf{V}}_{c,k-1}\end{array}\right],{\mathbf{\Phi }}_{k/k-1}^a=\left[\begin{array}{cc}{\mathbf{\Phi }}_{k/k-1}& 0\\ 0& {\mathbf{\Psi }}_{c,k-1/k-2}\end{array}\right],{\mathbf{\Gamma }}_k^a=\left[\begin{array}{cc}{\mathbf{\Gamma }}_k& 0\\ 0& \mathbf{I}\end{array}\right],{\mathbf{W}}_k^a=\left[\begin{array}{c}{\mathbf{W}}_k\\ {\mathbf{\varsigma }}_{k-1}\end{array}\right] \\ {\mathbf{H}}_k^a=\left[\begin{array}{ll}{\mathbf{H}}_k& {\mathbf{\Theta }}_{c,k}{\mathbf{\Psi }}_{c,k/k-1}\end{array}\right],{\mathbf{V}}_k^a={\mathbf{\Theta }}_{w,k}{\mathbf{V}}_{w,k}+{\mathbf{\Theta }}_{c,k}{\mathbf{\varsigma }}_{k-1} \end{gathered}$$
得到新的函数模型：
$$\{\begin{array}{l}{\mathbf{X}}_k^a={\mathbf{\Phi }}_{k/k-1}^a{\mathbf{X}}_{k-1}^a+{\mathbf{\Gamma }}_{k-1}^a{\mathbf{W}}_{k-1}^a\\ {\mathbf{Z}}_k={\mathbf{H}}_k^a{\mathbf{X}}_k^a+{\mathbf{V}}_k^a\end{array}$$
新的随机模型：
$$\{\begin{array}{l}\mathrm{E}\left[{\mathbf{W}}_k^a\right]=0,\mathrm{E}\left[{\mathbf{W}}_k^a{\left({\mathbf{W}}_j^a\right)}^{\mathrm{T}}\right]={\mathbf{Q}}_k^a{\delta }_{kj}=\mathrm{diag}\left({\mathbf{Q}}_k{\mathbf{R}}_{c,k-1}\right){\delta }_{kj}\\ \mathrm{E}\left[{\mathbf{V}}_k^a\right]=0,\mathrm{E}\left[{\mathbf{V}}_k^a({\mathbf{V}}_j^a{)}^{\mathrm{T}}\right]={\mathbf{R}}_k({\mathbf{Q}}_{\mathbf{w},\mathbf{k}}{\mathbf{R}}_{\mathbf{w},\mathbf{k}}{\mathbf{Q}}_{\mathbf{w},\mathbf{k}}^{\mathbf{T}}+{\mathbf{Q}}_{\mathbf{c},\mathbf{k}}{\mathbf{R}}_{\mathbf{c},\mathbf{k}}{\mathbf{Q}}_{\mathbf{c},\mathbf{k}}^{\mathbf{T}}){\mathbf{\delta }}_{\mathbf{kj}}\\ \mathrm{E}\left[{\mathbf{W}}_j^a{\mathbf{V}}_k^{\mathrm{T}}\right]=\mathrm{diag}\left(0{\mathbf{R}}_{c,k-1}{\mathbf{Q}}_{c,k}^T\right){\delta }_{kj}\end{array}$$
得出的系统噪声和量测噪声相关，之后还需要再去相关。

2、量测求差法

如果前后时刻相关，两时刻间差分消去相关性

$$\{\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}$$

假设 ${\mathbf{\psi }}_{k/k-1}$ 为相邻两时刻噪声转移矩阵，${\mathbf{\xi }}_{k-1}$ 为量测的激励噪白声
$$\begin{gathered} {\mathbf{V}}_k={\mathbf{\psi }}_{k/k-1}{\mathbf{V}}_{k-1}+{\mathbf{\xi }}_{k-1} \\ \mathrm{E}\left[{\mathbf{\xi }}_k\right]=0,\mathrm{E}\left[{\mathbf{\xi }}_k{\mathbf{\xi }}_j^{\mathrm{T}}\right]={\mathbf{R}}_{\xi ,k}{\delta }_{kj} \end{gathered}$$
前后相邻量测求差
$$\begin{array}{rl}{\mathbf{Z}}_k-{\mathbf{\psi }}_{k/k-1}{\mathbf{Z}}_{k-1}& =\left[{\mathbf{H}}_k\left({\mathbf{\Phi }}_{k/k-1}{\mathbf{X}}_{k-1}+{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}\right)+\left({\mathbf{\psi }}_{k\mid k-1}{\mathbf{V}}_{k-1}+{\mathbf{\xi }}_{k-1}\right)\right]-{\mathbf{\psi }}_{k/k-1}\left({\mathbf{H}}_{k-1}{\mathbf{X}}_{k-1}+{\mathbf{V}}_{k-1}\right)\\ & =\left({\mathbf{H}}_k{\mathbf{\Phi }}_{k/k-1}-{\mathbf{\psi }}_{k\mid k-1}{\mathbf{H}}_{k-1}\right){\mathbf{X}}_{k-1}+\left({\mathbf{H}}_k{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}+{\mathbf{\xi }}_{k-1}\right)\end{array}$$
得到新的量测噪声 ${\mathbf{H}}_k{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}+{\mathbf{\xi }}_{k-1}$ 为白噪声

简记
$$\begin{array}{l}{\mathbf{Z}}_k^{\ast }={\mathbf{Z}}_k-{\mathbf{\psi }}_{k/k-1}{\mathbf{Z}}_{k-1}\\ {\mathbf{H}}_k^{\ast }={\mathbf{H}}_k{\mathbf{\Phi }}_{k\mid k-1}-{\mathbf{\psi }}_{k\mid k-1}{\mathbf{H}}_{k-1}\\ {\mathbf{V}}_k^{\ast }={\mathbf{H}}_k{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}+{\mathbf{\xi }}_{k-1}\\ {\mathbf{X}}_k^{\ast }={\mathbf{X}}_{k-1},{\mathbf{\Phi }}_{k/k-1}^{\ast }={\mathbf{\Phi }}_{k-1/k-2},{\mathbf{\Gamma }}_k^{\ast }={\mathbf{\Gamma }}_{k-1},{\mathbf{W}}_k^{\ast }={\mathbf{W}}_{k-1}\end{array}$$
得到新的函数模型：
$$\{\begin{array}{l}{\mathbf{X}}_k^{\ast }={\mathbf{\Phi }}_{k/k-1}^{\ast }{\mathbf{X}}_{k-1}^{\ast }+{\mathbf{\Gamma }}_{k-1}^{\ast }{\mathbf{W}}_{k-1}^{\ast }\\ {\mathbf{Z}}_k^{\ast }={\mathbf{H}}_k^{\ast }{\mathbf{X}}_k^{\ast }+{\mathbf{V}}_k^{\ast }\end{array}$$
新的随机模型：
$$\{\begin{array}{l}\mathrm{E}\left[W_k^{\ast }\right]=0,\mathrm{E}\left[W_k^{\ast }{\left(W_j^{\ast }\right)}^{\top }\right]=Q_{k-1}{\delta }_{kj}\\ \mathrm{E}\left[V_k^{\ast }\right]=0,\mathrm{E}\left[V_k^{\top }{\left(V_j^{\ast }\right)}^{\mathrm{T}}\right]=\left({\mathbf{H}}_k{\Gamma }_{k-1}{Q}_{k-1}{I}_{k-1}^{\top }{H}_k^{\top }+{\mathbf{R}}_{\xi k-1}\right){\delta }_{kj}\\ \mathrm{E}{\left[W_k^{\ast }{\left(V_j^{\ast }\right)}^{\top }\right)}^{\mathrm{T}}]={\mathbf{Q}}_{k-1}{I}_{k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\top }{\delta }_{kj}\end{array}$$
得出的系统噪声和量测噪声相关，之后还需要再去相关。