本章首先介绍观测值逐次更新的 Kalman 滤波和扩展的 Kalman 滤波，它们可以在一定程度上减小线性化带来的模型误差，并提高计算效率。接着介绍信息滤波，它解决了初始值方差无穷大无法启动滤波的问题。然后，本章介绍的自适应的 Kalman 滤波能够有效地克服数学模型与现实不符造成的发散。最后，本章给出分解滤波算法，包括平方根滤波、UDU 滤波和平方根信息滤波，它们可以有效地抑制由于计算误差导致的滤波发散。

观测值逐次更新的 Kalman 滤波

观测值逐次更新的 Kalman 滤波对 Kalman 滤波基础方程测量更新部分做出改进，它用观测值逐个对状态 \(\bm{X}(k)\) 进行更新，每一次测量更新的新息都是一个标量，增益矩阵中矩阵的求逆运算也转化为对标量求倒数，不仅提高了计算效率，而且减小了计算误差，保证了数值计算的稳定性。此外，如果观测方程是非线性的，观测值逐次更新还起到了迭代的作用，在一定程度上减小了线性化带来的模型误差。

观测值相互独立时的逐次更新法

观测值逐次更新的 Kalman 滤波的时间预测与第 4 章介绍的步骤一样，为 \[\hat{\bm{X}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1) \tag{5.1.1}\] \[\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{5.1.2}\] 在得到时间预测 \(\hat{\bm{X}}(k,\ k-1)\) 后，对测量更新部分的算法做出了改进。

设观测方程为 \[\bm{Z}(k)=\bm{H}_k\bm{X}(k)+\bm{\Delta}(k) \tag{5.1.3}\] 其中观测值向量 \[\bm{Z}(k)=\left[\begin{array}{llll}Z_1(k) & \cdots Z_j(k) & \cdots & Z_{\ell}(k)\end{array}\right] \tag{5.1.4}\] 观测矩阵为 \[\bm{H}_k=\begin{bmatrix}\bm{h}_1(k)\\ \vdots\\ \bm{h}_j(k)\\ \vdots\\ \bm{h}_{\ell}(k)\end{bmatrix} \tag{5.1.5}\] 如果观测值是相互独立的，那么量测噪声方差阵为对角矩阵。设观测值向量的噪声方差阵为 \[\bm{D}_{\Delta}(k)=\mathrm{diag}\left[\begin{array}{lllll}d_1(k) & \cdots & d_j(k) & \cdots & d_{\ell}(k)\end{array}\right] \tag{5.1.6}\] 其中，第 1 个观测值的观测方程为 \[\underset{1\times 1}{Z_1(k)}=\underset{1\times n}{\bm{h}_1(k)}\ \underset{n\times 1}{\bm{X}(k)}+\underset{1\times 1}{\Delta_1(k)} \tag{5.1.7}\] 相应的观测噪声方差为 \(d_1(k)\)，那么 \(Z_1(k)\) 对时间预测的更新为 \[V_1(k,\ k-1)=Z_1(k)-\bm{h}_1(k)\hat{\bm{X}}(k,\ k-1) \tag{5.1.8}\] \[\bm{K}_k^{[1]}=\frac{\bm{D}_{\hat{X}}(k,\ k-1)\bm{h}_1(k)^{\mathrm{T}}} {\bm{h}_1(k)\bm{D}_{\hat{X}}(k,\ k-1)\bm{h}_1(k)^{\mathrm{T}}+d_1(k)} \tag{5.1.9}\] \[\hat{\bm{X}}^{[1]}(k)=\hat{\bm{X}}(k,\ k-1)+\bm{K}_k^{[1]}V_1(k,\ k-1) \tag{5.1.10}\] \[\bm{D}_{\hat{X}}^{[1]}(k)=\left(\bm{I}-\bm{K}_k^{[1]}\bm{h}_1(k)\right)\bm{D}_{\hat{X}}(k,\ k-1) \tag{5.1.11}\] 其中，\(V_1(k,\ k-1)\) 是观测值 \(Z_1(k)\) 的新息，为标量；\(\bm{K}_k^{[1]}\) 为观测值 \(Z_1(k)\) 的增益矩阵，其中的 \(\bm{h}_1(k)\bm{D}_{\hat{X}}(k,\ k-1)\bm{h}_1(k)^{\mathrm{T}}+d_1(k)\) 也为标量，所以这里矩阵求逆成为对其求倒数的运算；\(\hat{\bm{X}}^{[1]}(k)\) 为 \(Z_1(k)\) 对预测状态向量 \(\hat{\bm{X}}(k,\ k-1)\) 的更新；\(\bm{D}_{\hat{X}}^{[1]}(k)\) 为 \(\hat{\bm{X}}^{[1]}(k)\) 的方差矩阵。

第 2 个观测值的观测方程为 \[Z_2(k)=\bm{h}_2(k)\bm{X}(k)+\Delta_2(k) \tag{5.1.12}\] 其方差为 \(d_2(k)\) \(Z_2(k)\)，\(\hat{\bm{X}}^{[1]}(k)\) 再次进行 \(\hat{\bm{X}}^{[2]}(k)\) 和 \(\bm{D}_{\hat{X}}^{[2]}(k)\)。如此逐一进行下去，第 \(j\) 个观测值 \(Z_j(k)\) 对状态 \(\hat{\bm{X}}^{[j-1]}(k)\) 的更新为 \[V_j(k,\ k-1)=Z_j(k)-\bm{h}_j(k)\hat{\bm{X}}^{[j-1]}(k) \tag{5.1.13}\] \[\bm{K}_k^{[j]}=\frac{\bm{D}_{\hat{X}}^{[j-1]}(k)\bm{h}_j(k)^{\mathrm{T}}} {\bm{h}_j(k)\bm{D}_{\hat{X}}^{[j-1]}\bm{h}_j(k)^{\mathrm{T}}+d_j(k)} \tag{5.1.14}\] \[\hat{\bm{X}}^{[j]}(k)=\hat{\bm{X}}^{[j-1]}(k)+\bm{K}_k^{[j]}V_j(k,\ k-1) \tag{5.1.15}\] \[\bm{D}_{\hat{X}}^{[j]}(k)=\left(\bm{I}-\bm{K}_k^{[j]}\bm{h}_j(k)\right)\bm{D}_{\hat{X}}^{[j-1]}(k) \tag{5.1.16}\] 在第 1 个观测值（\(j=1\)）进行更新时 \[\hat{\bm{X}}^{[0]}(k)=\hat{\bm{X}}(k,\ k-1) \tag{5.1.17}\] \[\bm{D}_{\hat{X}}^{[0]}(k)=\bm{D}_{\hat{X}}(k,\ k-1) \tag{5.1.18}\] 重复以上的计算，直到观测向量 \(\bm{Z}(k)\) 中的最后一个观测值 \(Z_{\ell}(k)\) 更新完毕，得到 \(\hat{\bm{X}}^{[\ell]}(k)\) 和 \(\bm{D}_{\hat{X}}^{[\ell]}(k)\)。\(\hat{\bm{X}}^{[\ell]}(k)\) 和 \(\bm{D}_{\hat{X}}^{[\ell]}(k)\) 即为在 \(k\) 时刻所有观测值对 \(\hat{\bm{X}}(k,\ k-1)\) 的测量更新，即 \[\hat{\bm{X}}(k)=\hat{\bm{X}}^{[\ell]}(k) \tag{5.1.19}\] \[\bm{D}_{\hat{X}}(k)=\bm{D}_{\hat{X}}^{[\ell]}(k) \tag{5.1.20}\]

观测值相关时的逐次更新法

如果观测值向量 \(\bm{Z}(k)\) 中的各个观测值是相关的，那么观测噪声方差矩阵 \(\bm{D}_{\Delta}(k)\) 就不是对角矩阵了，这时需要对 \(\bm{Z}(k)\) 作线性变换，使变换后的观测值方差矩阵成为对角矩阵，这意味着线性变换后的观测值不再相关，接下来就可以按照上面的方法进行观测值对状态的逐次更新了。

设观测方程为 \[\bm{Z}(k)=\bm{H}_k\bm{X}(k)+\bm{\Delta}(k) \tag{5.1.21}\] 观测方差矩阵为 \(\bm{D}_{\Delta}(k)\)，\(\bm{D}_{\Delta}(k)\) 为非对角矩阵，即观测值相互相关。

由附录 A.7 可知，一个对称正定矩阵可以唯一地分解为下三角阵及其转置矩阵的乘积，这样的矩阵分解也称为 Cholesky 分解。在这里对 \(\bm{D}_{\Delta}(k)\) 进行 Cholesky 分解，得到 \[\bm{D}_{\Delta}(k)=\bm{L}\bm{L}^{\mathrm{T}} \tag{5.1.22}\] 这样的分解也称为矩阵的平方根分解。将矩阵 \(\bm{L}^{-1}\) 左乘式 (5.1.21) \[\bm{L}^{-1}\bm{Z}(k)=\bm{L}^{-1}\bm{H}_k\bm{X}(k)+\bm{L}^{-1}\bm{\Delta}(k) \tag{5.1.23}\] 并设 \[\bm{Y}(k)=\bm{L}^{-1}\bm{Z}(k) \tag{5.1.24}\] \[\bm{H}_k'=\bm{L}^{-1}\bm{H}_k \tag{5.1.25}\] \[\bm{\Delta}'(k)=\bm{L}^{-1}\bm{\Delta}(k) \tag{5.1.26}\] 那么式 (5.1.23) 为 \[\bm{Y}(k)=\bm{H}_k'\bm{X}(k)+\bm{\Delta}'(k) \tag{5.1.27}\] 视 \(\bm{Y}(k)\) 为新的观测值，其观测噪声为 \(\bm{\Delta}'(k)\)，根据误差传播定理可以得到 \(\bm{\Delta}'(k)\) 的方差为 \[\bm{D}_{\Delta}'(k)=\bm{L}^{-1}\bm{D}_{\Delta}(k)\left(\bm{L}^{-1}\right)^{\mathrm{T}} \tag{5.1.28}\] 由式 (5.1.22) 可知 \(\bm{D}_{\Delta}'(k)\) 为单位矩阵，即 \[\bm{D}_{\Delta}'(k)=\bm{I} \tag{5.1.29}\] 这意味着新的观测值向量 \(\bm{Y}(k)\) 中的观测值不仅相互独立，而且其方差均为 \(1\)，这个过程也称为对观测值 \(\bm{Z}(k)\) 的“标准化”，标准化后的观测值为 \(\bm{Y}(k)\)。

现将观测值相关情况逐次更新的计算步骤总结如下：

(1) 对 \(\bm{D}_{\Delta}(k)\) 进行 Cholesky 分解，得到下三角矩阵 \(\bm{L}\)；

(2) 将 \(\bm{L}^{-1}\) 左乘原来的观测方程，得到新的观测方程（线性变换）： \[\bm{Y}(k)=\bm{H}_k\bm{X}(k)+\bm{\Delta}'(k)\]

(3) 做变量替换 \[\begin{aligned} \bm{Y}(k)&\rightarrow\bm{Z}(k)\\ \bm{H}_k'&\rightarrow\bm{H}_k\\ \bm{\Delta}'(k)&\rightarrow\bm{\Delta}(k)\\ \bm{D}_{\Delta}'(k)&=\bm{D}_{\Delta}(k) \end{aligned}\] 根据式 (5.1.13)～式 (5.1.16) 逐次地对状态进行更新。

由于线性变换后的观测值方差均为 \(1\)，所以对增益矩阵的计算为 \[\bm{K}_k^{[j]}=\frac{\bm{D}_{\hat{X}}^{[j-1]}(k)\bm{h}_j(k)^{\mathrm{T}}} {\left(\bm{h}_j(k)\bm{D}_{\hat{X}}^{[j-1]}\bm{h}_j(k)^{\mathrm{T}}+1\right)} \tag{5.1.30}\]

扩展的 Kalman 滤波

Kalman 滤波是基于线性模型的估计，但在实际应用中，描述运动状态的微分方程和观测方程大多是非线性的。在第 3 章中，我们学习了如何将状态方程和观测方程线性化得到线性模型。通常情况下，微分方程将事先给定的参考轨迹 \(\bm{X}_{ref}(t)\) 作为近似值并进行线性化，如图 5.1(a) 所示。如果参考轨迹与实际估计的偏差 \(\Delta\bm{X}(t)\) 较大时，将会带来较大的模型误差。为了减小线性化带来的模型误差，扩展的 Kalman 滤波（EKF）在 \(t_{k-1}\) 到 \(t_k\) 时间段预测中，将 \(\hat{\bm{X}}(k-1)\) 作为近似值线性化微分方程；在 \(t_k\) 的测量更新时，将 \(\hat{\bm{X}}(k,\ k-1)\) 作为近似值线性化观测方程，测量更新得 \(\hat{\bm{X}}(k)\)。因此，滤波模型线性化的近似值是随时间不断更新的估计值，而不是事先给出的参考值，如图 5.1(b) 所示。

扩展的 Kalman 滤波

设非线性连续时间系统的状态方程为 \[\dot{\bm{X}}(t)=\bm{g}\left[\,\bm{X}(t),\ \bm{u}(t),\ \bm{e}(t)\,\right] \tag{5.2.1}\] 首先将式 (5.2.1) 线性化。在 \((t_{k-1},\ t_k)\) 时间段的预测中，\(\hat{\bm{X}}(k-1)\) 为近似值，线性化后得到 \[\dot{\bm{X}}(t)=\bm{A}(t)\bm{X}(t)+\bm{G}(t)+\bm{C}(t)\bm{e}(t) \tag{5.2.2}\] 其中 \[\begin{aligned} \bm{A}(t)&=\left[\frac{\partial\bm{g}}{\partial\bm{X}(t)}\right]_{\bm{X}(t)=\hat{\bm{X}}(k-1)}\\[8pt] \bm{C}(t)&=\left[\frac{\partial\bm{g}}{\partial\bm{e}(t)}\right]_{\bm{X}(t)=\hat{\bm{X}}(k-1)}\\[8pt] \bm{G}(t)&=\bm{g}\left[\,\hat{\bm{X}}(k-1)\quad \bm{u}(t)\quad 0\,\right]-\bm{A}(t)\hat{\bm{X}}(k-1) \end{aligned} \tag{5.2.3}\]

再解式 (5.2.2) 的微分方程，得到离散化后的状态方程 \[\bm{X}(k)=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{\Omega}(k-1)+\bm{w}(k-1) \tag{5.2.4}\] 其中 \[\bm{\Omega}(k-1)=\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\bm{G}(\tau)\,\mathrm{d}\tau\] 基于式 (5.2.4) 状态方程，时间预测为 \[\hat{\bm{X}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1)+\bm{\Omega}(k-1) \tag{5.2.5}\] \(\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{\Phi}_{k,\ k-1}^{\mathrm{T}}+\bm{D}_{w(k-1)} \tag{5.2.6}\] 观测方程为 \[\bm{Z}(k)=\bm{F}\left[\,\bm{X}(k)\,\right]+\bm{\Delta}(k) \tag{5.2.7}\] 在测量更新时将观测方程在近似值 \(\bm{X}^{*}(k)\) 处用泰勒公式展开并舍去高阶项 \[\bm{Z}(k)=\left.\frac{\partial\bm{F}}{\partial\bm{X}(k)}\right|_{\bm{X}(k)=\bm{X}^{*}(k)}\left(\bm{X}(k)-\bm{X}^{*}(k)\right)+\bm{F}\left[\,\bm{X}^{*}(k)\,\right]+\bm{\Delta}(k) \tag{5.2.8}\] 设 \[\left.\frac{\partial\bm{F}}{\partial\bm{X}(k)}\right|_{\bm{X}(k)=\bm{X}^{*}(k)}=\bm{H}_k \tag{5.2.9}\] 线性化后观测方程为 \[\bm{Z}(k)=\bm{H}_k\bm{X}(k)+\left\{\bm{F}\left[\,\bm{X}^{*}(k)\,\right]-\bm{H}(k)\bm{X}^{*}(k)\right\}+\bm{\Delta}(k) \tag{5.2.10}\] 其中 \(\left\{\bm{F}\left[\,\bm{X}^{*}(k)\,\right]-\bm{H}(k)\bm{X}^{*}(k)\right\}\) 为非随机部分。测量更新的新息为 \[\bm{V}(k)=\bm{Z}(k)-\left\{\bm{H}(k)\hat{\bm{X}}(k,\ k-1)+\bm{F}\left[\,\bm{X}^{*}(k)\,\right]-\bm{H}(k)\bm{X}^{*}(k)\right\} \tag{5.2.11}\] 由于 \(\hat{\bm{X}}(k,\ k-1)\) 是在 \(t_k\) 时刻无观测值时的最优估计，所以取 \(\hat{\bm{X}}(k,\ k-1)\) 为近似值，即 \[\bm{X}^{*}(k)=\hat{\bm{X}}(k,\ k-1) \tag{5.2.12}\] 那么式 (5.2.11) 为 \[\bm{V}(k)=\bm{Z}(k)-\bm{F}\left(\hat{\bm{X}}(k,\ k-1)\right) \tag{5.2.13}\] 在得到新息后，剩余的步骤与 Kalman 滤波基础方程一样。测量更新为 \[\hat{\bm{X}}(k)=\hat{\bm{X}}(k,\ k-1)+\bm{K}_k\bm{V}(k,\ k-1) \tag{5.2.14}\] \[\bm{K}_k=\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}}+\bm{D}_{\Delta}(k)\right)^{-1} \tag{5.2.15}\] 其方差为 \[\bm{D}_{\hat{X}}(k)=\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\bm{D}_{\hat{X}}(k,\ k-1) \tag{5.2.16}\] 扩展的 Kalman 滤波基本公式见表 5.1。

扩展 Kalman 滤波的时间预测和新息计算式虽然是非线性函数得到的，但从其推导过程看，还是作了线性化，实质上还是线性估计。

观测值逐次更新的扩展的 Kalman 滤波

如果观测方程的非线性程度很高，将观测方程在预测值 \(\hat{\bm{X}}(k,\ k-1)\) 处线性化后，非线性化部分给滤波模型带来的模型误差会比较大。这时可以对每个观测值对预测值进行逐次更新，每次更新后状态的近似值都比预测值更加接近实际轨迹，线性化的模型也更加准确。

