在标准的 Kalman 滤波模型中，总是假设观测噪声和系统噪声均为白噪声，但在实际应用中，符合白噪声特性的情况是很少的，大多数情况下观测噪声或者系统噪声在时间上是相关的，即为有色噪声。此外，有时系统噪声与观测噪声之间是相关的，这些都不符合 Kalman 滤波标准模型的要求。如果忽略噪声的有色性和系统噪声与观测噪声之间的相关性，会引起滤波结果的失真甚至发散。本章将介绍系统噪声与观测噪声相关，以及系统噪声或观测噪声各自在有色情况下的非标准模型，并就此模型提出 Kalman 滤波解决方法，最后通过算例来分析不同方法的对有色噪声处理结果的差异。

系统噪声与观测噪声相关时的 Kalman 滤波

已知状态方程和观测方程为 \[\begin{aligned} \bm{X}(k)&=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{w}(k-1) \tag{6.1.1}\\ \bm{Z}(k)&=\bm{H}_k\bm{X}(k)+\bm{\Delta}(k) \tag{6.1.2} \end{aligned}\] 其中， \[\begin{aligned} E\left[\,\bm{w}(k)\,\right]&=0\ ,\quad \mathrm{Cov}\left[\,\bm{w}(k),\ \bm{w}(j)\,\right]=\bm{D}_w(k)\delta(k-j) \tag{6.1.3}\\ E\left[\,\bm{\Delta}(k)\,\right]&=0\ ,\quad \mathrm{Cov}\left[\,\bm{\Delta}(k),\ \bm{\Delta}(j)\,\right]=\bm{D}_{\Delta}(k)\delta(k-j) \tag{6.1.4} \end{aligned}\] 与标准的 Kalman 滤波模型不同的是，这里的系统噪声 \(\bm{w}(k-1)\) 与观测噪声 \(\bm{\Delta}(k)\) 是相关的，相关性表示为 \[E\left[\,\bm{w}(j)\bm{\Delta}(k)\,\right]=\bm{S}_k\delta\left(\,j-(k-1)\,\right)\quad (\bm{S}_k\neq\bm{0}) \tag{6.1.5}\] 由于 \(\bm{w}(k-1)\) 与 \(\bm{\Delta}(k-1)\) 并没有相关性，所以这并不影响滤波的一步预测，但 \(\bm{w}(k-1)\) 与 \(\bm{\Delta}(k)\) 的相关性将使滤波的测量更新与标准模型下的测量更新不同。下面给出具体的推导过程。

设测量更新为 \[\hat{\bm{X}}(k)=\hat{\bm{X}}(k,\ k-1)+\bm{K}_k\left[\,\bm{Z}(k)-\bm{H}_k\hat{\bm{X}}(k,\ k-1)\,\right] \tag{6.1.6}\] 上式表示 \(\hat{\bm{X}}(k)\) 是观测值 \(\bm{Z}(k)\) 的线性函数，\(\bm{Z}(k)\) 对 \(\hat{\bm{X}}(k,\ k-1)\) 进行更新，但是增益矩阵 \(\bm{K}_k\) 未知。基于式 (6.1.6)，\(\hat{\bm{X}}(k)\) 的估计误差为 \[\begin{aligned} \Delta\hat{\bm{X}}(k)&=\bm{X}(k)-\hat{\bm{X}}(k)\\ &=\bm{X}(k)-\left\{\hat{\bm{X}}(k,\ k-1)+\bm{K}_k\left[\,\bm{Z}(k)-\bm{H}_k\hat{\bm{X}}(k,\ k-1)\,\right]\right\} \end{aligned} \tag{6.1.7}\] 将式 (6.1.2) 代入上式，可得 \[\Delta\hat{\bm{X}}(k)=\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\Delta\hat{\bm{X}}(k,\ k-1)-\bm{K}_k\bm{\Delta}_k \tag{6.1.8}\] \(\hat{\bm{X}}(k)\) 的方差为 \[\begin{aligned} \bm{D}_{\hat{X}}(k)&=E\left[\,\Delta\hat{\bm{X}}(k)\,\Delta\hat{\bm{X}}^{\mathrm{T}}(k)\,\right]\\ &=E\left\{\left[\,\left(\bm{I}{-}\bm{K}_k\bm{H}_k\right)\Delta\hat{\bm{X}}(k,\ k{-}1){-}\bm{K}_k\bm{\Delta}_k\,\right]\right.\\ &\qquad\left.\cdot\left[\,\left(\bm{I}{-}\bm{K}_k\bm{H}_k\right)\Delta\hat{\bm{X}}(k,\ k{-}1){-}\bm{K}_k\bm{\Delta}_k\,\right]^{\mathrm{T}}\right\} \end{aligned} \tag{6.1.9}\] 与标准 Kalmam 滤波不同的是，上式中的 \(\Delta\hat{\bm{X}}(k,\ k-1)\) 与 \(\bm{\Delta}_k\) 存在相关性，相关性为 \[\begin{aligned} E\left[\,\Delta\hat{\bm{X}}(k,\ k-1)\,\bm{\Delta}^{\mathrm{T}}(k)\,\right] &=E\left\{\left[\,\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{w}(k-1)\right.\right.\\ &\qquad\left.\left.-\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1)\,\right]\bm{\Delta}^{\mathrm{T}}(k)\,\right\}\\ &=E\left\{\bm{w}(k-1)\,\bm{\Delta}^{\mathrm{T}}(k)\,\right\}\\ &=\bm{S}_k \end{aligned} \tag{6.1.10}\] 所以，式 (6.1.9) 为 \[\begin{aligned} \bm{D}_{\hat{X}}(k)&=E\left[\,\Delta\hat{\bm{X}}(k)\,\Delta\hat{\bm{X}}^{\mathrm{T}}(k)\,\right]\\ &=\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\bm{D}_{\hat{X}}(k,\ k-1)\,\left(\bm{I}-\bm{K}_k\bm{H}_k\right)^{\mathrm{T}} +\bm{K}_k\bm{D}_{\Delta}(k)\,\bm{K}_k^{\mathrm{T}}\\ &\quad-\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\bm{S}_k\bm{K}_k^{\mathrm{T}}\\ &\quad-\bm{K}_k\bm{S}_k^{\mathrm{T}}\left(\bm{I}-\bm{K}_k\bm{H}_k\right)^{\mathrm{T}} \end{aligned} \tag{6.1.11}\]

现根据最小方差准则来求式 (6.1.6) 中的增益矩阵 \(\bm{K}_k\)。