观测方程为 \[\bm{Z}(k)=\bm{F}\left[\,\bm{X}(k)\,\right]+\bm{\Delta}(k) \tag{5.2.17}\] 即 \[\begin{bmatrix}Z_1(k)\\ Z_2(k)\\ \vdots\\ Z_{\ell}(k)\end{bmatrix} =\begin{bmatrix}f_1\left(\bm{X}(k)\right)\\ f_2\left(\bm{X}(k)\right)\\ \vdots\\ f_{\ell}\left(\bm{X}(k)\right)\end{bmatrix} +\begin{bmatrix}\Delta_1(k)\\ \Delta_2(k)\\ \vdots\\ \Delta_{\ell}(k)\end{bmatrix} \tag{5.2.18}\] 如果观测值相互独立，那么 \[\bm{D}_{\Delta}(k)=\mathrm{diag}\left[\begin{array}{lllll}d_1(k) & \cdots & d_j(k) & \cdots & d_{\ell}(k)\end{array}\right] \tag{5.2.19}\] 首先，第一个观测值的观测方程为 \[Z_1(k)=f_1\left(\bm{X}(k)\right)+\Delta_1(k) \tag{5.2.20}\] 在 \(\hat{\bm{X}}(k,\ k-1)\) 处线性化，有 \[\left.\frac{\partial f_1\left(\bm{X}(k)\right)}{\partial\bm{X}(k)}\right|_{\bm{X}(k)=\hat{\bm{X}}(k,\ k-1)}=\bm{h}_1(k) \tag{5.2.21}\] 新息为 \[V_1(k)=Z_1(k)-f_1\left(\hat{\bm{X}}(k,\ k-1)\right) \tag{5.2.22}\] \[\bm{K}_k^{[1]}=\frac{\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{h}_1^{\mathrm{T}}(k)} {\bm{h}_1(k)\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{h}_1^{\mathrm{T}}(k)+d_1(k)} \tag{5.2.23}\] \[\hat{\bm{X}}^{[1]}(k)=\hat{\bm{X}}(k,\ k-1)+\bm{K}_k^{[1]}V_1(k) \tag{5.2.24}\] \[\bm{D}_{\hat{X}}^{[1]}(k)=\left(\bm{I}-\bm{K}_k^{[1]}\bm{h}_1(k)\right)\bm{D}_{\hat{X}}(k,\ k-1) \tag{5.2.25}\] \(\hat{\bm{X}}^{[1]}(k)\) 显然比 \(\hat{\bm{X}}(k,\ k-1)\) 更加靠近实际轨迹。然后，用第二个观测值 \(Z_2(k)\) 在 \(\hat{\bm{X}}^{[1]}(k)\) 处线性化后，对 \(\hat{\bm{X}}^{[1]}(k)\) 进行更新得到 \(\hat{\bm{X}}^{[2]}(k)\) 和 \(\bm{D}_{\hat{X}}^{[2]}(k)\)。如此逐一进行下去，第 \(j\) 个观测值 \(Z_j(k)\) 对状态 \(\hat{\bm{X}}^{[j-1]}(k)\) 的更新为 \[V_j(k)=Z_j(k)-f_j\left(\hat{\bm{X}}^{[j-1]}(k)\right) \tag{5.2.26}\] \[\left.\frac{\partial f_j\left(\bm{X}(k)\right)}{\partial\bm{X}(k)}\right|_{\bm{X}(k)=\hat{\bm{X}}^{[j-1]}(k)}=\bm{h}_j(k) \tag{5.2.27}\] \[\bm{K}_k^{[j]}=\frac{\bm{D}_{\hat{X}}^{[j-1]}(k)\,\bm{h}_j^{\mathrm{T}}(k)} {\bm{h}_j(k)\bm{D}_{\hat{X}}^{[j-1]}(k)\,\bm{h}_j^{\mathrm{T}}(k)+d_j(k)} \tag{5.2.28}\] \[\hat{\bm{X}}^{[j]}(k)=\hat{\bm{X}}^{[j-1]}(k)+\bm{K}_k^{[j]}V_j(k) \tag{5.2.29}\] \[\bm{D}_{\hat{X}}^{[j]}(k)=\left(\bm{I}-\bm{K}_k^{[j]}\bm{h}_j(k)\right)\bm{D}_{\hat{X}}^{[j-1]}(k) \tag{5.2.30}\] 在第一次观测值对状态更新时 \[\hat{\bm{X}}^{[0]}(k)=\hat{\bm{X}}(k,\ k-1) \tag{5.2.31}\] \[\bm{D}_{\hat{X}}^{[0]}(k)=\bm{D}_{\hat{X}}(k,\ k-1) \tag{5.2.32}\] 在 \(t_k\) 时刻的所有观测值更新完毕后，\(\hat{\bm{X}}^{[\ell]}(k)\) 即为 \(\hat{\bm{X}}(k)\)，\(\bm{D}_{\hat{X}}^{[\ell]}(k)\) 即为 \(\bm{D}_{\hat{X}}(k)\)。显然，每次更新的状态 \(\hat{\bm{X}}^{[j]}(k)\) 都比上一次的状态更接近实际轨迹，所以每次线性化的模型也更加准确。此方法的计算流程如图 5.2 所示。

算例分析

本节用一个算例来说明观测值逐次更新的扩展 Kalman 滤波与扩展的 Kalman 滤波的差异。

例 5.1如图 5.3 所示，从空中水平抛射出的物体，初始水平速度 \(v_x(0)\)，初始位置坐标 \((x(0),\ y(0))\)；受重力 \(g\) 和阻尼力影响，阻尼力与速度平方成正比，水平和垂直阻尼系数分别为 \(k_x\)，\(k_y\)。此外，还存在不确定干扰力，沿 \(x\)，\(y\) 轴分别为 \(e_x\) 和 \(e_y\)。在坐标原点处有一观测设备（不妨想象成雷达），可测得距离 \(r\) 和角度 \(\alpha\)。用扩展的 Kalman 滤波和观测值逐次更新法估计抛体的下落轨迹。

已知：\(k_x=0.01\)，\(k_y=0.05\)，重力加速度 \(g=9.8\)；初始位置和速度及其方差为 \[\bm{X}(t_0)=\begin{bmatrix}x(t_0)\\ v_x(t_0)\\ y(t_0)\\ v_y(t_0)\end{bmatrix} =\begin{bmatrix}0\,\mathrm{m}\\ 50\,\mathrm{m/s}\\ 500\,\mathrm{m}\\ 0\,\mathrm{m/s}\end{bmatrix}\ ,\quad \bm{D}_{\hat{X}}(t_0)=\begin{bmatrix} 100\,\mathrm{m}^2 & & & \\ & 100\,(\mathrm{m/s})^2 & & \\ & & 100\,\mathrm{m}^2 & \\ & & & 100\,(\mathrm{m/s})^2 \end{bmatrix}\] 将干扰力 \(e_x\) 和 \(e_y\) 视为零均值白噪声，并且 \[\bm{e}(t)=\begin{bmatrix}e_x\\ e_y\end{bmatrix}\ ,\quad \mathrm{Cov}\left[\,\bm{e}(t),\ \bm{e}(\tau)\,\right]=\bm{D}_e(t)\delta(t-\tau)\ ,\quad \bm{D}_e(t)=\begin{bmatrix}1.5^2 & \\ & 1.5^2\end{bmatrix}\ (\mathrm{m}^2/\mathrm{s}^3) \tag{5.2.33}\] 雷达观测 10 秒钟，观测值为 \(r\) 和 \(\alpha\)，采样间隔为 \(\Delta t=0.1\,\mathrm{s}\)，观测噪声与系统噪声不相关，并且 \[\bm{\Delta}(k)=\begin{bmatrix}\Delta_r(k)\\ \Delta_\alpha(k)\end{bmatrix}\ ,\quad \mathrm{Cov}\left[\,\bm{\Delta}(k),\ \bm{\Delta}(j)\,\right]=\bm{D}_{\Delta}(k)\delta(k-j) \tag{5.2.34}\] \[\bm{D}_{\Delta}(k)=\begin{bmatrix}10\,\mathrm{m}^2 & \\ & 1\times 10^{-5}\,\mathrm{rad}^2\end{bmatrix}\]

解：设状态为 \(\bm{X}(t)=\left[\begin{array}{llll}x(t) & v_x(t) & y(t) & v_y(t)\end{array}\right]^{\mathrm{T}}\)，根据题意可得到抛体的运动方程 \[\bm{g}:\quad \begin{cases} \dot{x}(t)=v_x(t)\\ \dot{v}_x(t)=-k_xv_x^2(t)+e_x(t)\\ \dot{y}(t)=v_y(t)\\ \dot{v}_y(t)=k_yv_y^2(t)-g+e_y(t) \end{cases} \tag{5.2.35}\] 显然，式 (5.2.35) 的微分方程为非线性函数，先将其线性化。设 \(\bm{X}(t)=\bm{X}^{*}(t)\)，\(\bm{e}^{*}(t)=0\)，得到 \[\dot{\bm{X}}(t)=\bm{A}(t)\bm{X}(t)+\bm{g}\left[\,\bm{X}^{*}(t)\quad \bm{u}(t)\quad 0\,\right]-\bm{A}(t)\bm{X}^{*}(t)+\bm{C}(t)\bm{e}(t) \tag{5.2.36}\] 其中 \[\bm{A}(t)=\left[\frac{\partial\bm{g}}{\partial\bm{X}(t)}\right]^{*} =\begin{bmatrix} 0 & 1 & 0 & 0\\ 0 & -2k_xv_x(t) & 0 & 0\\ 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 2k_yv_y(t) \end{bmatrix}_{\bm{X}(t)=\bm{X}^{*}(t)} \tag{5.2.37}\] \[\bm{C}(t)=\left[\frac{\partial\bm{g}}{\partial\bm{e}(t)}\right]^{*} =\begin{bmatrix}0 & 0\\ 1 & 0\\ 0 & 0\\ 0 & 1\end{bmatrix} \tag{5.2.38}\] \[\bm{g}\left[\,\bm{X}^{*}(t)\quad \bm{u}(t)\quad 0\,\right] =\begin{bmatrix} v_x(t)\\ -k_xv_x^2(t)\\ v_y(t)\\ k_yv_y^2(t)-g \end{bmatrix}_{\bm{X}(t)=\bm{X}^{*}(t)} \tag{5.2.39}\] 设 \[\bm{G}(t)=\bm{g}\left[\,\bm{X}^{*}(t)\quad \bm{u}(t)\quad 0\,\right]-\bm{A}(t)\bm{X}^{*}(t) \tag{5.2.40}\] 将式 (5.2.39) 和式 (5.2.37) 代入上式得到 \[\bm{G}(t)=\begin{bmatrix} 0\\ k_xv_x^{2}(t)\\ 0\\ -k_yv_y^{2}(t)-g \end{bmatrix}_{\bm{X}(t)=\bm{X}^{*}(t)} \tag{5.2.41}\] 注意到在本问题中，以上的 \(\bm{A}(t)\)、\(\bm{C}(t)\) 和 \(\bm{G}(t)\) 在代入近似值 \(\bm{X}^{*}(t)\) 后都成为与时间无关的常量，所以在下面的推导中用 \(\bm{A}\)、\(\bm{C}\) 和 \(\bm{G}\) 来代替 \(\bm{A}(t)\)、\(\bm{C}(t)\) 和 \(\bm{G}(t)\)。用差分方法将微分方程 (5.2.36) 离散化，状态转移矩阵为 \[\bm{\Phi}(t,\ \tau)=\bm{I}+\bm{A}\times(t-\tau) \tag{5.2.42}\] \[\bm{\Phi}(t,\ \tau)=\begin{bmatrix} 1 & t-\tau & 0 & 0\\ 0 & 1-2k_xv_x^{*}\times(t-\tau) & 0 & 0\\ 0 & 0 & 1 & t-\tau\\ 0 & 0 & 0 & 1+2k_yv_y^{*}\times(t-\tau) \end{bmatrix} \tag{5.2.43}\] 令 \[\begin{aligned} t_k&=t\\ t_{k-1}&=\tau \end{aligned} \tag{5.2.44}\] 将上式代入式 (5.2.43) \[\bm{\Phi}_{k,\ k-1}=\begin{bmatrix} 1 & 1\times 0.1 & 0 & 0\\ 0 & 1-2k_xv_x^{*}\times 0.1 & 0 & 0\\ 0 & 0 & 1 & 1\times 0.1\\ 0 & 0 & 0 & 1+2k_yv_y^{*}\times 0.1 \end{bmatrix} \tag{5.2.45}\] 离散化后的状态方程为 \[\bm{X}(k)=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\int_{t_{k-1}}^{t_k}\bm{\Phi}(t,\ \tau)\bm{G}\,\mathrm{d}\tau +\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\bm{C}\bm{e}(\tau)\,\mathrm{d}\tau \tag{5.2.46}\] 设 \[\bm{w}(k-1)=\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\bm{C}\bm{e}(\tau)\,\mathrm{d}\tau \tag{5.2.47}\] 注意到 \(\bm{G}^{*}\) 是与时间无关的量，因此可以放在积分外，所以式 (5.2.46) 为 \[\bm{X}(k)=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\,\mathrm{d}\tau\cdot\bm{G}+\bm{w}(k-1) \tag{5.2.48}\] 设式 (5.2.48) 的积分部分为 \(\bm{\Omega}(k-1)\)，那么 \[\begin{aligned} \bm{\Omega}(k-1)&=\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\,\mathrm{d}\tau\cdot\bm{G}\\ &=\begin{bmatrix} \Delta t & \dfrac{\Delta t^2}{2} & 0 & 0\\[8pt] 0 & \Delta t-k_xv_x^{*}\times\Delta t^2 & 0 & 0\\[8pt] 0 & 0 & \Delta t & \dfrac{\Delta t^2}{2}\\[8pt] 0 & 0 & 0 & \Delta t+k_yv_y^{*}\times\Delta t^2 \end{bmatrix} \begin{bmatrix}0\\ k_xv_x^{*2}\\ 0\\ -k_yv_y^{*2}-g\end{bmatrix} \end{aligned} \tag{5.2.49}\] \(\bm{w}(k-1)\) 的方差为 \[\bm{D}_w(k-1)=\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\bm{C}\bm{D}_e(\tau)\bm{C}^{\mathrm{T}}\bm{\Phi}^{\mathrm{T}}(t_k,\ \tau)\,\mathrm{d}\tau\] \[\resizebox{0.88\linewidth}{!}{$\displaystyle \begin{bmatrix} \dfrac{\Delta t^3}{3}\sigma_{e_x}^2 & \left(\dfrac{\Delta t^2}{2}-\dfrac{2k_xv_x^{*}\Delta t^3}{3}\right)\sigma_{e_x}^2 & 0 & 0\\[10pt] \left(\dfrac{\Delta t^2}{2}-\dfrac{2k_xv_x^{*}\Delta t^3}{3}\right)\sigma_{e_x}^2 & \dfrac{\left(3\Delta t-6k_xv_x^{*}\Delta t^2+4\left(k_xv_x^{*}\right)^2\Delta t^3\right)}{3}\sigma_{e_x}^2 & 0 & 0\\[10pt] 0 & 0 & \dfrac{\Delta t^3}{3}\sigma_{e_y}^2 & \left(\dfrac{\Delta t^2}{2}+\dfrac{2k_yv_y^{*}\Delta t^3}{3}\right)\sigma_{e_y}^2\\[10pt] 0 & 0 & \left(\dfrac{\Delta t^2}{2}+\dfrac{2k_yv_y^{*}\Delta t^3}{3}\right)\sigma_{e_y}^2 & \dfrac{\left(3\Delta t+6k_yv_y^{*}\Delta t^2+4\left(k_yv_y^{*}\right)^2\Delta t^3\right)}{3}\sigma_{e_y}^2 \end{bmatrix}$}\quad\text{(5.2.50)}\] 在以上模型中，取近似值为 \[\bm{X}^{*}=\begin{bmatrix}x^{*}\\ v_x^{*}\\ y^{*}\\ v_y^{*}\end{bmatrix} =\begin{bmatrix}\hat{x}(k-1)\\ \hat{v}_x(k-1)\\ \hat{y}(k-1)\\ \hat{v}_y(k-1)\end{bmatrix} \tag{5.2.51}\] 时间预测为 \[\hat{\bm{X}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1)+\bm{\Omega}(k-1) \tag{5.2.52}\] 其方差为 \[\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{5.2.53}\] 观测方程为 \[\begin{cases} f_1:\ r(k)=\sqrt{x^2(k)+y^2(k)}\ +\Delta_r(k)\\[6pt] f_2:\ \alpha(k)=\arctan\dfrac{x(k)}{y(k)}+\Delta_\alpha(k) \end{cases} \tag{5.2.54}\] 显然，观测方程也为非线性方程，这里用观测值逐次更新实现扩展的 Kalman 滤波。首先，用观测值 \(r(k)\) 对时间预测 \(\hat{\bm{X}}(k,\ k-1)\) 进行更新 \[V_1(k,\ k-1)=r(k)-\sqrt{x^{*2}+y^{*2}} \tag{5.2.55}\] \[\bm{h}_1(k)=\left.\frac{\partial f_1}{\partial\bm{X}(k)}\right|_{\bm{X}(k)=\bm{X}^{*}} =\begin{bmatrix}\dfrac{x^{*}}{\sqrt{x^{*2}+y^{*2}}} & 0 & \dfrac{y^{*}}{\sqrt{x^{*2}+y^{*2}}} & 0\end{bmatrix} \tag{5.2.56}\] \[\bm{K}_k^{[1]}=\frac{\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{h}_1(k)^{\mathrm{T}}} {\bm{h}_1(k)\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{h}_1(k)^{\mathrm{T}}+d_r(k)} \tag{5.2.57}\] \[\hat{\bm{X}}^{[1]}(k)=\hat{\bm{X}}(k,\ k-1)+\bm{K}_k^{[1]}V_1(k,\ k-1) \tag{5.2.58}\] \[\bm{D}_{\hat{X}}^{[1]}(k)=\left(\bm{I}-\bm{K}_k^{[1]}\bm{h}_1(k)\right)\bm{D}_{\hat{X}}(k,\ k-1) \tag{5.2.59}\] 在上面的计算中，近似值取 \(\bm{X}^{*}=\hat{\bm{X}}(k,\ k-1)\)。得到 \(r(k)\) 对时间预测的更新 \(\hat{\bm{X}}^{[1]}(k)\) 和 \(\bm{D}_{\hat{X}}^{[1]}(k)\)，接着用观测值 \(\alpha(k)\) 对 \(\hat{\bm{X}}^{[1]}(k)\) 和 \(\bm{D}_{\hat{X}}^{[1]}(k)\) 进行更新 \[V_2(k,\ k-1)=\alpha(k)-\arctan\left(\frac{x^{*}}{y^{*}}\right) \tag{5.2.60}\] \[\bm{h}_2(k)=\left.\frac{\partial f_2}{\partial\bm{X}(k)}\right|_{\bm{X}(k)=\bm{X}^{*}} =\begin{bmatrix}\dfrac{1/y^{*}}{1+\left(x^{*}/y^{*}\right)^2} & 0 & \dfrac{-x^{*}/y^{*2}}{1+\left(x^{*}/y^{*}\right)^2} & 0\end{bmatrix} \tag{5.2.61}\] \[\bm{K}_k^{[2]}=\frac{\bm{D}_{\hat{X}}^{[1]}(k)\,\bm{h}_2(k)^{\mathrm{T}}} {\bm{h}_2(k)\bm{D}_{\hat{X}}^{[1]}(k)\,\bm{h}_2(k)^{\mathrm{T}}+d_\alpha(k)} \tag{5.2.62}\] \[\hat{\bm{X}}^{[2]}(k)=\hat{\bm{X}}^{[1]}(k)+\bm{K}_k^{[2]}V_2(k,\ k-1) \tag{5.2.63}\] \[\bm{D}_{\hat{X}}^{[2]}(k)=\left(\bm{I}-\bm{K}_k^{[2]}\bm{h}_2(k)\right)\bm{D}_{\hat{X}}^{[1]}(k) \tag{5.2.64}\] 这时使 \(\bm{X}^{*}=\hat{\bm{X}}^{[1]}(k)\)，更新得到的 \(\hat{\bm{X}}^{[2]}(k)\) 和 \(\bm{D}_{\hat{X}}^{[2]}(k)\) 就是在 \(k\) 时刻的滤波结果：\(\hat{\bm{X}}(k)\) 和 \(\bm{D}_{\hat{X}}(k)\)。在得到 \(\hat{\bm{X}}(k)\) 和 \(\bm{D}_{\hat{X}}(k)\) 后即可对 \(k+1\) 时刻进行时间预测和测量更新，重复以上计算，直到得到最后一组观测值的滤波结果。

图 5.4 给出了抛体的实际轨迹、观测轨迹和滤波估计轨迹。图 5.5 给出了状态 \(x\) 和 \(y\) 的滤波方差 \(\sigma_{\hat{x}}^2(k)\) 和 \(\sigma_{\hat{y}}^2(k)\)。从图中可以看到，由于初始状态不确定，所以设定了比较大的初始状态方差，数值为 100。开始滤波后，滤波方差摆脱了初值的影响，方差迅速减小，接着随着时间递推缓慢减小到稳态值。图 5.6 给出了观测值 \(r\) 和 \(\alpha\) 新息。从图中看到，观测值 \(r\) 的新息值在 \(\pm 20\,\mathrm{m}\) 以内，观测值 \(\alpha\) 的新息在 \(\pm 0.03\,\mathrm{rad}\)。图 5.7 给出了 \(\bm{K}_k\bm{V}(k,\ k-1)\) 对 \(\left[\begin{array}{llll}\hat{x}(k,\ k-1) & \hat{v}_x(k,\ k-1) & \hat{y}(k,\ k-1) & \hat{v}_y(k,\ k-1)\end{array}\right]^{\mathrm{T}}\) 的增益。显然，在最初的 20 多个历元，增益变化较大；随后增益趋于稳定，并达到稳态。

信息滤波

在某些情况下，如果对初始状态 \(\hat{\bm{X}}(0)\) 了解很少，就需要数值很大的方差 \(\bm{D}_{\hat{X}}(0)\) 来描述其不确定性；当 \(\hat{\bm{X}}(0)\) 完全不知，那么 \(\bm{D}_{\hat{X}}(0)\rightarrow\infty\)。但当 \(\bm{D}_{\hat{X}}(0)\rightarrow\infty\) 时，给滤波计算带来了困难。我们知道当 \(\bm{D}_{\hat{X}}(0)\rightarrow\infty\) 时，可以认为其逆矩阵 \(\bm{D}_{\hat{X}}^{-1}(0)\rightarrow 0\)，所以，可通过计算 \(\bm{D}_{\hat{X}}^{-1}(k)\) 和 \(\bm{D}_{\hat{X}}^{-1}(k,\ k-1)\) 来完成滤波的递推。\(\bm{D}_{\hat{X}}^{-1}(k)\) 和 \(\bm{D}_{\hat{X}}^{-1}(k,\ k-1)\) 也被称为信息矩阵，利用 \(\bm{D}_{\hat{X}}^{-1}(k)\) 和 \(\bm{D}_{\hat{X}}^{-1}(k,\ k-1)\) 进行的滤波计算也称为信息滤波。

在 4.2.2 节中证明得到了 \[\left[\,\bm{D}_{\hat{X}}^{-1}(k,\ k-1)+\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\bm{H}_k\,\right]\hat{\bm{X}}(k) =\bm{D}_{\hat{X}}^{-1}(k,\ k-1)\hat{\bm{X}}(k,\ k-1)+\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\bm{Z}(k) \tag{5.3.1}\] \[\bm{D}_{\hat{X}}^{-1}(k)=\bm{D}_{\hat{X}}^{-1}(k,\ k-1)+\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\bm{H}_k \tag{5.3.2}\]

设 \[\bm{W}_{k,\ k-1}=\bm{D}_{\hat{X}}^{-1}(k,\ k-1)\ ,\quad \bm{W}_k=\bm{D}_{\hat{X}}^{-1}(k) \tag{5.3.3}\] 得到 \[\bm{W}_k\hat{\bm{X}}(k)=\bm{W}_{k,\ k-1}\hat{\bm{X}}(k,\ k-1)+\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\bm{Z}(k) \tag{5.3.4}\] 和 \[\bm{W}_k=\bm{W}_{k,\ k-1}+\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\bm{H}_k \tag{5.3.5}\] 由于 \[\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{5.3.6}\] 所以 \[\bm{W}_{k,\ k-1}=\left[\,\bm{\Phi}_{k,\ k-1}\bm{D}_{\hat{X}}(k-1)\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}+\bm{D}_w(k-1)\,\right]^{-1} \tag{5.3.7}\] 设 \[\bm{M}_{k-1}=\left[\,\bm{\Phi}_{k,\ k-1}\bm{D}_{\hat{X}}(k-1)\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\,\right]^{-1} \tag{5.3.8}\] 得到 \[\bm{W}_{k,\ k-1}=\left[\,\bm{M}_{k-1}^{-1}+\bm{D}_w(k-1)\,\right]^{-1} \tag{5.3.9}\] 根据附录 (A.9) 矩阵求逆的恒等式 \[\left(\bm{A}^{-1}+\bm{B}\bm{D}^{-1}\bm{C}\right)^{-1}=\bm{A}-\bm{A}\bm{B}(\bm{D}+\bm{C}\bm{A}\bm{B})^{-1}\bm{C}\bm{A} \tag{5.3.10}\] 式 (5.3.9) 为 \[\bm{W}_{k,\ k-1}=\bm{M}_{k-1}-\bm{M}_{k-1}\left[\,\bm{D}_w^{-1}(k-1)+\bm{M}_{k-1}\,\right]^{-1}\bm{M}_{k-1} \tag{5.3.11}\] 令 \[\bm{N}_{k-1}=\bm{M}_{k-1}\left[\,\bm{D}_w^{-1}(k-1)+\bm{M}_{k-1}\,\right]^{-1} \tag{5.3.12}\] 式 (5.3.11) 为 \[\bm{W}_{k,\ k-1}=\bm{M}_{k-1}-\bm{N}_{k-1}\bm{M}_{k-1} \tag{5.3.13}\] 式 (5.3.8) 还可以表示为 \[\bm{M}_{k-1}=\left(\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\right)^{-1}\bm{D}_{\hat{X}}^{-1}(k-1)\left(\bm{\Phi}_{k,\ k-1}\right)^{-1} \tag{5.3.14}\] 由于 \(\bm{\Phi}_{k,\ k-1}^{-1}=\bm{\Phi}_{k-1,\ k}\)，所以 \[\bm{M}_{k-1}=\bm{\Phi}_{k-1,\ k}^{\mathrm{T}}\bm{W}_{k-1}\bm{\Phi}_{k-1,\ k} \tag{5.3.15}\] 设 \[\bm{L}_{k,\ k-1}=\bm{W}_{k,\ k-1}\hat{\bm{X}}(k,\ k-1) \tag{5.3.16}\] \[\bm{L}_k=\bm{W}_k\hat{\bm{X}}(k) \tag{5.3.17}\] 将式 (5.3.13) 代入 (5.3.16) \[\bm{L}_{k,\ k-1}=(\bm{I}-\bm{N}_{k-1})\bm{M}_{k-1}\hat{\bm{X}}(k,\ k-1) \tag{5.3.18}\] 再将式 (5.3.15) 代入 (5.3.18) \[\bm{L}_{k,\ k-1}=(\bm{I}-\bm{N}_{k-1})\bm{\Phi}_{k-1,\ k}^{\mathrm{T}}\bm{W}_{k-1}\bm{\Phi}_{k-1,\ k}\hat{\bm{X}}(k,\ k-1) \tag{5.3.19}\] 由于 \(\hat{\bm{X}}(k-1)=\bm{\Phi}_{k-1,\ k}\hat{\bm{X}}(k,\ k-1)\)，上式为 \[\bm{L}_{k,\ k-1}=(\bm{I}-\bm{N}_{k-1})\bm{\Phi}_{k-1,\ k}^{\mathrm{T}}\bm{W}_{k-1}\hat{\bm{X}}(k-1) \tag{5.3.20}\] 由于 \(\bm{L}_{k-1}=\bm{W}_{k-1}\hat{\bm{X}}(k-1)\)，所以 \[\bm{L}_{k,\ k-1}=(\bm{I}-\bm{N}_{k-1})\bm{\Phi}_{k-1,\ k}^{\mathrm{T}}\bm{L}_{k-1} \tag{5.3.21}\] 将式 (5.3.16)、式 (5.3.17) 代入式 (5.3.4)，得到 \[\bm{L}_k=\bm{L}_{k,\ k-1}+\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\bm{Z}(k) \tag{5.3.22}\] 至此形成了信息滤波的递推。

现将信息滤波递推过程总结如下：

已知 \(\bm{W}_0\)，\(\bm{L}_0=\bm{W}_0\hat{\bm{X}}(0)\)，时间预测为 \[\begin{aligned} \bm{M}_{k-1}&=\bm{\Phi}_{k-1,\ k}^{\mathrm{T}}\bm{W}_{k-1}\bm{\Phi}_{k-1,\ k}\\ \bm{N}_{k-1}&=\bm{M}_{k-1}\left[\,\bm{D}_w^{-1}(k-1)+\bm{M}_{k-1}\,\right]^{-1}\\ \bm{L}_{k,\ k-1}&=(\bm{I}-\bm{N}_{k-1})\bm{\Phi}_{k-1,\ k}^{\mathrm{T}}\bm{L}_{k-1}\\ \bm{W}_{k,\ k-1}&=\bm{D}_{\hat{X}}^{-1}(k,\ k-1)=(\bm{I}-\bm{N}_{k-1})\bm{M}_{k-1} \end{aligned} \tag{5.3.23}\] 测量更新为 \[\begin{aligned} \bm{L}_k&=\bm{L}_{k,\ k-1}+\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\bm{Z}(k)\\ \bm{W}_k&=\bm{D}_{\hat{X}}^{-1}(k)=\bm{W}_{k,\ k-1}+\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\bm{H}_k \end{aligned} \tag{5.3.24}\]

信息滤波是在 Kalman 滤波算法的基础上推导出来的，二者在理论上是等价的。与 Kalman 滤波算法相比，信息滤波的优点是：

(1) 当缺乏初值的先验信息或者无先验信息时，可设 \(\bm{W}_0=\bm{D}_{\hat{X}}^{-1}(0)=0\)，从而启动滤波计算。当经过一段时间的递推后，滤波摆脱初值的影响趋于稳态，这时可计算得到 \[\hat{\bm{X}}(k)=\bm{W}_k^{-1}\bm{L}_k\ ,\qquad \bm{D}_{\hat{X}}(k)=\bm{W}_k^{-1}\]

(2) 常规 Kalman 滤波算法需要计算逆矩阵 \(\left(\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\bm{H}_k^{\mathrm{T}}+\bm{D}_{\Delta}(k)\right)^{-1}\)，而在信息滤波中，则计算逆矩阵 \(\left[\,\bm{D}_w^{-1}(k-1)+\bm{M}_{k-1}\,\right]^{-1}\)，当状态的维数少于观测值维数时，信息滤波有较少的计算量。

自适应的 Kalman 滤波

当建立的数学模型与实际一致时，Kalman 滤波估计器得到的是最优无偏估计。但在实际应用中，我们建立的数学模型只是对实际动态系统在一定程度上的近似描述，如对干扰信号统计特性缺乏了解或者根本未知，先验给出的噪声方差 \(\bm{D}_w(k)\) 和 \(\bm{D}_{\Delta}(k)\) 往往与实际不符。此外，对系统的运动规律了解不足，或者虽然有足够的了解，但是对函数模型作了线性化近似，导致状态转移矩阵 \(\bm{\Phi}_{k,\ k-1}\) 和量测矩阵 \(\bm{H}_k\) 也不能准确地给出。这些不确定的因素使得 Kalman 滤波算法失去最优性，估计准确性大大降低，严重时会引起滤波发散。为了克服以上原因引起的滤波发散，可利用观测值提供的信息在滤波递推的过程中不断地校正函数模型和随机模型，这就是自适应的 Kalman 滤波。

自适应的方法有很多，如次优极大验后滤波、贝叶斯法自适应滤波、相关法自适应滤波、协方差匹配法自适应滤波和强跟踪滤波器等。有的方法在理论上相对严密，但计算量大，实时性和稳定性难以保证；有的采用近似方法，但是在换取滤波器稳定的同时损失了滤波精度。在以上的自适应滤波中，应用较多的是相关法自适应滤波和次优极大验后滤波。相关自适应是根据观测值序列 \(\{\bm{Z}(k)\}\) 估计输出观测值的相关函数序列 \(\{\bm{C}(k)\}\)，再推算出最佳稳态增益矩阵 \(\bm{K}_k\)，使得增益矩阵 \(\bm{K}_k\) 不断地与实际观测值相适应来达到克服滤波器发散的目的。本节将不加推导地给出次优极大验后滤波，之所以称之为“次优”估计，是为了得到可实现的算法，在理论推导上做了近似，是非严格的最优估计。

离散线性系统的函数模型为 \[\begin{aligned} \bm{X}(k)&=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{w}(k-1)\\ \bm{Z}(k)&=\bm{H}_k\bm{X}(k)+\bm{\Delta}(k) \end{aligned} \tag{5.4.1}\] 随机模型为 \[\begin{gathered} E\left[\,\bm{w}(k)\,\right]=\bm{0}\ ,\quad \mathrm{Cov}\left[\,\bm{w}(k),\ \bm{w}(j)\,\right]=\bm{D}_w(k)\delta(k-j)\\ E\left[\,\bm{\Delta}(k)\,\right]=\bm{0}\ ,\quad \mathrm{Cov}\left[\,\bm{\Delta}(k),\ \bm{\Delta}(j)\,\right]=\bm{D}_{\Delta}(k)\delta(k-j)\\ \mathrm{Cov}\left[\,\bm{w}(k),\ \bm{\Delta}(j)\,\right]=0 \end{gathered} \tag{5.4.2}\] 此外，已知 \(\hat{\bm{X}}(0)\)，并且有 \[\begin{aligned} E\left[\,\bm{X}(t_0)\,\right]&=E\left[\,\hat{\bm{X}}(0)\,\right]\\ \mathrm{Var}\left[\,\hat{\bm{X}}(0)\,\right]&=\bm{D}_{\hat{X}}(0) \end{aligned} \tag{5.4.3}\] \[\begin{aligned} \mathrm{Cov}\left[\,\hat{\bm{X}}(0),\ \bm{w}(k)\,\right]&=0\\ \mathrm{Cov}\left[\,\hat{\bm{X}}(0),\ \bm{\Delta}(k)\,\right]&=0 \end{aligned} \tag{5.4.4}\] 以上是 Kalman 滤波的标准模型。为了补偿状态转移矩阵 \(\bm{\Phi}_{k,\ k-1}\) 和量测矩阵 \(\bm{H}_k\) 不准确的模型误差，将式 (5.4.1) 的函数模型改进为 \[\begin{aligned} \bm{X}(k)&=\left(\bm{\Phi}_{k,\ k-1}+\Delta\bm{\Phi}_{k,\ k-1}\right)\bm{X}(k-1)+\bm{w}(k-1)\\ \bm{Z}(k)&=\left(\bm{H}_k+\Delta\bm{H}_k\right)\bm{X}(k)+\bm{\Delta}(k) \end{aligned} \tag{5.4.5}\]

上式中的 \(\Delta\bm{\Phi}_{k,\ k-1}\) 和 \(\Delta\bm{H}_k\) 分别为补偿 \(\bm{\Phi}_{k,\ k-1}\) 和 \(\bm{H}_k\) 不准确的偏差。若将 \(\Delta\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)\) 视为未知干扰 \(\bm{C}_X(k-1)\)，将 \(\Delta\bm{H}_k\bm{X}(k)\) 视为未知干扰 \(\bm{C}_Z(k)\)，那么函数模型为 \[\begin{aligned} \bm{X}(k)&=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{C}_X(k-1)+\bm{w}(k-1)\\ \bm{Z}(k)&=\bm{H}_k\bm{X}(k)+\bm{C}_Z(k)+\bm{\Delta}(k) \end{aligned} \tag{5.4.6}\] 设 \(\bm{C}_X(0)\) 已知，那么在滤波计算时可对 \(\bm{C}_X(k-1)\) 和 \(\bm{C}_Z(k)\) 进行实时估计，从而达到补偿模型误差的目的。此外，噪声矩阵 \(\bm{D}_w(k)\) 和 \(\bm{D}_{\Delta}(k)\) 不准确或者未知，也需要观测值来修正估计。

次优无偏极大验后估计器