由于方差矩阵最小与方差矩阵的迹最小等价，所以将 \(\mathrm{trace}\left[\,\bm{D}_{\hat{X}}(k)\,\right]\) 对 \(\bm{K}_k\) 求导，并设导数为零 \[\begin{aligned} \frac{\mathrm{d}\left[\,\mathrm{trace}\left(\bm{D}_{\hat{X}}(k)\right)\,\right]}{\mathrm{d}\,\bm{K}_k} =&\ 2\bm{K}_k\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}}-2\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}} +2\bm{K}_k\bm{D}_{\Delta}(k)\\ &+2\bm{K}_k\bm{H}_k\bm{S}_k+2\bm{K}_k\bm{S}_k^{\mathrm{T}}\bm{H}_k^{\mathrm{T}}-2\bm{S}_k\\ =&\ 0 \end{aligned} \tag{6.1.12}\] 解得 \[\bm{K}_k=\left[\,\bm{S}_k+\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}}\,\right] \left[\,\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}}+\bm{D}_{\Delta}(k)+\bm{H}_k\bm{S}_k+\bm{S}_k^{\mathrm{T}}\bm{H}_k^{\mathrm{T}}\,\right]^{-1} \tag{6.1.13}\] 将式 (6.1.13) 代入式 (6.1.11)，可以推导得到 \[\bm{D}_{\hat{X}}(k)=\bm{D}_{\hat{X}}(k,\ k-1) -\bm{K}_k\left[\,\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}}+\bm{D}_{\Delta}(k)+\bm{H}_k\bm{S}_k+\bm{S}_k^{\mathrm{T}}\bm{H}_k^{\mathrm{T}}\,\right]\bm{K}_k^{\mathrm{T}} \tag{6.1.14}\] 或者 \[\bm{D}_{\hat{X}}(k)=\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\bm{D}_{\hat{X}}(k,\ k-1)-\bm{K}_k\bm{S}_k^{\mathrm{T}} \tag{6.1.15}\] 最后，将 \(\bm{w}(k-1)\) 与 \(\bm{\Delta}(k)\) 相关时的滤波递推公式整理如下： \[\begin{aligned} \hat{\bm{X}}(k,\ k-1)&=\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1) \tag{6.1.16}\\ \bm{D}_{\hat{X}}(k,\ k-1)&=\bm{\Phi}_{k,\ k-1}\bm{D}_{\hat{X}}(k-1)\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}+\bm{D}_w(k-1) \tag{6.1.17}\\ \bm{V}(k,\ k-1)&=\bm{Z}(k)-\bm{H}_k\hat{\bm{X}}(k,\ k-1) \tag{6.1.18}\\ \bm{K}_k&=\left(\bm{S}_k+\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}}\right)\notag\\ &\quad\cdot\left(\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}}+\bm{D}_{\Delta}(k)+\bm{H}_k\bm{S}_k+\bm{S}_k^{\mathrm{T}}\bm{H}_k^{\mathrm{T}}\right)^{-1} \tag{6.1.19}\\ \hat{\bm{X}}(k)&=\hat{\bm{X}}(k,\ k-1)+\bm{K}_k\bm{V}(k,\ k-1) \tag{6.1.20}\\ \bm{D}_{\hat{X}}(k)&=\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\bm{D}_{\hat{X}}(k,\ k-1)\,\left(\bm{I}-\bm{K}_k\bm{H}_k\right)^{\mathrm{T}} \\ &\quad+\bm{K}_k\bm{D}_{\Delta}(k)\,\bm{K}_k^{\mathrm{T}}\notag\\ &\quad-\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\bm{S}_k\bm{K}_k^{\mathrm{T}}\\ &\quad-\bm{K}_k\bm{S}_k^{\mathrm{T}}\left(\bm{I}-\bm{K}_k\bm{H}_k\right)^{\mathrm{T}} \tag{6.1.21} \end{aligned}\] 或者 \[\bm{D}_{\hat{X}}(k)=\bm{D}_{\hat{X}}(k,\ k-1)-\bm{K}_k\left(\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}}+\bm{D}_{\Delta}(k)+\bm{H}_k\bm{S}_k+\bm{H}_k^{\mathrm{T}}\bm{S}_k^{\mathrm{T}}\right)\bm{K}_k^{\mathrm{T}} \tag{6.1.22}\] 和 \[\bm{D}_{\hat{X}}(k)=\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\bm{D}_{\hat{X}}(k,\ k-1)-\bm{K}_k\bm{S}_k^{\mathrm{T}} \tag{6.1.23}\] 与标准的 Kalman 递推公式比较，上式的滤波方差计算中增加了 \(\bm{w}(k-1)\) 与 \(\bm{\Delta}(k)\) 的相关性部分，所以式 (6.1.16) 式 (6.1.23) 是更一般的滤波方差计算公式。当 \(\bm{S}_k=0\) 时，它们就退化为标准的 Kalman 递推公式了。

有色噪声的 Kalman 滤波

在第 1 章中介绍了几种典型的有色噪声，这些典型的有色噪声都能由白噪声激发得到，即可以用成型滤波器来表示有色噪声与白噪声的关系，所以当系统噪声或者观测噪声为有色噪声时，我们也可以考虑用将成型滤波器作为新增加的状态方程，将有色噪声与原来状态一并估计。

系统噪声有色的 Kalman 滤波

设系统的微分方程为 \[\dot{\bm{X}}(t)=\bm{A}(t)\bm{X}(t)+\bm{C}(t)\bm{\eta}(t) \tag{6.2.1}\] 其中，\(\bm{\eta}(t)\) 为有色噪声。有色噪声可由白噪声激发得到 \[\dot{\bm{\eta}}(t)=\bm{A}_{\eta}\bm{\eta}(t)+\bm{e}(t) \tag{6.2.2}\] 解式 (6.2.2) 的微分方程，可以得到 \[\bm{\eta}(t)=\bm{\Phi}_{\eta}\bm{\eta}(t_0)+\bm{w}_{\eta}(t) \tag{6.2.3}\] 式 (6.2.3) 为在 1.10 节中给出成型滤波器，其中 \(\bm{\eta}(t_0)\) 为 \(\bm{\eta}(t)\) 的初始值。从 1.10 节中可知，只要知道微分方程 \(\bm{\eta}(t)\) 的自相关函数，就可以求得如式 (6.2.3) 的成型滤波器。

将式 (6.2.1) 和式 (6.2.2) 联立，可以得到 \[\begin{bmatrix}\dot{\bm{X}}(t)\\ \dot{\bm{\eta}}(t)\end{bmatrix} =\begin{bmatrix}\bm{A}(t) & \bm{C}(t)\\ \bm{0} & \bm{A}_{\eta}\end{bmatrix} \begin{bmatrix}\bm{X}(t)\\ \bm{\eta}(t)\end{bmatrix} +\begin{bmatrix}\bm{0}\\ \bm{I}\end{bmatrix}\bm{e}(t) \tag{6.2.4}\] 若将状态扩展为 \[\bm{X}_E(t)=\begin{bmatrix}\bm{X}(t)\\ \bm{\eta}(t)\end{bmatrix} \tag{6.2.5}\] 并设 \[\bm{A}_E(t)=\begin{bmatrix}\bm{A}(t) & \bm{C}(t)\\ \bm{0} & \bm{A}_{\eta}\end{bmatrix}\ ,\quad \bm{C}_E(t)=\begin{bmatrix}\bm{0}\\ \bm{I}\end{bmatrix} \tag{6.2.6}\] 那么，式 (6.2.4) 就为 \[\dot{\bm{X}}_E(t)=\bm{A}_E(t)\bm{X}_E(t)+\bm{C}_E(t)\bm{e}(t) \tag{6.2.7}\] 通过状态扩展，就将有色噪声的系统转化为了白噪声系统，得到了微分方程的标准模型。解式 (6.2.7) 并将其离散化，即可得到状态方程。与扩展的状态相对应，观测方程为 \[\bm{Z}(k)=\bm{H}_{k,\ E}\bm{X}_E(k)+\bm{\Delta}(k) \tag{6.2.8}\] 其中， \[\bm{H}_{k,\ E}=\left[\begin{array}{ll}\bm{H}_k & \bm{0}\end{array}\right] \tag{6.2.9}\]

从以上过程看，通过扩展状态处理系统有色噪声的关键是得到成型滤波器。得到成型滤波器的方法一般有两种：时间序列分析法和相关函数法。时间序列分析法把有色噪声看成各时刻相关的序列和各时刻出现的白噪声的线性组合，建立自回归滑动平均模型（ARMA\((p,\ q)\)），然后用参数估计的方法估计出模型中的各参数。在相关函数法中，用一个样本时间过程中采集到的数据计算出相关函数，再由自相关函数求出成型滤波器。