次优无偏极大验后估计器的递推公式为 \[\hat{\bm{C}}_Z(k)=\frac{k-1}{k}\hat{\bm{C}}_Z(k-1)+\frac{1}{k}\left[\,\bm{Z}(k)-\bm{H}_k\hat{\bm{X}}(k,\ k-1)\,\right] \tag{5.4.7}\] \[\hat{\bm{D}}_{\Delta}(k)=\frac{k-1}{k}\hat{\bm{D}}_{\Delta}(k-1)+\frac{1}{k}\left[\,\bm{V}(k)\bm{V}^{\mathrm{T}}(k)-\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\bm{H}_k^{\mathrm{T}}\,\right] \tag{5.4.8}\] \[\hat{\bm{C}}_X(k)=\frac{k-1}{k}\hat{\bm{C}}_X(k-1)+\frac{1}{k}\left[\,\hat{\bm{X}}(k)-\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1)\,\right] \tag{5.4.9}\] \[\hat{\bm{D}}_w(k)=\frac{k-1}{k}\hat{\bm{D}}_w(k-1) +\frac{1}{k}\left(\bm{K}_k\bm{V}(k)\bm{V}(k)^{\mathrm{T}}\bm{K}_k^{\mathrm{T}}+\bm{D}_{\hat{X}}(k)-\bm{\Phi}_{k,\ k-1}\bm{D}_{\hat{X}}(k-1)\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\right) \tag{5.4.10}\]

在以上的计算公式中，式 (5.4.7) 和式 (5.4.9) 是对函数模型的修正，式 (5.4.8) 和式 (5.4.10) 是对随机模型的修正。现将自适应滤波的递推估计公式汇总如下：

(1) \(k=1\) 时，不考虑模型误差，按照常规的 Kalman 滤波进行计算，得到 \(\hat{\bm{X}}(1)\) 和 \(\bm{D}_{\hat{X}}(1)\)。然后估计 \[\begin{aligned} \hat{\bm{C}}_X(1)&=\hat{\bm{X}}(1)-\bm{\Phi}_{1,\ 0}\hat{\bm{X}}(0)\bm{\Phi}_{1,\ 0}^{\mathrm{T}}\\ \hat{\bm{D}}_w(1)&=\bm{K}_1\bm{V}(1)\bm{V}(1)^{\mathrm{T}}\bm{K}_1^{\mathrm{T}}+\bm{D}_{\hat{X}}(1)-\bm{\Phi}_{1,\ 0}\bm{D}_{\hat{X}}(0)\bm{\Phi}_{1,\ 0}^{\mathrm{T}} \end{aligned}\]

(2) \(k\geq 2\) 时，进行时间预测 \[\hat{\bm{X}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1)+\hat{\bm{C}}_X(k-1) \tag{5.4.11}\] \[\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}}+\hat{\bm{D}}_w(k-1) \tag{5.4.12}\] 测量更新 \[\hat{\bm{C}}_Z(k)=\frac{k-1}{k}\hat{\bm{C}}_Z(k-1)+\frac{1}{k}\left(\bm{Z}(k)-\bm{H}_k\hat{\bm{X}}(k,\ k-1)\right) \tag{5.4.13}\] \[\bm{V}(k)=\bm{Z}(k)-\bm{H}_k\hat{\bm{X}}(k,\ k-1)-\hat{\bm{C}}_Z(k) \tag{5.4.14}\] \[\hat{\bm{D}}_{\Delta}(k)=\frac{k-1}{k}\hat{\bm{D}}_{\Delta}(k-1)+\frac{1}{k}\left[\,\bm{V}(k)\bm{V}^{\mathrm{T}}(k)-\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\bm{H}_k^{\mathrm{T}}\,\right] \tag{5.4.15}\] \[\bm{K}_k=\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}}+\hat{\bm{D}}_{\Delta}(k)\,\right]^{-1} \tag{5.4.16}\] \[\hat{\bm{X}}(k)=\hat{\bm{X}}(k,\ k-1)+\bm{K}_k\bm{V}(k) \tag{5.4.17}\] \[\bm{D}_{\hat{X}}(k)=\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\bm{D}_{\hat{X}}(k,\ k-1) \tag{5.4.18}\] 然后，估计 \[\hat{\bm{C}}_X(k)=\frac{k-1}{k}\hat{\bm{C}}_X(k-1)+\frac{1}{k}\left[\,\hat{\bm{X}}(k)-\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1)\,\right] \tag{5.4.19}\] \[\hat{\bm{D}}_w(k)=\frac{k-1}{k}\hat{\bm{D}}_w(k-1) +\frac{1}{k}\left[\,\bm{K}_k\bm{V}(k)\bm{V}(k)^{\mathrm{T}}\bm{K}_k^{\mathrm{T}}+\bm{D}_{\hat{X}}(k)-\bm{\Phi}_{k,\ k-1}\bm{D}_{\hat{X}}(k-1)\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\,\right] \tag{5.4.20}\] 在完成 (5.4.19) 和 (5.4.20) 的计算后，即可进行下一个时间点的时间预测和测量更新，如此递推下去，直到完成所有的滤波计算。在以上的计算中，当 \(k=2\) 时，需要代入 \(\hat{\bm{C}}_Z(1)\) 和 \(\hat{\bm{D}}_{\Delta}(1)\) 进行计算，可以设 \(\hat{\bm{C}}_Z(1)=0\) 和 \(\hat{\bm{D}}_{\Delta}(1)=\bm{D}_{\Delta}(1)\)。

在上面的推导中，同时给出了函数模型偏差和噪声方差的自适应方法，但在实际的应用中，考虑计算的复杂性，一般选择自适应地估计函数模型偏差 \(\bm{C}_X(k)\) 和 \(\bm{C}_Z(k)\)，就不需要再去纠正随机模型。这是因为模型的不确定部分已经被 \(\hat{\bm{C}}_Z(k)\) 和 \(\hat{\bm{C}}_X(k)\) 吸收，就不需要调整噪声方差矩阵了，也就不需要计算式 (5.4.15) 和式 (5.4.20)。反之，如果自适应地估计了噪声方差，模型的不确定性也能被噪声方差吸收，就不需要去估计 \(\hat{\bm{C}}_Z(k)\) 和 \(\hat{\bm{C}}_X(k)\) 了。在具体应用中，可根据实际情况确定需要的自适应项，灵活应用以上公式。

固定窗口的估计方法

在次优无偏极大验后自适应滤波中，对自适应项的估计是历史信息的算术平均值，即每个时刻的观测值对估计的影响是等权的。但在时变系统中，模型偏差具有时间相关性，即越接近当前时刻，相关性越强；随着时间的推移，相关性逐渐减弱。为了充分利用相关性强的历史信息，可将估计时间固定，用距离当前时刻 \(k\) 最近的 \(N\) 个时刻的历史观测值来估计，根据 5.4.1 节的结果，有 \[\hat{\bm{C}}_Z(k)=\frac{1}{N}\sum_{j=k-N+1}^{k}\left[\,\bm{Z}(j)-\bm{H}_j\hat{\bm{X}}(j,\ j-1)\,\right] \tag{5.4.21}\] \[\hat{\bm{D}}_{\Delta}(k)=\frac{1}{N}\sum_{j=k-N+1}^{k}\left[\,\bm{V}(j)\bm{V}^{\mathrm{T}}(j)-\bm{H}_j\bm{D}_{\hat{X}}(j,\ j-1)\bm{H}_j^{\mathrm{T}}\,\right] \tag{5.4.22}\] \[\hat{\bm{C}}_X(k)=\frac{1}{N}\sum_{j=k-N+1}^{i=k}\left(\hat{\bm{X}}(j)-\bm{\Phi}_{j,\ j-1}\hat{\bm{X}}(j-1)\right) \tag{5.4.23}\] \[\hat{\bm{D}}_w(k)=\frac{1}{N}\sum_{j=k-N+1}^{k}\left[\,\bm{K}_j\bm{V}_Z(j)\bm{V}_Z(j)^{\mathrm{T}}\bm{K}_j^{\mathrm{T}}+\bm{D}_{\hat{X}}(j)-\bm{\Phi}_{j,\ j-1}\bm{D}_{\hat{X}}(j)\bm{\Phi}_{l,\ j-1}^{\mathrm{T}}\,\right] \tag{5.4.24}\] 窗口的大小 \(N\) 是影响估计结果的重要因素。一般可设置 \(N=100\)，但不同的动态系统中 \(N\) 的设置有较大区别，应根据经验或试验结果设置。

Sage-Husa 估计方法

模型偏差的时间相关性一般随着时间以指数函数减弱，为了更准确地利用历史信息，Sage-Husa 方法引入了遗忘因子 \(b\)（\(<b<1\)），并构造指数函数来增加距离当前时刻较近的观测值的权，相应地减少较陈旧数据对自适应估计项的影响。Sage-Husa 的具体方法如下：首先设置遗忘因子 \(b\)，构造指数函数 \[d_k=\frac{1-b}{1-b^k} \tag{5.4.25}\] 将式 (5.4.13)、式 (5.4.15)、式 (5.4.19) 和式 (5.4.20) 中的 \(\dfrac{1}{k}\) 用 \(d_k\) 代替，得到 \[\hat{\bm{C}}_X(k)=(1-d_k)\hat{\bm{C}}_X(k-1)+d_k\left(\hat{\bm{X}}(k)-\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1)\right) \tag{5.4.26}\] \[\hat{\bm{D}}_w(k)=(1-d_k)\hat{\bm{D}}_w(k-1)+d_k\left[\,\bm{K}_k\bm{V}(k)\bm{V}(k)^{\mathrm{T}}\bm{K}_k^{\mathrm{T}}+\bm{D}_{\hat{X}}(k)-\bm{\Phi}_{k,\ k-1}\bm{D}_{\hat{X}}(k-1)\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\,\right] \tag{5.4.27}\] \[\hat{\bm{C}}_Z(k)=(1-d_k)\hat{\bm{C}}_Z(k-1)+d_k\left(\bm{Z}(k)-\bm{H}_k\hat{\bm{X}}(k,\ k-1)\right) \tag{5.4.28}\] \[\hat{\bm{D}}_{\Delta}(k)=(1-d_k)\hat{\bm{D}}_{\Delta}(k-1)+d_k\left(\bm{V}(k)\bm{V}(k)^{\mathrm{T}}-\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\bm{H}_k^{\mathrm{T}}\right) \tag{5.4.29}\] 式 (5.4.26) 式 (5.4.29) 表明，历史信息的权为 \((1-d_k)\)，当前观测信息的权为 \(d_k\)。为了清楚地看到历史信息随时间的衰减，现将式 (5.4.25) 代入式 (5.4.27) 并将其展开 \[\begin{aligned} \hat{\bm{D}}_w(k)=&\frac{b^{k}(1-b^0)}{1-b^k}\hat{\bm{D}}_w(0)\\ &+\frac{b^{k-1}(1-b)}{1-b^k}\left[\,\bm{K}_1\bm{V}(1)\bm{V}^{\mathrm{T}}(1)\bm{K}_1^{\mathrm{T}}+\bm{D}_{\hat{X}}(1)-\bm{\Phi}_{1,\ 0}\bm{D}_{\hat{X}}(0)\bm{\Phi}_{1,\ 0}^{\mathrm{T}}\,\right]\\ &\ \vdots\\ &+\frac{b^{k-j}(1-b)}{1-b^k}\left[\,\bm{K}_j\bm{V}(j)\bm{V}^{\mathrm{T}}(j)\bm{K}_j^{\mathrm{T}}+\bm{D}_{\hat{X}}(j)-\bm{\Phi}_{j-1,\ j}\bm{D}_{\hat{X}}(j-1)\bm{\Phi}_{j,\ j-1}^{\mathrm{T}}\,\right]\\ &\ \vdots\\ &+\frac{b^0(1-b)}{1-b^k}\left[\,\bm{K}_k\bm{V}(k)\bm{V}^{\mathrm{T}}(k)\bm{K}_k^{\mathrm{T}}+\bm{D}_{\hat{X}}(k)-\bm{\Phi}_{k,\ k-1}\bm{D}_{\hat{X}}(k-1)\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\,\right] \end{aligned} \tag{5.4.30}\] 上式表明了所有的观测值对 \(\hat{\bm{D}}_w(k)\) 的估计都有影响；\(\bm{Z}(j)\) 对 \(\hat{\bm{D}}_w(k)\) 影响的大小取决于权值 \(\dfrac{b^{k-j}(1-b)}{1-b^k}\)；所有观测值的权值之和为“1”，即 \[\sum_{i=1}^{k}\frac{b^{k-j}(1-b)}{1-b^k}=1\] 在 Sage-Husa 方法中，\(b\) 的选取是影响估计效果的重要因素。图 5.8 给出当遗忘因子 \(b\) 取不同值，观测值 \(\bm{Z}(j)\)（\(j=1,\ 2,\ \cdots,\ 50\)）在估计 \(\hat{\bm{D}}_w(50)\) 时的权值。由图 5.8 可以看出，越陈旧的观测值对 \(\hat{\bm{D}}_w(50)\) 的影响就越小。此外，遗忘因子 \(b\) 的取值越大，历史信息被遗忘得越慢；反之，历史信息被遗忘得越快。所以，我们可以通过调节遗忘因子 \(b\) 的大小来决定历史观测信息对自适应项估计的影响。

平方根滤波

人们在对 Kalman 滤波的使用中发现，除了模型不准确导致滤波发散外，计算误差的累积也可导致滤波发散。计算误差是由计算机的舍入误差引起的。由于计算机的字长是有限的，这就使得滤波递推中每步计算都有截断误差，误差逐渐累积，使得计算值与理论值的差异越来越大，从而导致发散。下面首先解释计算误差导致滤波发散的原因，然后介绍克服计算发散的平方根滤波。

Kalman 滤波数值发散的原因

计算机是以二进制来存储的，如数值 \(\dfrac{1}{3}\) 在计算机内存储为 \[\frac{1}{3}=0_b010101010101010101010101\cdots\] 其中，下标 \(b\) 表示二进制的小数点。根据上面的存储得到的数值为 \[\begin{aligned} &0\times 2^{0}+0\times 2^{-1}+1\times 2^{-2}+0\times 2^{-3}+1\times 2^{-4}+0\times 2^{-5}+1\times 2^{-6}\cdots\\ &\qquad=0+0+\frac{1}{4}+0+\frac{1}{16}+0+\frac{1}{64}+\cdots \end{aligned}\] 以一个存储字长为 24 位的计算机来说，得到的数值为 \[0_b01010101010101010101010=\frac{11184811}{33554432}=\frac{1}{3}-\frac{1}{100663296}\] \(\dfrac{1}{100663296}\) 为这台计算机未存储的数值，也就是对 \(\dfrac{1}{3}\) 进行存储时产生的舍入误差。如果计算机将 \(1+\varepsilon\) 存储为 \(1\)，即 \[1+\varepsilon\equiv 1 \tag{5.5.1}\] \(\varepsilon\) 为计算机存储截断的数值部分，也称 \(\varepsilon\) 为计算机存储精度。现有的计算机大多为 32 位或 64 位存储，但在滤波计算中，计算误差给滤波结果带来的影响仍然需要考虑。

例 5.2设某定常系统的状态为一维，各矩阵为 \[\bm{H}=1\] \[\bm{\Phi}=1\] 由于无初值信息，设 \(\bm{D}_{\hat{X}}(0)\gg\bm{D}_{\Delta}\) 和 \(\bm{D}_{\hat{X}}(0)\gg\bm{D}_w\)。卡尔曼滤波基本方程为 \[\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)\] \[\bm{K}_k=\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}}+\bm{D}_{\Delta}(k)\right)^{-1}\] \[\bm{D}_{\hat{X}}(k)=\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\bm{D}_{\hat{X}}(k,\ k-1)\] 在 \(k=1\) 时，滤波的理论数值应该为表 5.2 的第二栏所示，计算数值如表中第三栏所示。在计算中由于 \(\bm{D}_{\hat{X}}(0)\) 远大于 \(\bm{D}_w\) 和 \(\bm{D}_{\Delta}\)，当 \(\bm{D}_{\hat{X}}(0)+\bm{D}_w\) 或者 \(\bm{D}_{\hat{X}}(0)+\bm{D}_w+\bm{D}_{\Delta}\) 时，只存储了数值 \(\bm{D}_{\hat{X}}(0)\)，从而产生了截断误差。截断误差使计算数值与理论数值偏离，导致 \(\bm{D}_{\hat{X}}(1)=0\)，从而使方差矩阵失去了正定性。

例 5.3设某定常系统 \[\bm{\Phi}=\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\ ,\quad \bm{D}_{\hat{X}}(0)=\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\ ,\quad \bm{D}_w(k)=0\ ,\quad \bm{H}=\left[\begin{array}{ll}1 & 0\end{array}\right]\ ,\quad \bm{D}_{\Delta}=\varepsilon^2\ (\varepsilon\ll 1)\] 在计算过程中 \(1+\varepsilon^2=1\)。在 \(k=1\) 时刻时间预测的方差为 \[\bm{D}_{\hat{X}}(1,\ 0)=\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\] 测量更新为 \[\begin{aligned} \bm{K}_1&=\bm{D}_{\hat{X}}(1,\ 0)\bm{H}^{\mathrm{T}}\left(\bm{H}\bm{D}_{\hat{X}}(1,\ 0)\bm{H}^{\mathrm{T}}+\bm{D}_{\Delta}\right)^{-1}\\ &=\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\begin{bmatrix}1\\ 0\end{bmatrix} \left(\left[\begin{array}{ll}1 & 0\end{array}\right]\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\begin{bmatrix}1\\ 0\end{bmatrix}+\varepsilon^2\right)^{-1}\\ &=\begin{bmatrix}1\\ 0\end{bmatrix}\left(1+\varepsilon^2\right)^{-1}\\ &=\begin{bmatrix}1\\ 0\end{bmatrix} \end{aligned}\] \[\begin{aligned} \bm{D}_{\hat{X}}(1)&=\left(\bm{I}-\bm{K}_1\bm{H}\right)\bm{D}_{\hat{X}}(1,\ 0)\\ &=\left(\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}-\begin{bmatrix}1\\ 0\end{bmatrix}\left[\begin{array}{ll}1 & 0\end{array}\right]\right)\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix} =\begin{bmatrix}0 & 0\\ 0 & 1\end{bmatrix} \end{aligned}\] 在 \(k=2\) 时刻时间预测的方差为 \[\begin{aligned} \bm{D}_{\hat{X}}(2,\ 1)&=\bm{\Phi}\bm{D}_{\hat{X}}(1)\bm{\Phi}^{\mathrm{T}}\\ &=\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\begin{bmatrix}0 & 0\\ 0 & 1\end{bmatrix}\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix} =\begin{bmatrix}0 & 0\\ 0 & 1\end{bmatrix} \end{aligned}\] 测量更新为 \[\begin{aligned} \bm{K}_2&=\bm{D}_{\hat{X}}(2,\ 1)\bm{H}^{\mathrm{T}}\left(\bm{H}\bm{D}_{\hat{X}}(2,\ 1)\bm{H}^{\mathrm{T}}+\bm{D}_{\Delta}\right)^{-1}\\ &=\begin{bmatrix}0 & 0\\ 0 & 1\end{bmatrix}\begin{bmatrix}1\\ 0\end{bmatrix} \left(\left[\begin{array}{ll}1 & 0\end{array}\right]\begin{bmatrix}0 & 0\\ 0 & 1\end{bmatrix}\begin{bmatrix}1\\ 0\end{bmatrix}+\varepsilon^2\right)^{-1}\\ &=\begin{bmatrix}0\\ 0\end{bmatrix}\left(1+\varepsilon^2\right)^{-1}\\ &=\begin{bmatrix}0\\ 0\end{bmatrix} \end{aligned}\] \[\begin{aligned} \bm{D}_{\hat{X}}(2)&=\left(\bm{I}-\bm{K}_2\bm{H}\right)\bm{D}_{\hat{X}}(2,\ 1)\\ &=\left(\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}-\begin{bmatrix}0\\ 0\end{bmatrix}\left[\begin{array}{ll}1 & 0\end{array}\right]\right)\begin{bmatrix}0 & 0\\ 0 & 1\end{bmatrix} =\begin{bmatrix}0 & 0\\ 0 & 1\end{bmatrix} \end{aligned}\] 从上面的过程看，由于 \(1+\varepsilon^2=1\)，在 \(k=1\) 时，方差矩阵就失去了正定性，当 \(k=2\) 时，\(\bm{K}_2\) 为零矩阵，这样观测值就失去了对预测的更新作用，如果再递推下去，没有观测值更新的预测值会积累更多的误差从而使滤波发散。

平方根滤波

从前面的分析看，在数值计算中，当有数量级相差较大的数值相加减时，容易产生存储上的截断误差。此外，从数值计算分析知识可知，当矩阵的数值相差较大时，矩阵的条件数也越大，在矩阵求逆时有较差的数值稳定性，因此需要从数值计算上来改进 Kalman 滤波。我们知道，一个数值很大的数的平方根的数量级是原来数量级的一半；反之，数值很小的数的平方根是原来数量级的两倍，如 \(X=10^{-6}\sim 10^{6}\) 的平方根为 \(\sqrt{x}=10^{-3}\sim 10^{3}\)。对一个正定矩阵 \(\bm{D}\) 而言，矩阵的条件数为 \[\mathrm{C}(\bm{D})=\frac{|\gamma|_{\max}}{|\gamma|_{\min}} \tag{5.5.2}\] 其中，\(\gamma\) 表示矩阵 \(\bm{D}\) 的特征值，\(|\cdot|\) 表示绝对值。若矩阵 \(\bm{D}\) 为对称的正定矩阵，可作 Cholesky 矩阵分解 \[\bm{D}=\bm{S}\bm{S}^{\mathrm{T}} \tag{5.5.3}\] 其中 \(\bm{S}\) 为下三角矩阵，或上三角矩阵。式 (5.5.3) 的矩阵分解也称为矩阵的平方根分解，这样的分解是唯一的。分解后平方根矩阵 \(\bm{S}\) 的条件数为 \[\mathrm{C}(\bm{S})=\sqrt{\frac{|\gamma|_{\max}}{|\gamma|_{\min}}} \tag{5.5.4}\] 由此可以看出，用平方根矩阵来传递方差可以减小数值过大或者过小导致的计算误差，达到提高数值计算稳定性的目的。在滤波计算中，如果将方差矩阵分解为 \[\bm{D}_{\hat{X}}(k,\ k-1)=\bm{S}_{k,\ k-1}\bm{S}_{k,\ k-1}^{\mathrm{T}}\ ,\quad \bm{D}_{\hat{X}}(k)=\bm{S}_k\bm{S}_k^{\mathrm{T}} \tag{5.5.5}\] 以 \(\bm{S}_k\) 和 \(\bm{S}_{k,\ k-1}\) 的递推关系式来代替原来的 \(\bm{D}_{\hat{X}}(k)\) 和 \(\bm{D}_{\hat{X}}(k,\ k-1)\) 的递推关系式，则可以保证对于任意时刻 \(k\)，\(\bm{S}_k\bm{S}_k^{\mathrm{T}}\) 和 \(\bm{S}_{k,\ k-1}\bm{S}_{k,\ k-1}^{\mathrm{T}}\) 是对称非负定矩阵，从而减小了由于计算误差引起滤波发散的可能性。下面给出基于卡尔曼滤波的平方根滤波算法。

1. 平方根滤波的时间预测

Kalman 滤波的时间预测为 \[\hat{\bm{X}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1) \tag{5.5.6}\] \[\underset{n\times n}{\bm{D}_{\hat{X}}(k,\ k-1)}=\underset{n\times n}{\bm{\Phi}_{k,\ k-1}}\ \underset{n\times n}{\bm{D}_{\hat{X}}(k-1)}\ \underset{n\times n}{\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}} +\underset{n\times q}{\bm{\varGamma}_{k-1}}\ \underset{q\times q}{\bm{D}_w(k-1)}\ \underset{q\times n}{\bm{\varGamma}_{k-1}^{\mathrm{T}}} \tag{5.5.7}\] 假设已经对 \(\bm{D}_{\hat{X}}(k-1)\) 做三角分解得到下三角矩阵 \(\bm{S}_{k-1}\)，有 \[\bm{D}_{\hat{X}}(k-1)=\bm{S}_{k-1}\bm{S}_{k-1}^{\mathrm{T}} \tag{5.5.8}\] 下面推导如何由 \(\bm{S}_{k-1}\) 得到时间更新的 \(\bm{S}_{k,\ k-1}\)。将式 (5.5.7) 表达为 \[\bm{S}_{k,\ k-1}\bm{S}_{k,\ k-1}^{\mathrm{T}} =\bm{\Phi}_{k,\ k-1}\bm{S}_{k-1}\bm{S}_{k-1}^{\mathrm{T}}\bm{\Phi}_{k,\ k-1}^{\mathrm{T}} +\bm{\varGamma}_{k-1}\bm{D}_w(k-1)\bm{\varGamma}_{k-1}^{\mathrm{T}} \tag{5.5.9}\] 上式也可以表示为 \[\bm{S}_{k,\ k-1}\bm{S}_{k,\ k-1}^{\mathrm{T}} =\left[\begin{array}{ll}\bm{\Phi}_{k,\ k-1}\bm{S}_{k-1} & \bm{\varGamma}_{k-1}\bm{D}_w^{1/2}(k-1)\end{array}\right] \begin{bmatrix}\bm{S}_{k-1}^{\mathrm{T}}\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\\ \left(\bm{\varGamma}_{k-1}\bm{D}_w^{1/2}(k-1)\right)^{\mathrm{T}}\end{bmatrix} \tag{5.5.10}\] 其中，\(\bm{D}_w^{1/2}(k-1)\) 为 \(\bm{D}_w(k-1)\) 的平方根分解矩阵。设 \[\underset{(n+q)\times n}{\bm{A}}=\begin{bmatrix}\bm{S}_{k-1}^{\mathrm{T}}\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\\ \left(\bm{\varGamma}_{k-1}\bm{D}_w^{1/2}(k-1)\right)^{\mathrm{T}}\end{bmatrix} \tag{5.5.11}\] 显然 \(\bm{A}\) 不是方阵，不是 \(\bm{D}_{\hat{X}}(k,\ k-1)\) 的下三角矩阵。由于 \(\mathrm{rank}(\bm{A})=n\)，可将矩阵 \(\bm{A}\) 作 QR 正交分解 \[\underset{(n+q)\times n}{\bm{A}}=\underset{(n+q)\times(n+q)}{\bm{Q}}\ \underset{(n+q)\times n}{\bm{R}} \tag{5.5.12}\] 其中矩阵 \(\bm{Q}\) 为正交矩阵，即 \(\bm{Q}^{\mathrm{T}}\bm{Q}=\bm{I}\)；\(\bm{R}\) 为 \[\underset{(n+q)\times n}{\bm{R}}=\begin{bmatrix}\underset{n\times n}{\widetilde{\bm{R}}}\\ \underset{q\times n}{\bm{0}}\end{bmatrix} \tag{5.5.13}\] \(\widetilde{\bm{R}}\) 为上三角矩阵。由式 (5.5.9) 式 (5.5.12) 可以得到 \[\bm{S}_{k,\ k-1}\bm{S}_{k,\ k-1}^{\mathrm{T}}=\bm{A}^{\mathrm{T}}\bm{A}=\bm{R}^{\mathrm{T}}\bm{Q}^{\mathrm{T}}\bm{Q}\bm{R} =\bm{R}^{\mathrm{T}}\bm{R}=\widetilde{\bm{R}}^{\mathrm{T}}\widetilde{\bm{R}} \tag{5.5.14}\] 由于 \(\widetilde{\bm{R}}^{\mathrm{T}}\) 为下三角矩阵，而且矩阵的三角分解是唯一的，所以 \[\bm{S}_{k,\ k-1}=\widetilde{\bm{R}}^{\mathrm{T}} \tag{5.5.15}\] 可见，矩阵 \(\bm{A}\) 的 QR 正交分解中得到的 \(\widetilde{\bm{R}}^{\mathrm{T}}\) 即为时间预测的平方根矩阵 \(\bm{S}_{k,\ k-1}\)。

下面给出改进的 Gram-Schmidt 正交化方法（简称 MCS），实现由 \(\bm{S}_{k-1}\) 到 \(\bm{S}_{k,\ k-1}\) 的计算。

记 \[\bm{Q}=\left[\begin{array}{ll}\underset{(n+q)\times n}{\bm{Q}_1} & \underset{(n+q)\times q}{\bm{Q}_2}\end{array}\right] \tag{5.5.16}\] 容易得到 \[\bm{A}=\bm{Q}\bm{R}=\bm{Q}_1\widetilde{\bm{R}} \tag{5.5.17}\] 设 \[\bm{A}=\left[\begin{array}{lll}\bm{a}_1 & \bm{a}_2 & \cdots\quad \bm{a}_n\end{array}\right] \tag{5.5.18}\] \[\bm{Q}_1=\left[\begin{array}{lll}\bm{q}_1 & \bm{q}_2 & \cdots\quad \bm{q}_n\end{array}\right] \tag{5.5.19}\] \[\widetilde{\bm{R}}=\begin{bmatrix}c_{11} & K & c_{1n}\\ & \ddots & \vdots\\ 0 & & c_{nn}\end{bmatrix} \tag{5.5.20}\] 根据式 (5.5.17) 可以得到 \[\begin{cases} \bm{a}_1=c_{11}\bm{q}_1\\ \bm{a}_2=c_{12}\bm{q}_1+c_{22}\bm{q}_2\\ \quad\cdots\\ \bm{a}_m=c_{1m}\bm{q}_1+c_{2m}\bm{q}_2+\cdots c_{mm}\bm{q}_m\\ \quad\cdots\\ \bm{a}_n=c_{1n}\bm{q}_1+c_{2n}\bm{q}_2+\cdots c_{n-1,\ n}\bm{q}_{n-1}+c_{nn}\bm{q}_n \end{cases} \tag{5.5.21}\] 利用式 (5.5.21) 和 \(\bm{q}_j\)（\(j=1,\ \cdots,\ n\)）相互之间的正交性可以解得到 \(c_{mj}\)（\(m=1,\ \cdots n\)，\(j=1,\ \cdots,\ n\)）。下面以简代码的形式给出 \(c_{mj}\) 的计算步骤：

通过以上计算得到 \(c_{mj}\)（\(m=1,\ \cdots n\)，\(j=1,\ \cdots,\ n\)）后，就得到了三角矩阵 \(\bm{S}_{k,\ k-1}\)。

2. 平方根滤波的观测值逐次更新

平方根滤波的测量更新是已知 \(\bm{S}_{k,\ k-1}\) 求 \(\bm{S}_k\)。这里介绍平方根滤波的逐次测量更新的 Potter 算法。当量测为标量，即 \(\bm{Z}(k)\) 为一维，\(\ell=1\)，则量测方程为 \[\underset{1\times 1}{Z(k)}=\underset{1\times n}{\bm{h}_k}\ \underset{n\times 1}{\bm{X}(k)}+\underset{1\times 1}{\Delta(k)} \tag{5.5.22}\] 式中 \(E\left[\,\Delta(k)\,\right]=0\)，\(\mathrm{Var}\left(\,\Delta(k)\,\right)=d_{\Delta}(k)\)。由第 4 章中 Kalman 滤波估计公式，有 \[\bm{K}_k=\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}}+d_{\Delta}(k)\right)^{-1} \tag{5.5.23}\] 测量更新为 \[\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{5.5.24}\] \[\bm{D}_{\hat{X}}(k)=\left(\bm{I}-\bm{K}_k\bm{h}_k\right)\bm{D}_{\hat{X}}(k,\ k-1) \tag{5.5.25}\] 将式 (5.5.23) 代入式 (5.5.25) 中可得 \[\bm{D}_{\hat{X}}(k)=\bm{D}_{\hat{X}}(k,\ k-1) -\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}}+d_{\Delta}(k)\right)^{-1}\bm{h}_k\bm{D}_{\hat{X}}(k,\ k-1) \tag{5.5.26}\] 将 \(\bm{D}_{\hat{X}}(k)\) 和 \(\bm{D}_{\hat{X}}(k,\ k-1)\) 分别用平方根形式表示 \[\begin{aligned} \bm{S}_k\bm{S}_k^{\mathrm{T}}&=\bm{S}_{k,\ k-1}\bm{S}_{k,\ k-1}^{\mathrm{T}} -\bm{S}_{k,\ k-1}\bm{S}_{k,\ k-1}^{\mathrm{T}}\bm{H}_k^{\mathrm{T}} \left(\bm{h}_k\bm{S}_{k,\ k-1}\bm{S}_{k,\ k-1}^{\mathrm{T}}\bm{H}_k^{\mathrm{T}}+d_{\Delta}(k)\right)^{-1} \bm{h}_k\bm{S}_{k,\ k-1}\bm{S}_{k,\ k-1}^{\mathrm{T}}\\ &=\bm{S}_{k,\ k-1}\left[\,\bm{I}-\bm{S}_{k,\ k-1}^{\mathrm{T}}\bm{H}_k^{\mathrm{T}} \left(\bm{h}_k\bm{S}_{k,\ k-1}\bm{S}_{k,\ k-1}^{\mathrm{T}}\bm{H}_k^{\mathrm{T}}+d_{\Delta}(k)\right)^{-1} \bm{h}_k\bm{S}_{k,\ k-1}\,\right]\bm{S}_{k,\ k-1}^{\mathrm{T}} \end{aligned} \tag{5.5.27}\] 令 \[\bm{a}_k=\left(\bm{h}_k\bm{S}_{k,\ k-1}\right)^{\mathrm{T}} \tag{5.5.28}\] 式 (5.5.27) 成为 \[\bm{S}_k\bm{S}_k^{\mathrm{T}}=\bm{S}_{k,\ k-1}\left[\,\bm{I}-\bm{a}_k\left(\bm{a}_k^{\mathrm{T}}\bm{a}_k+d_{\Delta}(k)\right)^{-1}\bm{a}_k^{\mathrm{T}}\,\right]\bm{S}_{k,\ k-1}^{\mathrm{T}} \tag{5.5.29}\] 再令 \[b_k=\left[\,\bm{a}_k^{\mathrm{T}}\bm{a}_k+d_{\Delta}(k)\,\right]^{-1} \tag{5.5.30}\] 由于 \(\bm{h}_k\) 为 \(1\times n\) 的向量，所以 \(\left(\bm{a}_k^{\mathrm{T}}\bm{a}_k+d_{\Delta}(k)\right)^{-1}\) 为一标量，得到 \[\bm{S}_k\bm{S}_k^{\mathrm{T}}=\bm{S}_{k,\ k-1}\left[\,\bm{I}-b_k\bm{a}_k\bm{a}_k^{\mathrm{T}}\,\right]\bm{S}_{k,\ k-1}^{\mathrm{T}} \tag{5.5.31}\] 如果能将上式的 \(\left[\,\bm{I}-b_k\bm{a}_k\bm{a}_k^{\mathrm{T}}\,\right]\) 分解为 \[\left[\,\bm{I}-b_k\bm{a}_k\bm{a}_k^{\mathrm{T}}\,\right]=\bm{F}\bm{F}^{\mathrm{T}}\] 那么式 (5.5.31) 为 \[\bm{S}_k\bm{S}_k^{\mathrm{T}}=\bm{S}_{k,\ k-1}\bm{F}\bm{F}^{\mathrm{T}}\bm{S}_{k,\ k-1}^{\mathrm{T}} \tag{5.5.32}\] 由于矩阵的平方根分解是唯一的，所以 \(\bm{S}_{k,\ k-1}\bm{F}\) 一定是 \(\bm{S}_k\)。