观测噪声有色的 Kalman 滤波

当系统噪声有色时，可以考虑采用有色噪声状态扩展的处理方式，但在观测噪声有色的情况下，虽然可以通过扩展状态后得到无噪声输入的等效观测方程，但这样会导致滤波方差奇异从而无法进行滤波的递推。下面首先说明为什么扩展状态会导致滤波方差奇异，然后给出解决观测噪声有色的方法——量测求差法。

\[\dot{\bm{X}}(t)=\bm{A}(t)\bm{X}(t)+\bm{C}(t)\bm{e}(t) \tag{6.2.10}\] 观测方程为 \[\bm{Z}(t)=\bm{H}(t)\bm{X}(t)+\bm{v}(t) \tag{6.2.11}\] 其中，\(\bm{v}(t)\) 为有色噪声，\(\bm{v}(t)\) 可以表示为 \[\dot{\bm{v}}(t)=\bm{H}_v\bm{v}(t)+\bm{\Delta}(t) \tag{6.2.12}\] 其中，\(\bm{\Delta}(t)\) 为零均值白噪声，方差为 \(\bm{D}_{\Delta}(t)\)。

将状态扩展为 \[\bm{X}^{*}(t)=\begin{bmatrix}\bm{X}(t)\\ \bm{v}(t)\end{bmatrix} \tag{6.2.13}\] 那么，状态方程为 \[\begin{bmatrix}\dot{\bm{X}}(t)\\ \dot{\bm{v}}(t)\end{bmatrix} =\begin{bmatrix}\bm{A}(t) & \bm{0}\\ \bm{0} & \bm{H}_v\end{bmatrix} \begin{bmatrix}\bm{X}(t)\\ \bm{v}(t)\end{bmatrix} +\begin{bmatrix}\bm{C}(t) & \bm{0}\\ \bm{0} & \bm{I}\end{bmatrix} \begin{bmatrix}\bm{e}(t)\\ \bm{\Delta}(t)\end{bmatrix} \tag{6.2.14}\] 与扩展的状态对应，观测方程为 \[\bm{Z}(k)=\left[\begin{array}{ll}\bm{H}(k) & \bm{I}\end{array}\right]\begin{bmatrix}\bm{X}(k)\\ \bm{v}(k)\end{bmatrix} \tag{6.2.15}\] 由于没有观测噪声，在滤波递推中观测噪声为零矩阵，即 \(\bm{D}_{\Delta}(k)=0\)，那么测量更新后的滤波方差为 \[\bm{D}_{\hat{X}}(k)=\left[\,\bm{I}-\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}} \left(\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}}\right)^{-1}\bm{H}_k\,\right]\bm{D}_{\hat{X}}(k,\ k-1) \tag{6.2.16}\] 将式 (6.2.16) 左乘 \(\bm{H}_k\) 和右乘 \(\bm{H}_k^{\mathrm{T}}\)，可得到 \[\bm{H}_k\bm{D}_{\hat{X}}(k)\,\bm{H}_k^{\mathrm{T}}=0 \tag{6.2.17}\] 这表明 \(\bm{D}_{\hat{X}}(k)\) 一定为奇异矩阵，从而无法进行滤波递推，这也说明了若观测方程没有噪声是无法实现滤波估计的。为了解决上面的问题，在实际应用中可以将观测噪声方差 \(\bm{D}_{\Delta}(k)\) 设置为相对系统噪声方差较小的数值，以此实现滤波的递推。虽然这样得到的是次优滤波，但却是简单且现实可用的方法。

下面给出观测噪声有色的严密解决方法——“量测求差法”，此方法将观测值在时间上差分，有色噪声部分被抵消，重组后的观测方程的噪声为白噪声，这样就可以直接利用 Kalman 滤波递推公式了。用重组观测值法的优点是不增加状态向量的维数，因为不增加计算量。