设将 \(\left[\,\bm{I}-b_k\bm{a}_k\bm{a}_k^{\mathrm{T}}\,\right]\) 分解为 \[\bm{I}-b_k\bm{a}_k^{\mathrm{T}}\bm{a}_k=\left[\,\bm{I}-\gamma_kb_k\bm{a}_k\bm{a}_k^{\mathrm{T}}\,\right]\left[\,\bm{I}-\gamma_kb_k\bm{a}_k\bm{a}_k^{\mathrm{T}}\,\right]^{\mathrm{T}} \tag{5.5.33}\] 式中 \(\gamma_k\) 为待定的标量。展开上式的右侧，得 \[\bm{I}-b_k\bm{a}_k\bm{a}_k^{\mathrm{T}}=\bm{I}-2b_k\gamma_k\bm{a}_k\bm{a}_k^{\mathrm{T}} +b_k^2\gamma_k^2\left(\bm{a}_k\bm{a}_k^{\mathrm{T}}\right)\left(\bm{a}_k\bm{a}_k^{\mathrm{T}}\right) \tag{5.5.34}\] 得到 \[\left(\bm{I}-2\gamma_k+b_k\gamma_k^2\bm{a}_k\bm{a}_k^{\mathrm{T}}\right)b_k\bm{a}_k\bm{a}_k^{\mathrm{T}}=0 \tag{5.5.35}\] 解得 \[\gamma_k=\frac{1\pm\sqrt{1-b_k\bm{a}_k^{\mathrm{T}}\bm{a}_k}}{b_k\bm{a}_k^{\mathrm{T}}\bm{a}_k} \tag{5.5.36}\] 上式中的“\(\pm\)”无论取“\(+\)”还是“\(-\)”均可以满足式 (5.5.35)。式 (5.5.36) 也可以表达为 \[\gamma_k=\frac{1}{1\pm\sqrt{d_{\Delta}(k)b_k}} \tag{5.5.37}\] 与式 (5.5.36) 比较，式 (5.5.37) 避免了在平方根内的减法运算。为了避免出现 \(\gamma_k\rightarrow\infty\)，上式取“\(+\)”，即 \[\gamma_k=\frac{1}{1+\sqrt{d_{\Delta}(k)b_k}} \tag{5.5.38}\] 将 \(\gamma_k\) 代入 \(\left[\,\bm{I}-\gamma_kb_k\bm{a}_k\bm{a}_k^{\mathrm{T}}\,\right]\)，矩阵 \(\left[\,\bm{I}-\gamma_kb_k\bm{a}_k\bm{a}_k^{\mathrm{T}}\,\right]\) 即为 \(\bm{F}\)，这时就得到了 \(\bm{S}_{k,\ k-1}\) 递推 \(\bm{S}_k\) 的计算 \[\bm{S}_k=\bm{S}_{k,\ k-1}\left[\,\bm{I}-\gamma_kb_k\bm{a}_k\bm{a}_k^{\mathrm{T}}\,\right] \tag{5.5.39}\] 增益矩阵为 \[\begin{aligned} \bm{K}_k&=\bm{S}_{k,\ k-1}\bm{S}_{k,\ k-1}^{\mathrm{T}}\bm{h}_k^{\mathrm{T}} \left(\bm{h}_k\bm{S}_{k,\ k-1}\bm{S}_{k,\ k-1}^{\mathrm{T}}\bm{h}_k^{\mathrm{T}}+d_{\Delta}(k)\right)^{-1}\\ &=b_k\bm{S}_{k,\ k-1}\bm{a}_k \end{aligned} \tag{5.5.40}\] 综上所述，平方根滤波的量测更新方程为 \[\begin{aligned} \bm{a}_k&=\left(\bm{h}_k\bm{S}_{k,\ k-1}\right)^{\mathrm{T}} \tag{5.5.41}\\ b_k&=\left[\,\bm{a}_k^{\mathrm{T}}\bm{a}_k+d_{\Delta}(k)\,\right]^{-1} \tag{5.5.42}\\ \gamma_k&=\frac{1}{1+\sqrt{d_{\Delta}(k)b_k}} \tag{5.5.43}\\ \bm{K}_k&=b_k\bm{S}_{k,\ k-1}\bm{a}_k \tag{5.5.44}\\ \bm{S}_k&=\bm{S}_{k,\ k-1}-\gamma_k\bm{K}_k\bm{a}_k^{\mathrm{T}} \tag{5.5.45}\\ \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{5.5.46} \end{aligned}\] 若测量向量 \(\bm{Z}(k)\) 为 \(\ell\)（\(\ell>1\)）维，对于 \(\ell\) 维独立量测的情况，量测向量和量测噪声方差阵分别为 \[\bm{Z}(k)=\left[\begin{array}{lllll}Z_1(k) & \cdots & Z_j(k) & \cdots & Z_{\ell}(k)\end{array}\right] \tag{5.5.47}\] \[\bm{D}_{\Delta}(k)=\mathrm{diag}\left[\begin{array}{lllll}d_{\Delta}^1(k) & \cdots & d_{\Delta}^j(k) & \cdots & d_{\Delta}^{\ell}(k)\end{array}\right] \tag{5.5.48}\] 如观测值不相互独立，采用 Cholesky 分解方法去相关，去相关后的观测噪声矩阵为对角阵。设量测矩阵为 \[\bm{H}_k=\begin{bmatrix}\bm{h}_k^1\\ \vdots\\ \bm{h}_k^j\\ \vdots\\ \bm{h}_k^{\ell}\end{bmatrix} \tag{5.5.49}\] 其中，\(\bm{h}_k^j\) 表示 \(\bm{H}_k\) 中的第 \(j\) 个观测值 \(Z_j(k)\) 对应的行向量，对应的观测噪声为 \(d_{\Delta}^j(k)\)，这样就可以用观测值逐次更新的方法对状态进行量测更新。

下面汇总观测值逐次更新的平方根滤波公式。

由一步预测已经得到 \(\hat{\bm{X}}(k,\ k-1)\) 和 \(\bm{S}_{k,\ k-1}\)，则 \(k\) 时刻的测量更新步骤为：

取 \[\begin{cases} \hat{\bm{X}}^0(k)=\hat{\bm{X}}(k,\ k-1)\\ \bm{S}_k^0=\bm{S}_{k,\ k-1} \end{cases} \tag{5.5.50}\] 对于 \(j=1,\ 2,\ \cdots,\ \ell\)，进行以下的迭代计算 \[\begin{aligned} \bm{a}_k&=\left(\bm{h}_k^j\bm{S}_k^{j-1}\right)^{\mathrm{T}} \tag{5.5.51}\\ b_k&=\left[\,\left(\bm{a}_k\right)^{\mathrm{T}}\bm{a}_k+d_{\Delta}^j(k)\,\right]^{-1} \tag{5.5.52}\\ \gamma_k&=\frac{1}{1+\sqrt{d_{\Delta}(k)b_k}} \tag{5.5.53}\\ \bm{K}_k^j&=b_k\bm{S}_k^{j-1}\bm{a}_k \tag{5.5.54}\\ \bm{S}_k^j&=\bm{S}_k^{j-1}-\gamma_k\bm{K}_k^j\bm{a}_k^{\mathrm{T}} \tag{5.5.55}\\ \hat{\bm{X}}^j(k)&=\hat{\bm{X}}^{j-1}(k)+\bm{K}_k^j\left(Z_j(k)-\bm{h}_k^j\hat{\bm{X}}^{j-1}(k)\right) \tag{5.5.56} \end{aligned}\] 当 \(j=\ell\) 时，即获得 \(k\) 时刻所有观测值对状态更新后的结果 \[\begin{cases} \hat{\bm{X}}(k)=\hat{\bm{X}}^{\ell}(k)\\ \bm{S}_k=\bm{S}_k^{\ell} \end{cases} \tag{5.5.57}\] 综合以上各式，得到的平方根滤波递推公式见表 5.3。

例 5.4在例 5.3 中，设某定常系统 \[\bm{\Phi}=\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\ ,\quad \bm{D}_{\hat{X}}(0)=\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\ ,\quad \bm{D}_w(k)=0\ ,\quad \bm{H}=\left[\begin{array}{ll}1 & 0\end{array}\right]\ ,\quad \bm{D}_{\Delta}=\varepsilon^2(\varepsilon\ll 1)\] 由于 \(1+\varepsilon^2=1\)，在计算过程的截断误差导致方差矩阵失去正定并且增益矩阵为零矩阵，这里用平方根分解实现滤波计算。

首先，容易得到 \(\bm{D}_{\hat{X}}(1,\ 0)=\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\) 的平方根分解矩阵 \[\bm{S}_{1,\ 0}=\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\] 然后，进行测量更新 \[\bm{a}_1=\left(\bm{H}\bm{S}_{1,\ 0}\right)^{\mathrm{T}} =\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\begin{bmatrix}1\\ 0\end{bmatrix}=\begin{bmatrix}1\\ 0\end{bmatrix}\] \[b_1=\left[\,\bm{a}_1^{\mathrm{T}}\bm{a}_1+\bm{D}_{\Delta}\,\right]^{-1} =\left[\,\left[\begin{array}{ll}1 & 0\end{array}\right]\begin{bmatrix}1\\ 0\end{bmatrix}+\varepsilon^2\,\right]^{-1} =\frac{1}{1+\varepsilon^2}=1\] \[\gamma_1=\frac{1}{1+\sqrt{D_{\Delta}(1)b_1}}=\frac{1}{1+\varepsilon}\] \[\bm{K}_1=b_1\bm{S}_{1,\ 0}\bm{a}_1 =\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\begin{bmatrix}1\\ 0\end{bmatrix}=\begin{bmatrix}1\\ 0\end{bmatrix}\] \[\bm{S}_1=\bm{S}_{1,\ 0}-\gamma_1\bm{K}_1\bm{a}_1^{\mathrm{T}} =\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix} -\frac{1}{1+\varepsilon}\begin{bmatrix}1\\ 0\end{bmatrix}\left[\begin{array}{ll}1 & 0\end{array}\right] =\begin{bmatrix}\dfrac{\varepsilon}{1+\varepsilon} & 0\\[8pt] 0 & 1\end{bmatrix}\] \[\bm{D}_{\hat{X}}(1)=\bm{S}_1\bm{S}_1^{\mathrm{T}} =\begin{bmatrix}\dfrac{\varepsilon}{1+\varepsilon} & 0\\[8pt] 0 & 1\end{bmatrix} \begin{bmatrix}\dfrac{\varepsilon}{1+\varepsilon} & 0\\[8pt] 0 & 1\end{bmatrix} =\begin{bmatrix}\dfrac{\varepsilon^2}{(1+\varepsilon)^2} & 0\\[8pt] 0 & 1\end{bmatrix}\] 从上述计算过程可以看出，\(\varepsilon^2\) 是计算机的存储精度，在标准 Kalman 滤波中，当 \(\varepsilon^2\) 与其他数值相加减时，\(\varepsilon^2\) 并没有被存储进去。但在平方根分解滤波中，\(\varepsilon^2\) 被分解为 \(\varepsilon\)，当计算 \(1+\varepsilon\) 时，\(\varepsilon\) 仍然能够被存储下来从而将数值传递下去；又由于 \(\bm{D}=\bm{S}\bm{S}^{\mathrm{T}}\)，方差矩阵在递推时总能够保持正定性。

UDU 分解滤波

平方根滤波可以有效克服 Kalman 滤波在递推过程中出现的数值发散，但在运算中有如式 (5.5.53) 的开方运算，这给滤波的递推加重了计算负担。这里介绍无平方根运算的滤波递推算法——UDU 分解滤波。如果滤波过程中 \(\bm{D}_{\hat{X}}(k)\) 和 \(\bm{D}_{\hat{X}}(k,\ k-1)\) 为非负定阵，\(\bm{D}_{\hat{X}}(k)\) 和 \(\bm{D}_{\hat{X}}(k,\ k-1)\) 可分解成 \[\bm{D}_{\hat{X}}(k,\ k-1)=\bm{U}_{k,\ k-1}\bm{D}_{k,\ k-1}\bm{U}_{k,\ k-1}^{\mathrm{T}} \tag{5.6.1}\] \[\bm{D}_{\hat{X}}(k)=\bm{U}_k\bm{D}_k\bm{U}_k^{\mathrm{T}} \tag{5.6.2}\] 其中，\(\bm{D}_k\) 和 \(\bm{D}_{k,\ k-1}\) 为对角矩阵，\(\bm{U}_k\) 和 \(\bm{U}_{k,\ k-1}\) 为 \(n\times n\) 的单位上三角阵，主对角元全为 \(1\)。UDU 分解滤波就是用 \(\bm{D}_k\)、\(\bm{D}_{k,\ k-1}\)、\(\bm{U}_k\) 和 \(\bm{U}_{k,\ k-1}\) 来完成数值递推，这不仅确保了数值传递的精度，也避免了开方运算。在下面的介绍中，为便于叙述，在时间预测中，用 \(\widetilde{\bm{U}}\) 和 \(\widetilde{\bm{D}}\) 来代替 \(\bm{U}_{k,\ k-1}\) 和 \(\bm{D}_{k,\ k-1}\)；在测量更新中，用 \(\hat{\bm{U}}\) 和 \(\hat{\bm{D}}\) 来代替 \(\bm{U}_k\) 和 \(\bm{D}_k\)，那么式 (5.6.1) 和 (5.6.2) 为

\[\bm{D}_{\hat{X}}(k,\ k-1)=\widetilde{\bm{U}}\widetilde{\bm{D}}\widetilde{\bm{U}}^{\mathrm{T}}\ ,\quad \bm{D}_{\hat{X}}(k)=\hat{\bm{U}}\hat{\bm{D}}\hat{\bm{U}}^{\mathrm{T}} \tag{5.6.3}\] 其中， \[\widetilde{\bm{U}}=\begin{bmatrix}1 & \widetilde{u}_{12} & \cdots & \widetilde{u}_{1n}\\ 0 & 1 & \cdots & \widetilde{u}_{2n}\\ \vdots & \vdots & & \vdots\\ 0 & & & 1\end{bmatrix}\ ,\quad \widetilde{\bm{D}}=\begin{bmatrix}\widetilde{d}_1 & & & \\ & \widetilde{d}_2 & & \\ & & \ddots & \\ & & & \widetilde{d}_n\end{bmatrix} \tag{5.6.4}\] \[\hat{\bm{U}}=\begin{bmatrix}1 & \hat{u}_{12} & \cdots & \hat{u}_{1n}\\ 0 & 1 & \cdots & \hat{u}_{2n}\\ \vdots & \vdots & & \vdots\\ 0 & & & 1\end{bmatrix}\ ,\quad \hat{\bm{D}}=\begin{bmatrix}\hat{d}_1 & & & \\ & \hat{d}_2 & & \\ & & \ddots & \\ & & & \hat{d}_n\end{bmatrix} \tag{5.6.5}\]

UDU\(^{\mathrm{T}}\) 分解滤波的时间预测

UDU 分解滤波的时间预测是已知 \(\hat{\bm{U}}\) 和 \(\hat{\bm{D}}\)，求 \(\widetilde{\bm{U}}\) 和 \(\widetilde{\bm{D}}\)。下面给出 Thornton 的 UDU 分解滤波，其中矩阵分解采用的是 Gram-Schimit 正交分解方法。

时间预测的方差矩阵为 \[\bm{D}_{\hat{X}}(k,\ k-1)=\bm{\Phi}\bm{D}_{\hat{X}}(k-1)\bm{\Phi}^{\mathrm{T}}+\bm{\varGamma}_{k-1}\bm{D}_w(k-1)\bm{\varGamma}_{k-1}^{\mathrm{T}} \tag{5.6.6}\] 将式 (5.6.3) 代入上式，为了表达简单起见，用 \(\bm{Q}\) 代替 \(\bm{D}_w(k-1)\)，用 \(\bm{\varGamma}\) 代替 \(\bm{\varGamma}_{k-1}\)，上式可以表示为 \[\begin{aligned} \widetilde{\bm{U}}\widetilde{\bm{D}}\widetilde{\bm{U}}^{\mathrm{T}} &=\bm{\Phi}\hat{\bm{U}}\hat{\bm{D}}\hat{\bm{U}}^{\mathrm{T}}\bm{\Phi}^{\mathrm{T}}+\bm{\varGamma}\bm{Q}\bm{\varGamma}^{\mathrm{T}}\\ &=\left[\begin{array}{ll}\bm{\Phi}\hat{\bm{U}} & \bm{\varGamma}\end{array}\right] \begin{bmatrix}\hat{\bm{D}} & \\ & \bm{Q}\end{bmatrix} \begin{bmatrix}\left(\bm{\Phi}\hat{\bm{U}}\right)^{\mathrm{T}}\\ \bm{\varGamma}^{\mathrm{T}}\end{bmatrix} \end{aligned} \tag{5.6.7}\] 设 \[\bm{A}=\begin{bmatrix}\left(\bm{\Phi}\hat{\bm{U}}\right)^{\mathrm{T}}\\ \bm{\varGamma}^{\mathrm{T}}\end{bmatrix} =\left[\begin{array}{llll}\bm{a}_1 & \bm{a}_2 & \cdots & \bm{a}_n\end{array}\right] \tag{5.6.8}\] \[\bm{D}=\begin{bmatrix}\hat{\bm{D}} & \\ & \bm{Q}\end{bmatrix} \tag{5.6.9}\] 其中，\(\bm{a}_i\) 为有个 \((n+q)\) 元素的列向量，\(\bm{D}\) 为 \((n+q)\times(n+q)\) 的矩阵。下面以简代码形式给出已知 \(\hat{\bm{U}}\) 和 \(\hat{\bm{D}}\)，求 \(\widetilde{\bm{U}}\) 和 \(\widetilde{\bm{D}}\) 的计算步骤。

UDU\(^{\mathrm{T}}\) 分解滤波的测量更新

UDU 分解滤波的测量更新是已知 \(\widetilde{\bm{U}}\) 和 \(\widetilde{\bm{D}}\)，求 \(\hat{\bm{U}}\) 和 \(\hat{\bm{D}}\)。这里给出 Thornton(1976) 和 Bierman(1977) 提出的无开方运算的观测值逐次更新方法。

设有量测方程为 \[\underset{1\times 1}{Z(k)}=\underset{1\times n}{\bm{h}_k}\ \underset{n\times 1}{\bm{X}(k)}+\underset{1\times 1}{\Delta(k)} \tag{5.6.10}\] 式中，\(E\left[\,\Delta(k)\,\right]=0\)，\(\mathrm{Var}\left(\,\Delta(k)\,\right)=d_{\Delta}\)。滤波测量更新的方差为 \[\bm{D}_{\hat{X}}(k)=\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\bm{D}_{\hat{X}}(k,\ k-1) \tag{5.6.11}\] 将式 (5.6.3) 和增益矩阵代入上式，得到 \[\hat{\bm{U}}\hat{\bm{D}}\hat{\bm{U}}^{\mathrm{T}} =\widetilde{\bm{U}}\widetilde{\bm{D}}\widetilde{\bm{U}}^{\mathrm{T}} -\widetilde{\bm{U}}\widetilde{\bm{D}}\widetilde{\bm{U}}^{\mathrm{T}}\bm{h}_k^{\mathrm{T}} \left(\bm{h}_k\widetilde{\bm{U}}\widetilde{\bm{D}}\widetilde{\bm{U}}^{\mathrm{T}}\bm{h}_k^{\mathrm{T}}+d_{\Delta}\right)^{-1} \bm{h}_k\widetilde{\bm{U}}\widetilde{\bm{D}}\widetilde{\bm{U}}^{\mathrm{T}} \tag{5.6.12}\] 设 \[\alpha=\left(\bm{h}_k\widetilde{\bm{U}}\widetilde{\bm{D}}\widetilde{\bm{U}}^{\mathrm{T}}\bm{h}_k^{\mathrm{T}}+d_{\Delta}\right)^{-1} \tag{5.6.13}\] 当只有一个观测值时，\(\alpha\) 为标量，式 (5.6.12) 为 \[\hat{\bm{U}}\hat{\bm{D}}\hat{\bm{U}}^{\mathrm{T}} =\widetilde{\bm{U}}\left(\widetilde{\bm{D}}-\alpha\widetilde{\bm{D}}\widetilde{\bm{U}}^{\mathrm{T}}\bm{h}_k^{\mathrm{T}}\bm{h}_k\widetilde{\bm{U}}\widetilde{\bm{D}}\right)\widetilde{\bm{U}}^{\mathrm{T}} \tag{5.6.14}\] 设 \[\bm{f}=\widetilde{\bm{U}}^{\mathrm{T}}\bm{h}_k^{\mathrm{T}}=\begin{bmatrix}f_1\\ \vdots\\ f_n\end{bmatrix}\ ,\quad \bm{V}=\widetilde{\bm{D}}\bm{f}=\begin{bmatrix}v_1\\ \vdots\\ v_n\end{bmatrix} =\begin{bmatrix}\widetilde{d}_1f_1\\ \vdots\\ \widetilde{d}_nf_n\end{bmatrix} \tag{5.6.15}\] 式 (5.6.14) 为 \[\hat{\bm{U}}\hat{\bm{D}}\hat{\bm{U}}^{\mathrm{T}} =\widetilde{\bm{U}}\left(\widetilde{\bm{D}}-\alpha\bm{V}\bm{V}^{\mathrm{T}}\right)\widetilde{\bm{U}}^{\mathrm{T}} \tag{5.6.16}\] 如果将 \(\left(\widetilde{\bm{D}}-\alpha\bm{V}\bm{V}^{\mathrm{T}}\right)\) 分解为 \[\left(\widetilde{\bm{D}}-\alpha\bm{V}\bm{V}^{\mathrm{T}}\right)=\bm{M}\bm{N}\bm{M}^{\mathrm{T}} \tag{5.6.17}\] 其中，\(\bm{M}\) 为单位上三角矩阵，\(\bm{N}\) 为对角矩阵。将式 (4.6.17) 代入式 (5.6.16) 为 \[\hat{\bm{U}}\hat{\bm{D}}\hat{\bm{U}}^{\mathrm{T}}=\widetilde{\bm{U}}\bm{M}\bm{N}\bm{M}^{\mathrm{T}}\widetilde{\bm{U}}^{\mathrm{T}} \tag{5.6.18}\]

由于一个单位上三角矩阵乘以另一个单位上三角矩阵，仍然为单位上三角矩阵，又由于 UDU 分解的唯一性，所以 \[\hat{\bm{U}}=\widetilde{\bm{U}}\bm{M} \tag{5.6.19}\] \[\hat{\bm{D}}=\bm{N}\] 最后，给出以简代码形式给出 Bierman 的 UDU 测量更新计算步骤：

以上步骤除了输出 \(\widetilde{\bm{U}}\) 和 \(\hat{\bm{D}}\) 外，还得到了 \[\bm{b}=\left[\begin{array}{lll}b_1 & \cdots & b_n\end{array}\right]^{\mathrm{T}} \tag{5.6.21}\] 接着对状态进行更新 \[\begin{aligned} \bm{K}_k&=\bm{b}/\alpha_n\\ \hat{\bm{X}}(k)&=\hat{\bm{X}}(k,\ k-1)+\bm{K}_k\left[\,Z_j(k)-\bm{h}_k\hat{\bm{X}}(k)\,\right] \end{aligned} \tag{5.6.22}\] 以上是对一个观测值 \(Z(k)\) 的测量更新。当有多个观测值时，实施观测值逐次更新，将上一个观测更新的结果代替式 (5.6.15)，即 \[\begin{aligned} \hat{\bm{U}}&\rightarrow\widetilde{\bm{U}}\\ \hat{\bm{D}}&\rightarrow\widetilde{\bm{D}} \end{aligned} \tag{5.6.23}\] 并且 \[\hat{\bm{X}}(k)\rightarrow\hat{\bm{X}}(k,\ k-1) \tag{5.6.24}\] 重复以上过程，直到 \(k\) 时刻的所有观测值更新完毕。\(\hat{\bm{X}}(k)\) 的方差为最后一个观测值更新后的结果 \[\bm{D}_{\hat{X}}(k)=\hat{\bm{U}}\hat{\bm{D}}\hat{\bm{U}}^{\mathrm{T}} \tag{5.6.25}\]

平方根信息滤波

平方根信息滤波（Square Root Information Filtering，SRIF）是以信息矩阵 \(\bm{W}\) 代替方差矩阵的平方根滤波，平方根信息滤波与 Kalman 滤波在理论上是等价的，但由于用 \(\bm{W}\) 的平方根进行滤波递推，减小了滤波计算误差。

离散线性系统的函数模型为 \[\underset{n\times 1}{\bm{X}(k)}=\underset{n\times n}{\bm{\Phi}_{k,\ k-1}}\ \underset{n\times 1}{\bm{X}(k-1)} +\underset{n\times q}{\bm{\varGamma}_{k-1}}\ \underset{q\times 1}{\bm{w}(k-1)} \tag{5.7.1}\] \[\bm{Z}(k)=\bm{H}_k\bm{X}(k)+\bm{\Delta}(k) \tag{5.7.2}\] 随机模型为 \[\begin{gathered} E\left[\,\bm{w}(k)\,\right]=\overline{\bm{w}}(k)\ ,\quad \mathrm{Cov}\left[\,\bm{w}(k),\ \bm{w}(j)\,\right]=\bm{D}_w(k)\delta(k-j)\\ E\left[\,\bm{\Delta}(k)\,\right]=\bm{0}\ ,\quad \mathrm{Cov}\left[\,\bm{\Delta}(k),\ \bm{\Delta}(j)\,\right]=\bm{I}\delta(k-j)\\ \mathrm{Cov}\left[\,\bm{w}(k),\ \bm{\Delta}(j)\,\right]=0 \end{gathered} \tag{5.7.3}\] 其中，\(\overline{\bm{w}}(k)\) 为系统噪声的均值，它可以是零也可以不是零。观测噪声的方差为 \(\bm{I}\)，表明观测向量的每个分量的方差均为 \(1\) 并且互相独立。如果观测向量的噪声方差不是单位矩阵，可以根据 5.1.2 节的方法对观测向量进行标准化。

为了表述方便，这里设 \(\bm{S}_{w_k}\) 为 \(\bm{D}_w(k)\) 的平方根矩阵，即 \[\bm{D}_w(k)=\bm{S}_{w_k}\bm{S}_{w_k}^{\mathrm{T}} \tag{5.7.4}\] 同样，\(\bm{D}_{\hat{X}}(k,\ k-1)\) 和 \(\bm{D}_{\hat{X}}(k)\) 分解为 \[\bm{D}_{\hat{X}}(k,\ k-1)=\bm{S}_{k,\ k-1}\bm{S}_{k,\ k-1}^{\mathrm{T}}\ ,\quad \bm{D}_{\hat{X}}(k)=\bm{S}_k\bm{S}_k^{\mathrm{T}} \tag{5.7.5}\] 下面推导的顺序为：在 \(t_k\) 时刻，观测值 \(\bm{Z}(k)\) 对预测 \(\hat{\bm{X}}(k,\ k-1)\) 的测量更新 \(\hat{\bm{X}}(k)\)，在 \(t_k\) 时刻对 \(t_{k+1}\) 时刻的时间预测 \(\hat{\bm{X}}(k+1,\ k)\)，在 \(t_{k+1}\) 时刻，观测值 \(\bm{Z}(k+1)\) 对 \(\hat{\bm{X}}(k+1,\ k)\) 的测量更新 \(\hat{\bm{X}}(k+1)\)。

平方根信息滤波的推导

1. \(t_k\) 时刻的测量更新

在 \(t_k\) 时刻，已知时间预测 \(\hat{\bm{X}}(k,\ k-1)\)，\(\bm{D}_{\hat{X}}(k,\ k-1)\)，如果将 \(\hat{\bm{X}}(k,\ k-1)\) 视为 \(\bm{X}(k)\) 的虚拟观测值，那么虚拟观测方程和随机模型为 \[\begin{aligned} \hat{\bm{X}}(k,\ k-1)&=\bm{X}(k)+\bm{\Delta}_{\hat{X}}(k,\ k-1)\ ,\quad E\left[\,\bm{\Delta}_{\hat{X}}(k,\ k-1)\,\right]=\bm{0}\\ E&\left[\,\bm{\Delta}_{\hat{X}}(k,\ k-1)\bm{\Delta}_{\hat{X}}^{\mathrm{T}}(k,\ k-1)\,\right]=\bm{D}_{\hat{X}}(k,\ k-1) \end{aligned} \tag{5.7.6}\] \[\bm{\Delta}_{\hat{X}}(k,\ k-1)\sim\left[\,\bm{0},\ \bm{D}_{\hat{X}}(k,\ k-1)\,\right] \tag{5.7.7}\] 上式的 \(\left[\,\bm{0},\ \bm{D}_{\hat{X}}(k,\ k-1)\,\right]\) 表示期望为 \(\bm{0}\)，方差为 \(\bm{D}_{\hat{X}}(k,\ k-1)\) 的白噪声序列。将式 (5.7.6) 中的第一式左乘 \(\bm{S}_{k,\ k-1}^{-1}\) \[\bm{S}_{k,\ k-1}^{-1}\hat{\bm{X}}(k,\ k-1)=\bm{S}_{k,\ k-1}^{-1}\bm{X}(k)+\bm{S}_{k,\ k-1}^{-1}\bm{\Delta}_{\hat{X}}(k,\ k-1)\] 并设 \[\widetilde{\bm{L}}_{\hat{X}}(k)=\bm{S}_{k,\ k-1}^{-1}\hat{\bm{X}}(k,\ k-1)\ ,\quad \overline{\bm{\Delta}}_{\hat{X}}(k)=\bm{S}_{k,\ k-1}^{-1}\bm{\Delta}_{\hat{X}}(k,\ k-1) \tag{5.7.8}\] 那么 \[\widetilde{\bm{L}}_{\hat{X}}(k)=\bm{S}_{k,\ k-1}^{-1}\bm{X}(k)+\overline{\bm{\Delta}}_{\hat{X}}(k) \tag{5.7.9}\] 易知 \[\overline{\bm{\Delta}}_{\hat{X}}(k)\sim\left[\,\bm{0},\ \bm{I}\,\right] \tag{5.7.10}\] 将在 \(t_k\) 时刻所有的误差项放在一起 \[\begin{cases} -\overline{\bm{\Delta}}_{\hat{X}}(k)=\bm{S}_{k,\ k-1}^{-1}\bm{X}(k)-\widetilde{\bm{L}}_{\hat{X}}(k)\\[4pt] -\bm{\Delta}(k)=\bm{H}_k\bm{X}(k)-\bm{Z}(k) \end{cases} \tag{5.7.11}\] 误差平方和为 \[\hat{\bm{L}}(k)=\begin{bmatrix}\overline{\bm{\Delta}}_{\hat{X}}(k)\\ \bm{\Delta}(k)\end{bmatrix}^{\mathrm{T}} \begin{bmatrix}\overline{\bm{\Delta}}_{\hat{X}}(k)\\ \bm{\Delta}(k)\end{bmatrix} =\left\|\begin{bmatrix}\overline{\bm{\Delta}}_{\hat{X}}(k)\\ \bm{\Delta}(k)\end{bmatrix}\right\|^2 \tag{5.7.12}\] 上式中的 \(\left\|\cdot\right\|^2\) 表示向量的 2-范数的平方，所以 \(\hat{\bm{L}}(k)\) 也是误差向量的长度平方。将式 (5.7.11) 代入上式 \[\hat{\bm{L}}(k)=\left\|\begin{bmatrix}\bm{S}_{k,\ k-1}^{-1}\\ \bm{H}_k\end{bmatrix}\bm{X}(k) -\begin{bmatrix}\widetilde{\bm{L}}_{\hat{X}}(k)\\ \bm{Z}(k)\end{bmatrix}\right\|^2 \tag{5.7.13}\] 再将 \(\bm{X}(k)\) 前的系数矩阵作 QR 分解 \[\begin{bmatrix}\bm{S}_{k,\ k-1}^{-1}\\ \bm{H}_k\end{bmatrix}_{(n+\ell)\times n} =\hat{\bm{Q}}_k\hat{\bm{R}}_k=\left[\begin{array}{ll}\hat{\bm{Q}}_1 & \hat{\bm{Q}}_2\end{array}\right] \begin{bmatrix}\hat{\bm{R}}_{\hat{X}}(k)\\ \bm{0}\end{bmatrix} \tag{5.7.14}\] 其中，\(\hat{\bm{Q}}_k\) 为正交矩阵，根据附录式 (A-60)，式 (5.7.13) 可表示为 \[\hat{\bm{L}}(k)=\left\|\hat{\bm{Q}}_k^{\mathrm{T}}\begin{bmatrix}\bm{S}_{k,\ k-1}^{-1}\\ \bm{H}_k\end{bmatrix}\bm{X}(k) -\hat{\bm{Q}}_k^{\mathrm{T}}\begin{bmatrix}\widetilde{\bm{L}}_{\hat{X}}(k)\\ \bm{Z}(k)\end{bmatrix}\right\|^2 \tag{5.7.15}\] 设 \[\begin{bmatrix}\hat{\bm{L}}_{\hat{X}}(k)\\ \bm{\xi}(k)\end{bmatrix} =\hat{\bm{Q}}_k^{\mathrm{T}}\begin{bmatrix}\widetilde{\bm{L}}_{\hat{X}}(k)\\ \bm{Z}(k)\end{bmatrix} \tag{5.7.16}\] \[\hat{\bm{Q}}_k^{\mathrm{T}}\begin{bmatrix}\bm{S}_{k,\ k-1}^{-1}\\ \bm{H}_k\end{bmatrix} =\begin{bmatrix}\hat{\bm{R}}_{\hat{X}}(k)\\ \bm{0}\end{bmatrix} \tag{5.7.17}\] 所以式 (5.7.13) 为 \[\begin{aligned} \hat{\bm{L}}(k)&=\left\|\begin{bmatrix}\hat{\bm{R}}_{\hat{X}}(k)\\ \bm{0}\end{bmatrix}\bm{X}(k) -\begin{bmatrix}\hat{\bm{L}}_{\hat{X}}(k)\\ \bm{\xi}(k)\end{bmatrix}\right\|^2\\ &=\left\|\hat{\bm{R}}_{\hat{X}}(k)\bm{X}(k)-\hat{\bm{L}}_{\hat{X}}(k)\right\|^2+\left\|\bm{\xi}(k)\right\|^2 \end{aligned} \tag{5.7.18}\] 只有当式 (5.7.18) 中的 \(\left\|\hat{\bm{R}}_{\hat{X}}(k)\bm{X}(k)-\hat{\bm{L}}_{\hat{X}}(k)\right\|^2\) 为零时，才能使 \(\hat{\bm{L}}(k)\) 最小。记使 \(\hat{\bm{L}}(k)\) 最小的 \(\bm{X}(k)\) 的估计为 \(\hat{\bm{X}}(k)\)，那么 \[\bm{R}_{\hat{X}}(k)\hat{\bm{X}}(k)-\hat{\bm{L}}_{\hat{X}}(k)=\bm{0} \tag{5.7.19}\] 解得 \[\hat{\bm{X}}(k)=\hat{\bm{R}}_{\hat{X}}^{-1}(k)\hat{\bm{L}}_{\hat{X}}(k) \tag{5.7.20}\] 容易推导得到

\[\bm{D}_{\hat{X}}(k)=\hat{\bm{R}}_{\hat{X}}^{-1}(k)\hat{\bm{R}}_{\hat{X}}^{-\mathrm{T}}(k) \tag{5.7.21}\] \(\hat{\bm{X}}(k)\) 即为 \(t_k\) 时刻的滤波。

2. \(t_{k+1}\) 时刻的时间预测