已知状态方程为 \[\bm{X}(k)=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{w}(k-1) \tag{6.2.18}\] 其中，\(\bm{w}(k-1)\) 为零矩阵白噪声，方差为 \(\bm{D}_w(k-1)\)。观测方程为 \[\bm{Z}(k)=\bm{H}_k\bm{X}(k)+\bm{v}(k) \tag{6.2.19}\] 设有色噪声 \(\bm{v}(t)\) 的成型滤波器为 \[\bm{v}(k)=\bm{\Phi}'_{k,\ k-1}\bm{v}(k-1)+\bm{\Delta}(k-1) \tag{6.2.20}\] 其中 \(\bm{\Delta}(k)\) 为均值为零的白噪声序列，即 \[E\left[\,\bm{\Delta}(k)\,\right]=0 \tag{6.2.21}\] \[\mathrm{Cov}\left[\,\bm{\Delta}(k),\ \bm{\Delta}(j)\,\right]=\bm{D}_{\Delta}(k)\delta(k-j) \tag{6.2.22}\] 并且 \(\mathrm{Cov}\left(\,\bm{w}(k)\bm{\Delta}(j)\,\right)=0\)。

将式 (6.2.20) 代入式 (5.6.19)

\[\bm{Z}(k)=\bm{H}_k\bm{X}(k)+\bm{\Phi}'_{k,\ k-1}\bm{v}(k-1)+\bm{\Delta}(k-1) \tag{6.2.23}\] 在时刻 \(t_{k-1}\) 的观测方程为 \[\bm{Z}(k-1)=\bm{H}_{k-1}\bm{X}(k-1)+\bm{v}(k-1) \tag{6.2.24}\] 上式左乘 \(\bm{\Phi}'_{k,\ k-1}\) \[\bm{\Phi}'_{k,\ k-1}\bm{Z}(k-1)=\bm{\Phi}'_{k,\ k-1}\bm{H}_{k-1}\bm{X}(k-1)+\bm{\Phi}'_{k,\ k-1}\bm{v}(k-1) \tag{6.2.25}\] 式 (6.2.23) 减去式 (6.2.25)，并令 \[\Delta\bm{Z}(k)=\bm{Z}(k)-\bm{\Phi}'_{k,\ k-1}\bm{Z}(k-1) \tag{6.2.26}\] 有 \[\Delta\bm{Z}(k)=\bm{H}_k\bm{X}(k)-\bm{\Phi}'_{k,\ k-1}\bm{H}_{k-1}\bm{X}(k-1)+\bm{\Delta}(k-1) \tag{6.2.27}\] 将式 (6.2.27) 表示为 \[\Delta\bm{Z}(k)=\left[\begin{array}{ll}\bm{H}_k & -\bm{\Phi}'_{k,\ k-1}\bm{H}_{k-1}\end{array}\right] \begin{bmatrix}\bm{X}(k)\\ \bm{X}(k-1)\end{bmatrix}+\bm{\Delta}(k-1) \tag{6.2.28}\] 相应地，状态方程表示为 \[\begin{bmatrix}\bm{X}(k)\\ \bm{X}(k-1)\end{bmatrix} =\begin{bmatrix}\bm{\Phi}_{k,\ k-1} & \bm{0}\\ \bm{I} & \bm{0}\end{bmatrix} \begin{bmatrix}\bm{X}(k-1)\\ \bm{X}(k-2)\end{bmatrix} +\begin{bmatrix}\bm{w}(k-1)\\ \bm{0}\end{bmatrix} \tag{6.2.29}\] 设 \[\begin{aligned} \bm{X}^{*}(k)&=\begin{bmatrix}\bm{X}(k)\\ \bm{X}(k-1)\end{bmatrix}\\[8pt] \bm{\Phi}^{*}_{k,\ k-1}&=\begin{bmatrix}\bm{\Phi}_{k,\ k-1} & \bm{0}\\ \bm{I} & \bm{0}\end{bmatrix}\\[8pt] \bm{w}^{*}(k-1)&=\begin{bmatrix}\bm{w}(k-1)\\ \bm{0}\end{bmatrix}\\[8pt] \bm{H}_k^{*}&=\left[\begin{array}{ll}\bm{H}_k & -\bm{\Phi}'_{k,\ k-1}\bm{H}_{k-1}\end{array}\right] \end{aligned} \tag{6.2.30}\] 有状态方程 \[\bm{X}^{*}(k)=\bm{\Phi}^{*}_{k,\ k-1}\bm{X}^{*}(k-1)+\bm{w}^{*}(k-1) \tag{6.2.31}\] 和观测方程 \[\Delta\bm{Z}(k)=\bm{H}_k^{*}\bm{X}^{*}(k)+\bm{\Delta}(k-1) \tag{6.2.32}\]

从以上可见，观测值差分将有色噪声消除了，但状态向量的维数从 \(n\) 增加到了 \(2n\)。

与以上的函数模型对应，随机模型为 \[\mathrm{Cov}\left(\,\bm{w}^{*}(k),\ \bm{w}^{*}(j)\,\right)=\bm{D}_{w^{*}}(k)\cdot\delta(k-j)\ ,\quad \bm{D}_{w^{*}}(k)=\begin{bmatrix}\bm{D}_w(k) & \bm{0}\\ \bm{0} & \bm{0}\end{bmatrix} \tag{6.2.33}\] \[\mathrm{Cov}\left[\,\bm{\Delta}(k),\ \bm{\Delta}(j)\,\right]=\bm{D}_{\Delta}(k)\delta(k-j) \tag{6.2.34}\] \[\mathrm{Cov}\left(\,\bm{w}^{*}(k)\bm{\Delta}(j)\,\right)=0 \tag{6.2.35}\] 状态方程 (6.2.31) 和观测方程 (6.2.32) 符合 Kalman 滤波的标准模型，根据 Kalman 滤波递推公式可估计得到 \(\hat{\bm{X}}^{*}(k)\) 和 \(\bm{D}_{\hat{X}^{*}}(k)\)。

时间预测为 \[\hat{\bm{X}}^{*}(k,\ k-1)=\begin{bmatrix}\hat{\bm{X}}(k,\ k-1)\\ \hat{\bm{X}}(k-1)\end{bmatrix} =\begin{bmatrix}\bm{\Phi}_{k,\ k-1} & \bm{0}\\ \bm{I} & \bm{0}\end{bmatrix} \begin{bmatrix}\hat{\bm{X}}(k-1)\\ \hat{\bm{X}}(k-2)\end{bmatrix} \tag{6.2.36}\] 其中 \(\hat{\bm{X}}^{*}(k,\ k-1)\) 中的前 \(n\) 个状态即为 \(\hat{\bm{X}}(k,\ k-1)\)，可将其抽取出来 \[\hat{\bm{X}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1) \tag{6.2.37}\] \(\hat{\bm{X}}^{*}(k,\ k-1)\) 的方差为 \[\bm{D}_{\hat{X}^{*}}(k,\ k-1)=\bm{\Phi}^{*}_{k,\ k-1}\bm{D}_{\hat{X}^{*}}(k-1)\bm{D}_{\hat{X}^{*}}(k-1)\bm{\Phi}_{k,\ k-1}^{*\mathrm{T}}+\bm{D}_{w^{*}}(k-1) \tag{6.2.38}\] \(\bm{D}_{\hat{X}^{*}}(k,\ k-1)\) 为 \(2n\times 2n\) 个元素的方阵，其结构为 \[\bm{D}_{\hat{X}^{*}}(k,\ k-1)=\begin{bmatrix} \bm{D}_{\hat{X}}(k,\ k-1) & \mathrm{Cov}\left(\hat{\bm{X}}(k,\ k-1),\ \hat{\bm{X}}(k-1)\right)\\[6pt] \mathrm{Cov}\left(\hat{\bm{X}}(k-1),\ \hat{\bm{X}}(k,\ k-1)\right) & \bm{D}_{\hat{X}}(k-1) \end{bmatrix} \tag{6.2.39}\] \(\bm{D}_{\hat{X}^{*}}(k,\ k-1)\) 左上角的 \(n\times n\) 的方阵为 \(\hat{\bm{X}}(k,\ k-1)\) 的方差。

将式 (6.2.30) 和式 (6.2.33) 代入式 (6.2.38)，得到 \[\begin{aligned} \bm{D}_{\hat{X}^{*}}(k,\ k-1)=& \begin{bmatrix}\bm{\Phi}_{k,\ k-1} & \bm{0}\\ \bm{I} & \bm{0}\end{bmatrix} \begin{bmatrix}\bm{D}_{\hat{X}}(k-1) & \mathrm{Cov}\left(\hat{\bm{X}}(k),\ \hat{\bm{X}}(k-1)\right)\\[4pt] \mathrm{Cov}\left(\hat{\bm{X}}(k-1),\ \hat{\bm{X}}(k)\right) & \bm{D}_{\hat{X}}(k-2)\end{bmatrix} \begin{bmatrix}\bm{\Phi}_{k,\ k-1} & \bm{0}\\ \bm{I} & \bm{0}\end{bmatrix}^{\mathrm{T}} +\begin{bmatrix}\bm{D}_w(k-1) & \bm{0}\\ \bm{0} & \bm{0}\end{bmatrix}\\ =&\begin{bmatrix} \bm{\Phi}_{k,\ k-1}\bm{D}_{\hat{X}}(k-1)\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}+\bm{D}_w(k-1) & \bm{\Phi}_{k,\ k-1}\bm{D}_{\hat{X}}(k-1)\\[6pt] \bm{D}_{\hat{X}}(k-1)\bm{\Phi}_{k,\ k-1}^{\mathrm{T}} & \bm{D}_{\hat{X}}(k-1) \end{bmatrix} \end{aligned}\] 将 \(\bm{D}_{\hat{X}^{*}}(k,\ k-1)\) 中的左上角的 \(n\times n\) 的方阵抽取出来，即 \[\bm{D}_{\hat{X}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}\bm{D}_{\hat{X}}(k-1)\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}+\bm{D}_w(k-1) \tag{6.2.40}\] 基于扩展状态后的模型，测量更新为 \[\hat{\bm{X}}^{*}(k)=\hat{\bm{X}}^{*}(k,\ k-1)+\bm{K}_k^{*}\left[\,\Delta\bm{Z}(k)-\bm{H}_k^{*}\widetilde{\bm{X}}(k,\ k-1)\,\right] \tag{6.2.41}\] 增益矩阵 \(\bm{K}_k^{*}\) 的维数为 \(2n\times\ell\)，其结构为 \[\bm{K}_k^{*}=\begin{bmatrix}\underset{n\times\ell}{\bm{K}_k}\\ \underset{n\times\ell}{\bm{K}_k'}\end{bmatrix} \tag{6.2.42}\] 其中上半部分的 \(\bm{K}_k\) 即为差分观测值对 \(\hat{\bm{X}}(k)\) 的增益矩阵。采用与求 \(\bm{D}_{\hat{X}}(k,\ k-1)\) 同样的方法，将式 (6.2.30) 和式 (6.2.39) 代入增益矩阵的计算公式，推导后并将上半部分抽取出即可得到 \(\bm{K}_k\)，进而得到 \(\hat{\bm{X}}(k)\)。

\(\hat{\bm{X}}^{*}(k)\) 的方差为 \[\bm{D}_{\hat{X}^{*}}(k)=\left[\,\bm{I}-\bm{K}_k^{*}\bm{H}_k^{*}\,\right]\bm{D}_{\hat{X}^{*}}(k,\ k-1) \tag{6.2.43}\] 与 \(\bm{D}_{\hat{X}^{*}}(k,\ k-1)\) 一样，将 \(\bm{D}_{\hat{X}^{*}}(k)\) 中左上角的 \(n\times n\) 的方阵为取出即为 \(\hat{\bm{X}}(k)\) 的方差 \(\bm{D}_{\hat{X}}(k)\)。

下面不加推导地给出提取结果，并将观测值噪声相关情况下的 Kalman 滤波公式总结如下： \[\begin{aligned} \bm{v}(k)&=\bm{\Phi}'_{k,\ k-1}\bm{v}(k-1)+\bm{\Delta}(k-1) \tag{6.2.44}\\ \Delta\bm{Z}(k)&=\bm{Z}(k)-\bm{\Phi}'_{k,\ k-1}\bm{Z}(k-1) \tag{6.2.45} \end{aligned}\] \[\begin{aligned} \bm{K}_k&=\left[\,\bm{\Phi}_{k,\ k-1}\bm{D}_{\hat{X}}(k-1)\,\widetilde{\bm{H}}_k^{*\mathrm{T}}+\bm{D}_w(k-1)\,\bm{H}_k^{\mathrm{T}}\,\right]\notag\\ &\quad\times\left[\,\widetilde{\bm{H}}_k^{*}\,\bm{D}_{\hat{X}}(k-1)\,\widetilde{\bm{H}}_k^{*\mathrm{T}} +\bm{H}_k\bm{D}_w(k-1)\,\bm{H}_k^{\mathrm{T}}+\bm{D}_{\Delta}(k-1)\,\right]^{-1} \tag{6.2.46}\\ \widetilde{\bm{H}}_k^{*}&=\bm{H}_k\bm{\Phi}_{k,\ k-1}-\bm{\Phi}'_{k,\ k-1}\bm{H}_{k-1} \tag{6.2.47}\\ \hat{\bm{X}}(k)&=\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1)+\bm{K}_k\left[\,\Delta\bm{Z}(k)-\widetilde{\bm{H}}_k^{*}\hat{\bm{X}}(k-1)\,\right] \tag{6.2.48}\\ \bm{D}_{\hat{X}}(k)&=\bm{\Phi}_{k,\ k-1}\bm{D}_{\hat{X}}(k-1)\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}+\bm{D}_w(k-1)\notag\\ &\quad-\bm{K}_k\left[\,\widetilde{\bm{H}}_k^{*\mathrm{T}}\bm{D}_{\hat{X}}(k-1)\bm{\Phi}_{k,\ k-1}+\bm{H}_k\bm{D}_w(k-1)\,\right] \tag{6.2.49} \end{aligned}\] 从以上的递推公式看出，要估计 \(\hat{\bm{X}}(1)\) 必须有观测值 \(\bm{Z}(0)\)，但从 \(t_1\) 时刻才开始有观测值。为了得到 \(\hat{\bm{X}}(1)\)，可以认为 \(\bm{Z}(1)\) 的噪声近似为白噪声，按照常规滤波估计 \(\hat{\bm{X}}(1)\)，在 \(t_2\) 时刻得到 \(\bm{Z}(2)\) 后，开始量测求差的滤波递推来消除有色噪声。

算例分析

例 6.1某质点从 \(s_0\) 处作匀速直线运动，初始位置为 \(s(t_0)=0\,\mathrm{m}\)，以一定的速度匀速运动。同时，运动中受到未知的具有正弦周期的加速度干扰。一台传感器位于 \(s_{\mathrm{str}}=-10\,\mathrm{m}\) 的位置，以 \(\Delta t=0.1\,\mathrm{s}\) 的采样间隔观测质点的距离和速度，观测噪声的方差为 \(\bm{D}_{\Delta}(k)=\begin{bmatrix}\sigma_s^2 & \\ & \sigma_v^2\end{bmatrix}=\begin{bmatrix}1\,\mathrm{m}^2 & \\ & (0.1\,\mathrm{m/s})^2\end{bmatrix}\)。设状态为 \(\bm{X}(t)=\begin{bmatrix}x_1(t)\\ x_2(t)\end{bmatrix}=\begin{bmatrix}s(t)\\ v(t)\end{bmatrix}\)，已知初值为 \(\bm{X}(t_0)=\begin{bmatrix}x(t_0)\\ \dot{x}(t_0)\end{bmatrix}=\begin{bmatrix}0\,\mathrm{m}\\ 10\,\mathrm{m/s}\end{bmatrix}\)。为了比较对系统噪声处理方法不同导致的滤波差异，本例将对此问题进行模拟分析，在模拟分析中，质点受到的未知的加速度干扰为 \[u(t)=\frac{2\pi}{10}\cos\left(\frac{2\pi}{10}t\right)\ \mathrm{m/s}^2 \tag{6.3.1}\] 图 6.1 给出了这个加速度扰动以及加速度扰动对速度和位置的影响。

针对这样的动态系统，本例用三种方法来估计质点的状态：1⃝状态方程不考虑系统加速度扰动；2⃝考虑加速度扰动，但简单地将其视为白噪声；3⃝考虑加速度扰动，并考虑扰动在时间上的相关性，将其视为有色噪声，建立成型滤波器，扩展状态来估计。

(1) 方法一：

不考虑加速度对运动质点速度的扰动，微分方程为 \[\begin{bmatrix}\dot{x}_1(t)\\ \dot{x}_2(t)\end{bmatrix} =\begin{bmatrix}0 & 1\\ 0 & 0\end{bmatrix}\begin{bmatrix}x_1(t)\\ x_2(t)\end{bmatrix} \tag{6.3.2}\] 在此问题中 \[\bm{A}(t)=\begin{bmatrix}0 & 1\\ 0 & 0\end{bmatrix} \tag{6.3.3}\] 根据例 3.7，可得转移矩阵 \[\bm{\Phi}(t_k,\ \tau)=\begin{bmatrix}1 & t_k-\tau\\ 0 & 1\end{bmatrix} \tag{6.3.4}\] 状态方程为 \[\begin{bmatrix}x_1(k)\\ x_2(k)\end{bmatrix} =\begin{bmatrix}1 & \Delta t\\ 0 & 1\end{bmatrix}\begin{bmatrix}x_1(k-1)\\ x_2(k-1)\end{bmatrix} \tag{6.3.5}\] 由于没有噪声干扰，所以系统噪声 \(\bm{D}_w(k-1)=0\)。设对 \(x\) 和 \(v\) 的观测值为 \(\bm{Z}(k)=\left[\begin{array}{ll}Z_s(k) & Z_v(k)\end{array}\right]^{\mathrm{T}}\)，观测方程为 \[\begin{bmatrix}Z_s(k)\\ Z_v(k)\end{bmatrix} =\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\begin{bmatrix}x_1(k)\\ x_2(k)\end{bmatrix} +\begin{bmatrix}\Delta_s(k)\\ \Delta_v(k)\end{bmatrix} \tag{6.3.6}\] 观测噪声方差为 \[\bm{D}_{\Delta}(k)=\begin{bmatrix}\sigma_s^2 & \\ & \sigma_v^2\end{bmatrix} =\begin{bmatrix}1\,\mathrm{m}^2 & \\ & (0.1/\mathrm{s})^2\end{bmatrix} \tag{6.3.7}\]

(2) 方法二：

考虑加速度对速度的扰动，并将其视为白噪声，微分方程为 \[\begin{bmatrix}\dot{x}_1(t)\\ \dot{x}_2(t)\end{bmatrix} =\begin{bmatrix}0 & 1\\ 0 & 0\end{bmatrix}\begin{bmatrix}x_1(t)\\ x_2(t)\end{bmatrix} +\begin{bmatrix}0\\ 1\end{bmatrix}e(t) \tag{6.3.8}\] \[\mathrm{Cov}\left[\,e(t),\ e(\tau)\,\right]=\sigma_e^2\delta(t-\tau) \tag{6.3.9}\] 离散化的状态方程为 \[\begin{bmatrix}x_1(k)\\ x_2(k)\end{bmatrix} =\begin{bmatrix}1 & \Delta t\\ 0 & 1\end{bmatrix}\begin{bmatrix}x_1(k-1)\\ x_2(k-1)\end{bmatrix} +\bm{w}(k-1) \tag{6.3.10}\] 其中，噪声为 \[\bm{w}(k-1)=\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\bm{C}(\tau)e(\tau)\,\mathrm{d}\tau \tag{6.3.11}\] 噪声的方差为 \[\begin{aligned} \bm{D}_w(k-1)&=\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\bm{C}(\tau)\sigma_e^2\bm{C}^{\mathrm{T}}(\tau)\bm{\Phi}^{\mathrm{T}}(t_k,\ \tau)\,\mathrm{d}\tau\\ &=\sigma_e^2\begin{bmatrix}\dfrac{(\Delta t)^3}{3} & \dfrac{(\Delta t)^2}{2}\\[8pt] \dfrac{(\Delta t)^2}{2} & \Delta t\end{bmatrix} \end{aligned} \tag{6.3.12}\] 观测方程和观测噪声模型与方法一相同。

(3) 方法三：

设对速度的扰动为 \(\eta(k)\)，那么 \[\dot{v}(t)=\dot{x}_2(t)=\eta(t) \tag{6.3.13}\] 其中，\(\eta(t)\) 可表示为一阶高斯-马尔可夫过程 \[\dot{\eta}(t)=-\beta\eta(t)+e(t) \tag{6.3.14}\] 和 \[\beta=\frac{1}{\tau} \tag{6.3.15}\] \(\tau\) 为相关时间；\(e(t)\) 为零均值白噪声过程，方差为 \(\sigma_e^2\)。高斯-马尔可夫过程 \(\eta(t)\) 的自相关函数为 \[R_{\eta}(t_1,\ t_2)=\sigma^2e^{-\beta\left|t_1-t_2\right|} \tag{6.3.16}\] 现将状态扩展为 \[\bm{X}(t)=\begin{bmatrix}x_1(t)\\ x_2(t)\\ \eta(t)\end{bmatrix} \tag{6.3.17}\] 状态方程为 \[\begin{aligned} \dot{x}_1(t)&=x_2(t)\\ \dot{x}_2(t)&=\eta(t)\\ \dot{\eta}(t)&=-\beta\eta(t)+e(t) \end{aligned} \tag{6.3.18}\] 状态方程的矩阵形式为 \[\dot{\bm{X}}(t)=\bm{A}\bm{X}(t)+\bm{C}e(t) \tag{6.3.19}\] 其中 \[\bm{A}=\begin{bmatrix}0 & 1 & 0\\ 0 & 0 & 1\\ 0 & 0 & -\beta\end{bmatrix} \tag{6.3.20}\] \[\bm{C}=\begin{bmatrix}0\\ 0\\ 1\end{bmatrix}\] 已知初值 \(\eta(t_0)\)，根据本书 1.10 节的介绍，微分方程 (6.3.14) 的解为 \[\eta(t)=e^{-\beta(t-t_0)}\eta(t_0)+\int_{t_0}^{t}e^{-\beta(t-\tau)}e(\tau)\,\mathrm{d}\tau \tag{6.3.21}\] 设 \(w_{\eta}(t_0)=\displaystyle\int_{t_0}^{t}e^{-\beta(t-\tau)}e(\tau)\,\mathrm{d}\tau\)，上式为 \[\eta(t)=e^{-\beta(t-t_0)}\eta(t_0)+w_{\eta}(t_0) \tag{6.3.22}\] \(w_{\eta}(t_i)\) 白噪声序列，且满足 \(E\left[\,w_{\eta}(t_i)\,\right]=0\)，其方差为 \[\begin{aligned} \sigma_{w_{\eta}}^2(t_i)&=\int_{t_i}^{t}e^{-\beta(t-\tau)}\sigma_e^2e^{-\beta(t-\tau)}\,\mathrm{d}\tau\\ &=\frac{\sigma_e^2}{2\beta}\left(1-e^{-2\beta(t-t_i)}\right) \end{aligned} \tag{6.3.23}\] 式 (6.3.22) 即是状态方程 (6.3.18) 中第三个状态方程的解。为解得 (6.3.18) 中第二个微分方程的解，将式 (6.3.22) 代入 (6.3.18) 中第二个微分方程 \[\dot{x}_2(t)=e^{-\beta(t-t_0)}\eta(t_0)+\int_{t_0}^{t}e^{-\beta(t-\tau)}e(\tau)\,\mathrm{d}\tau \tag{6.3.24}\] 对上式积分 \[\int_{t_0}^{t}\dot{x}_2(s)\,\mathrm{d}s=\int_{t_0}^{t}e^{-\beta(s-t_0)}\eta(t_0)\,\mathrm{d}s +\int_{t_0}^{t}\int_{t_0}^{s}e^{-\beta(s-\tau)}e(\tau)\,\mathrm{d}\tau\mathrm{d}s \tag{6.3.25}\] 得到 \[x_2(t)=x_2(t_0)+\frac{\eta(t_0)}{\beta}\left(1-e^{-\beta(t-t_0)}\right)+w_2(t_0) \tag{6.3.26}\] 其中 \[w_2(t_0)=\int_{t_0}^{t}\int_{t_0}^{s}e^{-\beta(s-\tau)}e(\tau)\,\mathrm{d}\tau\mathrm{d}s \tag{6.3.27}\] 同样，为了求微分方程 (6.3.18) 中第一式的解，将式 (6.3.26) 代入 (6.3.18) 中的第一式，积分并考虑初值，得到 \[x_1(t)=x_1(t_0)+x_2(t_0)(t-t_0)+\frac{\eta(t_0)}{\beta}(t-t_0) +\frac{\eta(t_0)}{\beta^2}\left(e^{-\beta(t-t_0)}-1\right)+w_1(t_0) \tag{6.3.28}\] 其中 \[w_1(t_0)=\int_{t_0}^{t}\int_{t_0}^{u}\int_{t_0}^{s}e^{-\beta(s-\tau)}e(\tau)\,\mathrm{d}\tau\mathrm{d}s\mathrm{d}u \tag{6.3.29}\] \(w_1(t_i)\)、\(w_2(t_i)\) 和 \(w_{\eta}(t_i)\) 的方差和协方差在后面一并给出。

将式 (6.3.28)、式 (6.3.26) 和式 (6.3.22) 联立，并令 \(t=t_k\)，\(t_0=t_{k-1}\)，得到 \[\bm{X}(k)=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{w}(k-1) \tag{6.3.30}\] 其中 \[\bm{\Phi}_{k,\ k-1}=\begin{bmatrix} 1 & (t_k-t_{k-1}) & \dfrac{1}{\beta}(t_k-t_{k-1})+\dfrac{1}{\beta^2}\left(e^{-\beta(t_k-t_{k-1})}-1\right)\\[10pt] 0 & 1 & \dfrac{1}{\beta}\left(1-e^{-\beta(t_k-t_{k-1})}\right)\\[10pt] 0 & 0 & e^{-\beta(t_k-t_{k-1})} \end{bmatrix} \tag{6.3.31}\] \(\bm{w}(k-1)\) 为 \[\bm{w}(k-1)=\begin{bmatrix}w_1(k-1)\\ w_2(k-1)\\ w_{\eta}(k-1)\end{bmatrix} =\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\cdot\bm{C}^{*}\cdot e(\tau)\,\mathrm{d}\tau \tag{6.3.32}\] 将 \(t_{k-1}=\tau\) 代入式 (6.3.31)，即可得到上式中的 \(\bm{\Phi}(t_k,\ \tau)\)。\(\bm{w}(k-1)\) 的方差-协方差矩阵为 \[\bm{D}_w(k-1)=\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\ \bm{C}^{*}\sigma_e^2\bm{C}^{*\,\mathrm{T}}\bm{\Phi}^{\mathrm{T}}(t_k,\ \tau)\,\mathrm{d}\tau \tag{6.3.33}\] 为了更清楚地推导得到 \(\bm{D}_w(k-1)\)，这里设 \[\bm{\Phi}(t_k,\ \tau)=\begin{bmatrix}\varphi_{11} & \varphi_{12} & \varphi_{13}\\ \varphi_{21} & \varphi_{22} & \varphi_{23}\\ \varphi_{31} & \varphi_{32} & \varphi_{33}\end{bmatrix} \tag{6.3.34}\] 将式 (6.3.34) 代入式 (6.3.33) \[\bm{D}_w(k-1)=\sigma_e^2\int_{t_{k-1}}^{t_k}\begin{bmatrix} \varphi_{13}^2 & \varphi_{13}\varphi_{23} & \varphi_{13}\varphi_{33}\\ \varphi_{23}\varphi_{13} & \varphi_{23}^2 & \varphi_{23}\varphi_{33}\\ \varphi_{33}\varphi_{13} & \varphi_{33}\varphi_{23} & \varphi_{33}^2 \end{bmatrix}\mathrm{d}\tau \tag{6.3.35}\] 积分后可得到 \[\bm{D}_w(k-1)=\begin{bmatrix} \sigma_{w_1}^2 & \sigma_{w_1w_2} & \sigma_{w_1w_{\eta}}\\ \sigma_{w_2w_1} & \sigma_{w_2}^2 & \sigma_{w_2w_{\eta}}\\ \sigma_{w_{\eta}w_1} & \sigma_{w_{\eta}w_2} & \sigma_{w_{\eta}}^2 \end{bmatrix} \tag{6.3.36}\] 其中 \[\begin{aligned} \sigma_{w_1}^2&=\sigma_e^2\left(\frac{1}{3\beta^2}\Delta t^3-\frac{1}{\beta^3}\Delta t^2 +\frac{1}{\beta^4}\Delta t\left(1-2e^{-\beta\Delta t}\right)+\frac{1}{2\beta^5}\left(1-e^{-2\beta\Delta t}\right)\right)\\ \sigma_{w_1w_2}&=\sigma_e^2\left(\frac{1}{2\beta^2}\Delta t^2-\frac{1}{\beta^3}\Delta t\left(1-e^{-\beta\Delta t}\right) +\frac{1}{\beta^4}\left(1-e^{-\beta\Delta t}\right)-\frac{1}{2\beta^4}\left(1-e^{-2\beta\Delta t}\right)\right) \end{aligned}\] \[\begin{aligned} \sigma_{w_1w_{\eta}}&=\sigma_e^2\left(\frac{1}{2\beta^3}\left(1-e^{-2\beta\Delta t}\right)-\frac{1}{\beta^2}\Delta t\,e^{-\beta\Delta t}\right)\notag\\[4pt] \sigma_{w_2}^2&=\sigma_e^2\left(\frac{1}{\beta^2}\Delta t-\frac{2}{\beta^3}\left(1-e^{-\beta\Delta t}\right)+\frac{1}{2\beta^3}\left(1-e^{-2\beta\Delta t}\right)\right)\notag\\[4pt] \sigma_{w_2w_{\eta}}&=\sigma_e^2\left(\frac{1}{2\beta^2}\left(1+e^{-2\beta\Delta t}\right)-\frac{1}{\beta^2}e^{-\beta\Delta t}\right)\notag\\[4pt] \sigma_{w_{\eta}}^2&=\frac{\sigma_e^2}{2\beta}\left(1-e^{-2\beta\Delta t}\right) \tag{6.3.37} \end{aligned}\] 上式中，\(\Delta t=t_k-t_{k-1}\)。

基于以上三种方法的模型，进行滤波计算。为了比较三种模型估计结果的差异，分别计算了这三种方法的滤波误差、滤波中误差对滤波误差的包络情况和滤波的 rms。这里的滤波误差是指以真值为参考值，位移估计 \(\hat{x}_1(k)\) 和速度估计 \(\hat{x}_2(k)\) 与真值的差异： \[\begin{aligned} \Delta\hat{x}_1(k)&=\hat{x}_1(k)-x_1(k)\\ \Delta\hat{x}_2(k)&=\hat{x}_2(k)-x_2(k) \end{aligned} \tag{6.3.38}\] 滤波中误差对滤波误差的包络概率为 \[P_{\Delta\hat{x}_i}=P\left(-\sigma_{\hat{x}_i}(k)<\Delta\hat{x}_i(k)<\sigma_{\hat{x}_i}(k)\right)\quad i=1,\ 2 \tag{6.3.39}\] 其中 \(\sigma_{\hat{x}_i}(k)\) 从滤波方差矩阵 \(\bm{D}_{\hat{X}}(k)\) 中提取。滤波中误差给出了滤波误差的置信范围，如果中误差估计正确，滤波中误差应该能够较好地包络滤波误差。在 \(\Delta\hat{x}_i(k)\) 服从正态分布的情况下，\(P_{\Delta\hat{x}_i}\) 应在为 \(68\%\) 左右。

\(\hat{x}_1(k)\) 和 \(\hat{x}_2(k)\) rms 为 \[\mathrm{rms}_{\Delta\hat{x}_i}=\sqrt{\frac{\displaystyle\sum_{k=1}^{m}\Delta\hat{x}_i^2(k)^2}{m}}\ ,\qquad i=1,\ 2 \tag{6.3.40}\] 其中 \(m\) 为采样次数。

图 6.2、图 6.3 和图 6.4 分别给出了以上三种方法的滤波误差和中误差。

从结果可以看出，在方法一中，滤波误差 \(\Delta\hat{x}_i(k)\) 较大，滤波中误差 \(\sigma_{\hat{x}_i}(k)\) 并不能能很好地包络滤波误差 \(\Delta\hat{x}_i(k)\)，这表明方法一输出的方差并不能反映其滤波精度。在方法二中，滤波误差 \(\Delta\hat{x}_i(k)\) 较方法一明显减小，滤波中误差 \(\sigma_{\hat{x}_i}(k)\) 较好地包络了滤波误差 \(\Delta\hat{x}_i(k)\)。在方法三中，滤波误差 \(\Delta\hat{x}_i(k)\) 较方法二相当，但滤波中误差 \(\sigma_{\hat{x}_i}(k)\) 能更好地包络滤波误差 \(\Delta\hat{x}_i(k)\)。需要说明的是，方法二和三的结果都与 \(\sigma_e\) 的取值有关，方法二的结果对 \(\sigma_e\) 的取值比方法三敏感很多，图 6.3 和图 6.4 给出的是在各自方法下 \(\sigma_e\) 取值最好的结果。

\(\mathrm{rms}_{\Delta\hat{x}_i}\) 的计算结果和 \(P_{\Delta\hat{x}_i}\) 的统计结果见表 6.1。\(\mathrm{rms}_{\Delta\hat{x}_i}\) 表征了滤波估计的外符合精度，客观地反映了滤波估计的准确性。在以上三种方法中，方法一的 \(\mathrm{rms}_{\Delta\hat{x}_i}\) 明显大于方法二和方法三。

例 6.2全球定位系统 GNSS 和惯性导航系统 INS 是目前应用最广泛的导航技术。将两者组合起来，可以克服 GNSS 易受到地物遮挡导致定位中断和 INS 定位误差随时间积累的缺陷。在 GNSS/INS 松组合导航中，观测值有：INS 给出的加速度、速度、位移和 GNSS 给出的位移。这里以 GNSS/INS 松组合导航为例，给出 Kalman 滤波模型。为了将问题简化，这里只考虑运动载体在一维坐标系中的情况，设状态为 \[\begin{aligned} \bm{X}_1&=\text{真实位置}\\ \bm{X}_2&=\text{真实速度}\\ \bm{X}_3&=\text{真实加速度} \end{aligned} \tag{6.3.41}\] 观测值有 \[\begin{aligned} Z_1&=\text{INS 的位置观测值}\ , &Z_2&=\text{INS 的速度观测值}\\ Z_3&=\text{INS 的加速度观测}\ , &Z_4&=\text{GNSS 的位置观测值} \end{aligned} \tag{6.3.42}\] 将载体的运动规律描述为 \[\begin{bmatrix}\dot{\bm{X}}_1(t)\\ \dot{\bm{X}}_2(t)\\ \dot{\bm{X}}_3(t)\end{bmatrix} =\begin{bmatrix}0 & 1 & 0\\ 0 & 0 & 1\\ 0 & 0 & -\beta_a\end{bmatrix} \begin{bmatrix}\bm{X}_1(t)\\ \bm{X}_2(t)\\ \bm{X}_3(t)\end{bmatrix} +\begin{bmatrix}0\\ 0\\ 1\end{bmatrix}e_a(t) \tag{6.3.43}\] 上面的状态方程将载体加速度看做一阶高斯-马尔可夫过程，并且 \[E\left[\,e_a(t)e_a(\tau)\,\right]=\sigma_a^2\cdot\delta(t-\tau) \tag{6.3.44}\] 观测方程为 \[\begin{bmatrix}Z_1\\ Z_2\\ Z_3\\ Z_4\end{bmatrix}(k) =\begin{bmatrix}1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1\\ 1 & 0 & 0\end{bmatrix} \begin{bmatrix}\bm{X}_1\\ \bm{X}_2\\ \bm{X}_3\end{bmatrix}(k) +\begin{bmatrix}v_1\\ v_2\\ v_3\\ \Delta\end{bmatrix}(k) \tag{6.3.45}\] 其中，\(\Delta(k)\) 为 GNSS 位移观测值的噪声，为零均值白噪声；由于 INS 的加速度计有偏差，所以导致观测值 \(\left[\begin{array}{lll}Z_1(k) & Z_2(k) & Z_3(k)\end{array}\right]^{\mathrm{T}}\) 都受到偏差的影响从而存在有色噪声 \(\left[\begin{array}{lll}v_1(k) & v_2(k) & v_3(k)\end{array}\right]^{\mathrm{T}}\)，有色噪声的特征为 \[\begin{bmatrix}\dot{v}_1\\ \dot{v}_2\\ \dot{v}_3\end{bmatrix} =\begin{bmatrix}0 & 1 & 0\\ 0 & 0 & 1\\ 0 & 0 & -\beta_b\end{bmatrix} \begin{bmatrix}v_1\\ v_2\\ v_3\end{bmatrix} +\begin{bmatrix}0\\ 0\\ 1\end{bmatrix}e_b(t) \tag{6.3.46}\] 在式 (6.3.46) 的模型中视加速度计偏差 \(v_3(t)\) 为一阶 Gauss-Markov 过程，并且 \[E\left[\,e_b(t)e_b(\tau)\,\right]=\sigma_b^2\cdot\delta(t-\tau) \tag{6.3.47}\] 由于观测值存在有色噪声，现将有色噪声作为状态一并估计，扩展后的状态方程为 \[\begin{bmatrix}\dot{\bm{X}}_1\\ \dot{\bm{X}}_2\\ \dot{\bm{X}}_3\\ \dot{v}_1\\ \dot{v}_2\\ \dot{v}_3\end{bmatrix} =\begin{bmatrix} 0 & 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & -\beta_a & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0 & 0 & -\beta_b \end{bmatrix} \begin{bmatrix}\bm{X}_1\\ \bm{X}_2\\ \bm{X}_3\\ v_1\\ v_2\\ v_3\end{bmatrix} +\begin{bmatrix}0 & 0\\ 0 & 0\\ 1 & 0\\ 0 & 0\\ 0 & 0\\ 0 & 1\end{bmatrix} \begin{bmatrix}e_a\\ e_b\end{bmatrix} \tag{6.3.48}\] 扩展状态后的观测方程为 \[\begin{bmatrix}Z_1(k)\\ Z_2(k)\\ Z_3(k)\\ Z_4(k)\end{bmatrix} =Z_4(k)\begin{bmatrix} 1 & 0 & 0 & 1 & 0 & 0\\ 0 & 1 & 0 & 0 & 1 & 0\\ 0 & 0 & 1 & 0 & 0 & 1\\ 1 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix}\bm{X}_1(k)\\ \bm{X}_2(k)\\ \bm{X}_3(k)\\ v_1(t)\\ v_2(t)\\ v_3(t)\end{bmatrix} +\begin{bmatrix}0\\ 0\\ 0\\ \Delta(k)\end{bmatrix} \tag{6.3.49}\]

观测值噪声的方差矩阵为 \[\bm{D}_{\Delta}(k)=\begin{bmatrix}0 & & &\\ & 0 & &\\ & & 0 &\\ & & & \sigma_{\Delta}^2\end{bmatrix} \tag{6.3.50}\] 显然，除了观测值 \(Z_4(k)\)，其他观测值的噪声方差为零。为了实现以上模型的 Kalman 滤波递推，可以采用量测求差法重组观测值法，根据式 (6.3.44) 式 (6.2.49) 进行滤波递推。

此外，也可以采用次优滤波方法，设观测值噪声的方差矩阵为 \[\bm{D}_{\Delta}(k)=\begin{bmatrix}0^{+} & & &\\ & 0^{+} & &\\ & & 0^{+} &\\ & & & \sigma_{\Delta}^2\end{bmatrix} \tag{6.3.51}\] 式 (6.3.51) 中的 \(0^{+}\) 表示大于零的微小数值，\(0^{+}\) 使方差矩阵 \(\bm{D}_{\Delta}(k)\) 保持正定性，这样就可以采用常规 Kalman 滤波递推公式计算了。