由状态方程可得 \[\bm{X}(k)=\bm{\Phi}_{k+1,\ k}^{-1}\left[\,\bm{X}(k+1)-\bm{\varGamma}_k\bm{w}(k)\,\right] \tag{5.7.22}\] 将系统噪声的期望 \(\overline{\bm{w}}(k)\) 看做是系统噪声 \(\bm{w}(k)\) 的观测值 \[\overline{\bm{w}}(k)=\bm{w}(k)+\bm{\Delta}_w(k) \tag{5.7.23}\] \[\bm{\Delta}_w(k)\sim\left[\,\bm{0},\ \bm{D}_w(k)\,\right] \tag{5.7.24}\] 同样，将式 (5.7.23) 左乘 \(\bm{S}_{w_k}^{-1}\) \[\bm{S}_{w_k}^{-1}\overline{\bm{w}}(k)=\bm{S}_{w_k}^{-1}\bm{w}(k)+\bm{S}_{w_k}^{-1}\bm{\Delta}_w(k)\] 并设 \[\hat{\bm{L}}_w(k)=\bm{S}_{w_k}^{-1}\overline{\bm{w}}(k)\ ,\quad \overline{\bm{\Delta}}_w(k)=\bm{S}_{w_k}^{-1}\bm{\Delta}_w(k) \tag{5.7.25}\] 那么 \[-\overline{\bm{\Delta}}_w(k)=\bm{S}_{w_k}^{-1}\bm{w}(k)-\hat{\bm{L}}_w(k) \tag{5.7.26}\] \[\overline{\bm{\Delta}}_w(k)\sim\left[\,\bm{0},\ \bm{I}\,\right] \tag{5.7.27}\] 与测量更新比较，时间预测时增加了系统噪声，这时的误差平方和为 \[\widetilde{\bm{L}}(k+1)=\hat{\bm{L}}(k)+\left\|\overline{\bm{\Delta}}_w(k)\right\|^2 =\left\|\hat{\bm{R}}_{\hat{X}}(k)\bm{X}(k)-\hat{\bm{L}}_{\hat{X}}(k)\right\|^2 +\left\|\bm{\xi}(k)\right\|^2+\left\|\overline{\bm{\Delta}}_w(k)\right\|^2 \tag{5.7.28}\] 将式 (5.7.22) 和式 (5.7.26) 代入上式，可得 \[\begin{aligned} \widetilde{\bm{L}}(k+1)&=\left\|\hat{\bm{R}}_{\hat{X}}(k)\bm{\Phi}_{k+1,\ k}^{-1}\left[\,\bm{X}_{k+1}-\bm{\varGamma}_k\bm{w}(k)\,\right]-\hat{\bm{L}}_{\hat{X}}(k)\right\|^2\\ &\quad+\left\|\bm{\xi}(k)\right\|^2+\left\|\bm{S}_{w_k}^{-1}\bm{w}(k)-\hat{\bm{L}}_w(k)\right\|^2\\ &=\left\|\begin{bmatrix}\bm{S}_{w_k}^{-1} & \bm{0}\\ -\hat{\bm{R}}_{\hat{X}}(k)\bm{\Phi}_{k+1,\ k}^{-1}\bm{\varGamma}_k & \hat{\bm{R}}_{\hat{X}}(k)\bm{\Phi}_{k+1,\ k}^{-1}\end{bmatrix} \begin{bmatrix}\bm{w}(k)\\ \bm{X}(k+1)\end{bmatrix}\right.\\ &\qquad\left.-\begin{bmatrix}\hat{\bm{L}}_w(k)\\ \hat{\bm{L}}_{\hat{X}}(k)\end{bmatrix}\right\|^2+\bm{\xi}^2(k) \end{aligned} \tag{5.7.29}\] 做以下的 QR 分解： \[\begin{bmatrix}\bm{S}_{w_k}^{-1}\\ -\hat{\bm{R}}_{\hat{X}}(k)\bm{\Phi}_{k+1,\ k}^{-1}\bm{\varGamma}_k\end{bmatrix} =\left[\begin{array}{ll}\widetilde{\bm{Q}}_1 & \widetilde{\bm{Q}}_2\end{array}\right] \begin{bmatrix}\widetilde{\bm{R}}_w(k+1)\\ \bm{0}\end{bmatrix} \tag{5.7.30}\] 可得到矩阵 \(\widetilde{\bm{Q}}_{k+1}=\left[\begin{array}{ll}\widetilde{\bm{Q}}_1 & \widetilde{\bm{Q}}_2\end{array}\right]\) 和 \(\widetilde{\bm{R}}_w(k+1)\)。设 \[\widetilde{\bm{Q}}_{k+1}^{\mathrm{T}}\begin{bmatrix}\bm{0} & \hat{\bm{L}}_w(k)\\ \hat{\bm{R}}_{\hat{X}}(k)\bm{\Phi}_{k+1,\ k}^{-1} & \hat{\bm{L}}_{\hat{X}}(k)\end{bmatrix} =\begin{bmatrix}\widetilde{\bm{R}}_{w\hat{X}}(k+1) & \widetilde{\bm{L}}_w(k+1)\\ \widetilde{\bm{R}}_{\hat{X}}(k+1) & \widetilde{\bm{L}}_{\hat{X}}(k+1)\end{bmatrix} \tag{5.7.31}\] 由于 \(\widetilde{\bm{Q}}_{k+1}\) 为正交矩阵，\(\widetilde{\bm{Q}}_{k+1}^{\mathrm{T}}\widetilde{\bm{Q}}_{k+1}=\bm{I}\)，所以有 \[\begin{aligned} \widetilde{\bm{L}}(k+1)&=\left\|\widetilde{\bm{Q}}_{k+1}^{\mathrm{T}}\begin{bmatrix}\bm{S}_{w_k}^{-1} & \bm{0}\\ -\hat{\bm{R}}_{\hat{X}}(k)\bm{\Phi}_{k+1,\ k}^{-1}\bm{\varGamma}_k & \hat{\bm{R}}_{\hat{X}}(k)\bm{\Phi}_{k+1,\ k}^{-1}\end{bmatrix} \begin{bmatrix}\bm{w}(k)\\ \bm{X}(k+1)\end{bmatrix} -\begin{bmatrix}\hat{\bm{L}}_w(k)\\ \hat{\bm{L}}_{\hat{X}}(k)\end{bmatrix}\right\|^2+\bm{\xi}^2(k)\\ &=\left\|\begin{bmatrix}\widetilde{\bm{R}}_w(k+1) & \widetilde{\bm{R}}_{w\hat{X}}(k+1)\\ \bm{0} & \widetilde{\bm{R}}_{\hat{X}}(k+1)\end{bmatrix}\begin{bmatrix}\bm{w}(k)\\ \bm{X}(k+1)\end{bmatrix} -\begin{bmatrix}\widetilde{\bm{L}}_w(k+1)\\ \widetilde{\bm{L}}_{\hat{X}}(k+1)\end{bmatrix}\right\|^2+\left\|\bm{\xi}(k)\right\|^2 \end{aligned} \tag{5.7.32}\] 根据附录 A-59，上式可表示为 \[\begin{aligned} \widetilde{\bm{L}}(k+1)=&\left\|\widetilde{\bm{R}}_w(k+1)\bm{w}(k)+\widetilde{\bm{R}}_{w\hat{X}}(k+1)\bm{X}(k+1)-\widetilde{\bm{L}}_w(k+1)\right\|^2\\ &+\left\|\widetilde{\bm{R}}_{\hat{X}}(k+1)\bm{X}(k+1)-\widetilde{\bm{L}}_{\hat{X}}(k+1)\right\|^2+\left\|\bm{\xi}(k)\right\|^2 \end{aligned}\] 只有当上式中的第一项和第二项都为零时，\(\widetilde{\bm{L}}(k+1)\) 才有最小值，所以 \[\begin{aligned} \widetilde{\bm{R}}_w(k+1)\bm{w}(k)+\widetilde{\bm{R}}_{w\hat{X}}(k+1)\bm{X}(k+1)&=\widetilde{\bm{L}}_w(k+1)\\[4pt] \widetilde{\bm{R}}_{\hat{X}}(k+1)\bm{X}(k+1)&=\widetilde{\bm{L}}_{\hat{X}}(k+1) \end{aligned} \tag{5.7.33}\] 记上式的解为 \(\hat{\bm{X}}(k+1,\ k)\)，那么 \[\hat{\bm{X}}(k+1,\ k)=\widetilde{\bm{R}}_{\hat{X}}^{-1}(k+1)\ \widetilde{\bm{L}}_{\hat{X}}(k+1) \tag{5.7.34}\] 容易得到 \[\bm{D}_{\hat{X}}(k+1,\ k)=\widetilde{\bm{R}}_{\hat{X}}^{-1}(k+1)\ \widetilde{\bm{R}}_{\hat{X}}^{-T}(k+1) \tag{5.7.35}\] \(\hat{\bm{X}}(k+1,\ k)\) 就是在无观测值 \(\bm{Z}(k+1)\) 使 \(\widetilde{\bm{L}}(k+1)\) 最小的解，即时间预测。将式 (5.7.34) 代入式 (5.7.33) 的第一式，进而可以解得 \(\bm{w}(k)\)，这里解得的 \(\bm{w}(k)\) 是使得一步预测的误差平方 \(\widetilde{\bm{L}}(k+1)\) 最小的解，但这里我们并不关心 \(\bm{w}(k)\) 的时间预测。

3. \(t_{k+1}\) 时刻的测量更新

在 \(t_{k+1}\) 时刻的观测值为 \(\bm{Z}(k+1)\)，误差方程为 \[-\bm{\Delta}(k+1)=\bm{H}_{k+1}\bm{X}(k+1)-\bm{Z}(k+1) \tag{5.7.36}\] 此时的误差平方和为 \[\begin{aligned} \hat{\bm{L}}(k+1)&=\widetilde{\bm{L}}(k+1)+\left\|\bm{\Delta}(k+1)\right\|^2\\ &=\left\|\widetilde{\bm{R}}_w(k+1)\bm{w}(k)+\widetilde{\bm{R}}_{w\hat{X}}(k+1)\bm{X}(k+1)-\widetilde{\bm{L}}_w(k+1)\right\|^2\\ &\quad+\left\|\widetilde{\bm{R}}_{\hat{X}}(k+1)\bm{X}(k+1)-\widetilde{\bm{L}}_{\hat{X}}(k+1)\right\|^2 +\left\|\bm{\xi}(k)\right\|^2+\left\|\bm{\Delta}(k+1)\right\|^2 \end{aligned} \tag{5.7.37}\] 将式 (5.7.36) 代入式 (5.7.37) \[\begin{aligned} \hat{\bm{L}}(k+1)&=\left\|\widetilde{\bm{R}}_w(k+1)\bm{w}(k)+\widetilde{\bm{R}}_{w\hat{X}}(k+1)\bm{X}(k+1)-\widetilde{\bm{L}}_w(k+1)\right\|^2\\ &\quad+\left\|\begin{bmatrix}\widetilde{\bm{R}}_{\hat{X}}(k+1)\\ \bm{H}_{k+1}\end{bmatrix}\bm{X}(k+1) -\begin{bmatrix}\widetilde{\bm{L}}_{\hat{X}}(k+1)\\ \bm{Z}(k+1)\end{bmatrix}\right\|^2+\left\|\bm{\xi}(k)\right\|^2 \end{aligned} \tag{5.7.38}\] 做如下的 QR 分解 \[\begin{bmatrix}\widetilde{\bm{R}}_{\hat{X}}(k+1)\\ \bm{H}_{k+1}\end{bmatrix} =\hat{\bm{Q}}_{k+1}\hat{\bm{R}}_{k+1} =\left[\begin{array}{ll}\hat{\bm{Q}}_1 & \hat{\bm{Q}}_2\end{array}\right] \begin{bmatrix}\hat{\bm{R}}_{\hat{X}}(k+1)\\ \bm{0}\end{bmatrix} \tag{5.7.39}\] 并设 \[\hat{\bm{Q}}_{k+1}^{\mathrm{T}}\begin{bmatrix}\widetilde{\bm{L}}_{\hat{X}}(k+1)\\ \bm{Z}(k+1)\end{bmatrix} =\begin{bmatrix}\hat{\bm{L}}_{\hat{X}}(k+1)\\ \bm{\xi}(k+1)\end{bmatrix} \tag{5.7.40}\] 由于 \(\hat{\bm{Q}}_{k+1}^{\mathrm{T}}\) 是正交矩阵，\(\hat{\bm{Q}}_{k+1}\hat{\bm{Q}}_{k+1}^{\mathrm{T}}=\bm{I}\)，那么式 (5.7.38) 中的第二项为 \[\begin{aligned} &\left\|\hat{\bm{Q}}_{k+1}^{\mathrm{T}}\left\{\begin{bmatrix}\widetilde{\bm{R}}_{\hat{X}}(k+1)\\ \bm{H}_{k+1}\end{bmatrix}\bm{X}(k+1) -\begin{bmatrix}\widetilde{\bm{L}}_{\hat{X}}(k+1)\\ \bm{Z}(k+1)\end{bmatrix}\right\}\right\|^2\\ &\qquad=\left\|\begin{bmatrix}\hat{\bm{R}}_{\hat{X}}(k+1)\\ \bm{0}\end{bmatrix}\bm{X}(k+1) -\begin{bmatrix}\hat{\bm{L}}_{\hat{X}}(k+1)\\ \bm{\xi}(k+1)\end{bmatrix}\right\|^2 \end{aligned} \tag{5.7.41}\] 因此，式 (5.7.38) 为 \[\begin{aligned} \hat{\bm{L}}(k+1)=&\left\|\widetilde{\bm{R}}_w(k+1)\bm{w}(k)+\widetilde{\bm{R}}_{w\hat{X}}(k+1)\bm{X}(k+1)-\widetilde{\bm{L}}_w(k+1)\right\|^2\\ &+\left\|\hat{\bm{R}}_{\hat{X}}(k+1)\bm{X}(k+1)-\hat{\bm{L}}_{\hat{X}}(k+1)\right\|^2 +\left\|\bm{\xi}(k+1)\right\|^2+\left\|\bm{\xi}(k)\right\|^2 \end{aligned} \tag{5.7.42}\] 显然，当式 (5.7.42) 中的第一项和第二项都为零时，有最小值 \[\hat{\bm{L}}(k+1)=\left\|\bm{\xi}(k+1)\right\|^2+\left\|\bm{\xi}(k)\right\|^2 \tag{5.7.43}\] 当第二项为零时，可以解得 \(\bm{X}(k+1)\) 的估计，这个估计就是 \(\bm{Z}(k+1)\) 对 \(\bm{X}(k+1)\) 的测量更新 \(\hat{\bm{X}}(k+1)\) \[\begin{aligned} \hat{\bm{X}}(k+1)&=\hat{\bm{R}}_{\hat{X}}^{-1}(k+1)\bm{L}_{\hat{X}}(k+1)\\[4pt] \bm{D}_{\hat{X}}(k+1)&=\hat{\bm{R}}_{\hat{X}}^{-1}(k+1)\ \hat{\bm{R}}_{\hat{X}}^{-T}(k+1) \end{aligned} \tag{5.7.44}\] 将式 (5.7.44) 代入式 (5.7.42)，并使第一项也为零，可以解得 \[\hat{\bm{w}}(k)=\widetilde{\bm{R}}_w^{-1}(k+1)\left[\,\widetilde{\bm{L}}_w(k+1)-\widetilde{\bm{R}}_{w\hat{X}}(k+1)\hat{\bm{X}}(k+1)\,\right] \tag{5.7.45}\]

平方根信息滤波流程

综合上述，归纳平方根信息滤波的递推过程如下：

已知 \(\hat{\bm{X}}(0)\) 和 \(\bm{D}_{\hat{X}}(0)\)，设 \[\begin{aligned} \hat{\bm{R}}_{\hat{X}}(0)&=\bm{I}\\[4pt] \hat{\bm{L}}_{\hat{X}}(0)&=\hat{\bm{R}}_{\hat{X}}(0)\hat{\bm{X}}(0)=\hat{\bm{X}}(0) \end{aligned} \tag{5.7.46}\]

(1) 做时间预测的 QR 分解： \[\begin{bmatrix}\bm{S}_{w_{k-1}}\\ -\hat{\bm{R}}_{\hat{X}}(k-1)\ \bm{\Phi}_{k,\ k-1}^{-1}\ \bm{\varGamma}_{k-1}\end{bmatrix} =\widetilde{\bm{Q}}_k\ \widetilde{\bm{R}}_k\ ,\quad t_k(k=1,\ 2,\ \cdots)\] 其中，\(\bm{S}_{w_{k-1}}\) 为 \(\bm{D}_w(k)\) 的平方根矩阵，得到矩阵 \(\widetilde{\bm{Q}}_k\)。

(2) 时间预测： \[\widetilde{\bm{Q}}_k^{\mathrm{T}}\underbrace{\begin{bmatrix}\bm{S}_{w_{k-1}} & \bm{0} & \hat{\bm{L}}_w(k-1)\\ -\hat{\bm{R}}_{\hat{X}}(k-1)\ \bm{\Phi}_{k,\ k-1}^{-1}\ \bm{\varGamma}_{k-1} & \hat{\bm{R}}_{\hat{X}}(k-1)\ \bm{\Phi}_{k,\ k-1}^{-1} & \hat{\bm{L}}_{\hat{X}}(k-1)\end{bmatrix}}_{\displaystyle q\quad n\quad 1} =\underbrace{\begin{bmatrix}\widetilde{\bm{R}}_w(k) & \widetilde{\bm{R}}_{w\hat{X}}(k) & \widetilde{\bm{L}}_w(k)\\ \bm{0} & \widetilde{\bm{R}}_{\hat{X}}(k) & \widetilde{\bm{L}}_{\hat{X}}(k)\end{bmatrix}}_{\displaystyle p\quad n\quad 1} \tag{5.7.47}\] 其中 \[\hat{\bm{L}}_w(k-1)=\bm{S}_{w_{k-1}}^{-1}\overline{\bm{w}}(k-1) \tag{5.7.48}\] 在式 (5.7.47) 中，矩阵 \(\widetilde{\bm{R}}_{\hat{X}}(k)\) 和矩阵 \(\widetilde{\bm{L}}_{\hat{X}}(k)\) 是下一步测量更新需要的矩阵。

如果需要，可以解出时间预测 \(\hat{\bm{X}}(k,\ k-1)\) 和 \(\bm{D}_{\hat{X}}(k,\ k-1)\) \[\begin{aligned} \hat{\bm{X}}(k,\ k-1)&=\widetilde{\bm{R}}_{\hat{X}}^{-1}(k)\ \widetilde{\bm{L}}_{\hat{X}}(k)\\[4pt] \bm{D}_{\hat{X}}(k,\ k-1)&=\widetilde{\bm{R}}_{\hat{X}}^{-1}(k)\ \widetilde{\bm{R}}_{\hat{X}}^{-T}(k) \end{aligned} \tag{5.7.49}\]

(3) 作测量更新 QR 分解： \[\begin{bmatrix}\widetilde{\bm{R}}_{\hat{X}}(k)\\ \bm{H}_k\end{bmatrix}=\hat{\bm{Q}}_k\hat{\bm{R}}_k\ ,\quad t_k(k=1,\ 2,\ \cdots) \tag{5.7.50}\] 得到矩阵 \(\hat{\bm{Q}}_k\)。

(4) 测量更新： \[\hat{\bm{Q}}_k^{\mathrm{T}}\underbrace{\begin{bmatrix}\widetilde{\bm{R}}_{\hat{X}}(k) & \widetilde{\bm{L}}_{\hat{X}}(k)\\ \bm{H}_k & \bm{Z}(k)\end{bmatrix}}_{\displaystyle n\quad 1} =\underbrace{\begin{bmatrix}\hat{\bm{R}}_{\hat{X}}(k) & \hat{\bm{L}}_{\hat{X}}(k)\\ \bm{0} & \bm{\xi}(k)\end{bmatrix}}_{\displaystyle n\quad 1} \tag{5.7.51}\] 解出 \[\begin{aligned} \hat{\bm{X}}(k)&=\hat{\bm{R}}_{\hat{X}}^{-1}(k)\bm{L}_{\hat{X}}(k)\\[4pt] \bm{D}_{\hat{X}}(k)&=\hat{\bm{R}}_{\hat{X}}^{-1}(k)\ \hat{\bm{R}}_{\hat{X}}^{-T}(k) \end{aligned} \tag{5.7.52}\] 式 (5.7.51) 中的矩阵 \(\hat{\bm{R}}_{\hat{X}}(k)\) 和矩阵 \(\hat{\bm{L}}_{\hat{X}}(k)\) 是下一刻 \(t_{k+1}\) 时间预测需要的矩阵。

一般情况下，我们并不需要解出 \(\bm{w}(k-1)\)，但如果需要平滑滤波，那么就可以根据式 (5.7.45) 解得 \[\hat{\bm{w}}(k-1)=\widetilde{\bm{R}}_w^{-1}(k)\left[\,\widetilde{\bm{L}}_w(k)-\widetilde{\bm{R}}_{w\hat{X}}(k)\hat{\bm{X}}(k)\,\right] \tag{5.7.53}\]