本章首先介绍 Kalman 滤波的历史和现状，然后详细推导随机线性离散系统的 Kalman 滤波、预测和平滑方程，接着通过算例来了解实现 Kalman 滤波器的一般步骤。在此基础上，本章推导了随机线性连续系统的 Kalman 滤波，并给出了滤波稳定性的概念和判定条件。

Kalman 滤波概述

Kalman 滤波的发展

信号是传递和运载信息的时间或者空间的函数，滤波就是从混合在一起的多种信号中提取出所需要的信号。有的信号变化规律是确定的，如调幅广播中的载波信号、阶跃信号和脉宽固定的矩形脉冲信号等，它们都有确定的频谱，这类信号称为确定性信号。对于这样的信号，我们可以根据各信号频带的不同，设置具有相应频率特性的滤波器，如低通、高通、带通和带阻滤波器能抑制干扰信号，使有用信号无衰减地通过。这样的滤波器是用物理方法实现的，称为模拟滤波器。滤波确定性信号还可以利用计算机通过算法实现，称为数值滤波器。

现实中不是所有的信号都是确定性的，如陀螺漂移、GNSS 接收机钟漂、海浪运动、作水平飞行的飞机的无线电高度表输出信号、接收机内部元件发热产生的热噪声等，它们都没有确定的频谱，这类信号称为随机信号，也就是随机变量。这些随机信号没有确定的频谱特性，无法用常规滤波提取，但它们具有确定的功率谱，可以根据有用信号和干扰信号的功率谱来设计滤波器。Wiener(N. Wiener)滤波就是根据随机信号的这一特性来抑制干扰信号的。Wiener 滤波将随机信号作功率谱分解，对信号作抑制和选通。由于 Wiener 滤波是在频域进行的滤波器，要求被处理的信号是一维平稳过程，而且需要求解维纳-霍普方程，计算量较大，需要大量的存储空间，这阻碍了 Wiener 滤波的应用。

采用频域设计是造成 Wiener 滤波器难以实现的根本原因。因此人们转向在时域内直接设计最优滤波器的方法。其中，匈牙利裔美国数学家 R. E. Kalman 提出的估计理论最具实用性，被称为 Kalman 滤波器。Kalman 滤波用状态空间来描述动态系统，在经典最小方差估计理论中加入了状态方程，由状态方程提供先验信息，并通过对被提取信号有关的量测来估计状态变量。Kalman 滤波的维数不再局限于一维，在算法实现时采用递推的形式，利用状态空间方法在时域内设计滤波器，适用于多维的平稳或非平稳随机过程，包括连续和离散两类算法，便于在计算机上实现。图 4.1 是 Kalman 滤波的知识架构。

Kalman 滤波最成功的应用是解决了“阿波罗”登月计划中的轨道确定问题和 C-5A 飞机导航系统的设计。1959 年起美国航空航天署开始研究载人太空飞船登月计划，他们曾试图用递推加权最小二乘和 Wiener 滤波方法来计算轨道，但是因为精度不够而且计算过于繁杂而无法进行下去。1960 年秋，Kalman 访问了 NASA，提出了最初的 Kalman 滤波思想，并对之进行研究。次年，他与 Bucy(R. S Bucy)把这一滤波方法推广到连续时间系统中去，完善了 Kalman 滤波估计理论。最初的 Kalman 滤波只适用于线性系统，在之后的十多年里，Bucy 和 Sunahara 等人致力于研究 Kalman 滤波理论在非线性系统下的扩展，提出了扩展的 Kalman(EKF)滤波，拓宽了 Kalman 滤波的适用范围。为了解决没有初始信息和先验信息的问题，Fraser 提出了信息滤波。随着微型计算机的发展，人们对 Kalman 滤波的数值稳定性、计算效率和实用性的要求越来越高。计算机的有限字长导致了计算中的截断误差，截断误差在滤波递推中不断积累，最终导致滤波结果数值不稳定。为了提高 Kalman 滤波计算的数值稳定性，Potter 提出了平方根滤波的思想；Carlson、Bierman、Thornton、Oschman 和 Schimidt 等人在此基础上继续完善平方根滤波方法，先后提出了 UD 分解滤波、UDU 分解滤波和平方根信息滤波。

从 Kalman 滤波思想的提出，到解决“阿波罗”登月中的轨道计算，Kalman 滤波方法逐步发展和形成了一套完整的理论。Kalman 滤波是一种线性、无偏且方差最小的最优估计方法。对于计算机运算来说，Kalman 滤波的递推使其运算量和存储量大为减少，容易满足实时估计的要求。由于算法的最优和高效，Kalman 滤波被广泛应用于各个领域。Kalman 滤波对现代工业和科技发展的影响深远，它不仅推动了惯性导航、制导系统、全球卫星定位系统、目标跟踪等系统的发展，而且在通信与信号处理和金融等领域都得到了广泛的应用。近年来更被应用于计算机图像处理，例如人脸识别、图像分割、图像边缘检测等领域。

问题的提出和解决思路

Kalman 滤波与经典估计方法的不同之处是考虑了系统自身的变化规律，即利用状态方程对状态进行预测，用状态的预测和对系统的观测值一起来对状态进行估计。但是，无论是状态的预测还是观测值都受到噪声的干扰。如何从噪声干扰中提取有用的信息给状态一个“最优”的估计是 Kalman 滤波要解决的问题。在下一节对 Kalman 滤波进行推导前，这里首先通过一个简单的例子来说明要解决的问题和思路。

某地区夏季夜间温度基本稳定。现在已知时刻 \(t_0=0:00\) 的温度为 \(X(0)=20.0^{\circ}\mathrm{C}\)，\(\sigma_x(0)=0.1^{\circ}\mathrm{C}\)。由于夜间温度变化不大，所以预测下一时刻 \(t_1=1:00\) 的温度也为 \(\hat{X}(1,\ 0)=20.0^{\circ}\mathrm{C}\)。到 \(t_1\) 时刻时，用温度计量测温度，量测值为 \(Z(1)=20.7^{\circ}\mathrm{C}\)，中误差为 \(\sigma_{\Delta}(1)=0.2^{\circ}\mathrm{C}\)。对于 \(t_1\) 时刻的温度，我们有两类信息可用：一类是预测温度，有不确定性，另一类是量测温度，也有观测误差，我们应该如何在这两类信息之间取舍呢？如图 4.2 所示，根据已经有的解决这类问题的经验，我们更愿意相信方差小的信息，所以当有两类信息可用的时候，方差小的信息对估计的影响或权重应该多一些，反之亦然。Kalman 滤波正是这样在预测值和观测值之间做出“折中”的算法。从这个问题的解决思路可以看出，Kalman 滤波首先用状态方程进行“时间预测”，获得时间预测信息，然后观测值信息和预测信息共同对状态进行估计（折中），这也是观测值对预测值的修正过程，即所谓的“测量更新”。在得到测量更新的最优估计后，又可以对下一个时间的状态进行预测，然后用新的观测值对预测状态进行修正。Kalman 滤波就是这样在两类信息之间平衡取舍并且随时间递推的算法。

线性离散系统的 Kalman 滤波

本节将在第 3 章给出的随机线性离散系统数学模型的基础上，分别给出基于最小方差准则和最小二乘准则的 Kalman 滤波推导，并对 Kalman 滤波器进行直观解释。

随机线性离散系统的函数模型为 \[\bm{X}(k)=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{w}(k-1) \tag{4.2.1}\] \[\bm{Z}(k)=\bm{H}_k\bm{X}(k)+\bm{\Delta}(k) \tag{4.2.2}\] 其中 \(\bm{\Phi}_{k,\ k-1}\) 为 \(t_{k-1}\) 时刻至 \(t_k\) 时刻的状态转移矩阵；\(\bm{w}(k-1)\) 系统噪声；\(\bm{H}_k\) 为量测矩阵；\(\bm{\Delta}(k)\) 为量测噪声。

随机模型为 \[\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{4.2.3}\] 此外，已知 \(\hat{\bm{X}}(0)\)，并且有 \[\begin{aligned} E\left[\,\hat{\bm{X}}(0)\,\right]&=E\left[\,\bm{X}(0)\,\right]\\ \mathrm{Var}\left[\,\hat{\bm{X}}(0)\,\right]&=\bm{D}_{\hat{X}}(0) \end{aligned} \tag{4.2.4}\] \[\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{4.2.5}\] 式 (4.2.1) 并没有考虑在第 3 章模型中的 \(\bm{\Psi}_{k,\ k-1}\bm{u}(k-1)\)。这是因为 \(\bm{\Psi}_{k,\ k-1}\bm{u}(k-1)\) 只与控制输入和常量项有关，并不影响以下最优估计的推导。

Kalman 滤波是预测和测量更新的过程，所谓预测就是在 \(t_{k-1}\) 时刻对 \(t_k\) 时刻的状态 \(\bm{X}(k)\) 进行估计，这时还没有得到 \(t_k\) 时刻的观测值 \(\bm{Z}(k)\)，也就是说预测用所有的历史观测值 \(\bm{Z}(1)\)，\(\bm{Z}(2)\)，…，\(\bm{Z}(k-1)\) 来估计 \(\bm{X}(k)\)。测量更新就是在 \(t_k\) 时刻得到观测值 \(\bm{Z}(k)\) 后，利用观测值 \(\bm{Z}(1)\)，\(\bm{Z}(2)\)，…，\(\bm{Z}(k-1)\) 和 \(\bm{Z}(k)\) 来估计 \(\bm{X}(k)\)。在证明推导之前，首先给出变量的定义。这里设 \(t_{k-1}\) 时刻对 \(\bm{X}(k)\) 的预测为 \(\hat{\bm{X}}(k,\ k-1)\)，预测误差为 \[\Delta\hat{\bm{X}}(k,\ k-1)=\bm{X}(k)-\hat{\bm{X}}(k,\ k-1) \tag{4.2.6}\] \(\hat{\bm{X}}(k,\ k-1)\) 的方差记为 \(\bm{D}_{\hat{X}}(k,\ k-1)\) \[\bm{D}_{\hat{X}}(k,\ k-1)=E\left[\,\Delta\hat{\bm{X}}(k,\ k-1)\,\Delta\hat{\bm{X}}^{\mathrm{T}}(k,\ k-1)\,\right] \tag{4.2.7}\] 在 \(t_k\) 对 \(\bm{X}(k)\) 的测量更新为 \(\hat{\bm{X}}(k)\)，\(\hat{\bm{X}}(k)\) 也称为滤波估计，滤波误差为 \[\Delta\hat{\bm{X}}(k)=\bm{X}(k)-\hat{\bm{X}}(k) \tag{4.2.8}\] 其方差为 \(\bm{D}_{\hat{X}}(k)\) \[\bm{D}_{\hat{X}}(k)=E\left(\,\Delta\hat{\bm{X}}(k)\,\Delta\hat{\bm{X}}^{\mathrm{T}}(k)\,\right)^{\mathrm{T}} \tag{4.2.9}\]

基于最小方差准则的推导

1. 滤波的时间预测

滤波的一步预测是用 \(\bm{Z}(1)\)，\(\bm{Z}(2)\)，…，\(\bm{Z}(k-1)\) 来估计 \(\bm{X}(k)\)。根据最小方差准则，\(\bm{Z}(1)\)，\(\bm{Z}(2)\)，…，\(\bm{Z}(k-1)\) 对 \(\bm{X}(k)\) 的最优估计为在 \(\bm{Z}(1)\)，\(\bm{Z}(2)\)，…，\(\bm{Z}(k-1)\) 条件下 \(\bm{X}(k)\) 的期望，即 \[\hat{\bm{X}}(k,\ k-1)=E\left[\,\bm{X}(k)\mid\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\,\right] \tag{4.2.10}\] 已知状态方程 \[\bm{X}(k)=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{w}(k-1) \tag{4.2.11}\] 将状态方程代入式 (4.2.10)，有 \[\begin{aligned} \hat{\bm{X}}(k,\ k-1)&=E\left[\,\left(\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{w}(k-1)\right)\mid\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\,\right]\\ &=E\left[\,\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)\mid\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\,\right]\\ &\quad+E\left[\,\bm{w}(k-1)\mid\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\,\right] \end{aligned} \tag{4.2.12}\] 由于系统噪声 \(\bm{w}(k-1)\) 与观测值 \(\bm{Z}(1)\)，\(\bm{Z}(2)\)，…，\(\bm{Z}(k-1)\) 无关，所以 \[E\left[\,\bm{w}(k-1)\mid\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\,\right]=E\left[\,\bm{w}(k-1)\,\right]=0 \tag{4.2.13}\] 因此，式 (4.2.12) 为 \[\hat{\bm{X}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}E\left[\,\bm{X}(k-1)\mid\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\,\right] \tag{4.2.14}\] 观察式 (4.2.14)，\(E\left[\,\bm{X}(k-1)\mid\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\,\right]\) 就是 \(\bm{Z}(1)\)，\(\bm{Z}(2)\)，…，\(\bm{Z}(k-1)\) 对状态 \(\bm{X}(k-1)\) 的最小方差估计 \(\hat{\bm{X}}(k-1)\)，所以 \[\hat{\bm{X}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1) \tag{4.2.15}\] 这样就得到并证明了 \(\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1)\) 是 \(\bm{Z}(1)\)，\(\bm{Z}(2)\)，…，\(\bm{Z}(k-1)\) 对 \(\bm{X}(k)\) 的最小方差估计。由于它是在获得 \(\bm{Z}(k)\) 之前对 \(\bm{X}(k)\) 的估计，所以也称式 (4.2.15) 为“时间预测”。

接下来推导 \(\hat{\bm{X}}(k,\ k-1)\) 的方差。将式 (4.2.11) 和 (4.2.15) 代入 (4.2.6) \[\begin{aligned} \Delta\hat{\bm{X}}(k,\ k-1)&=\bm{X}(k)-\hat{\bm{X}}(k,\ k-1)\\ &=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{w}(k-1)-\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1)\\ &=\bm{\Phi}_{k,\ k-1}\Delta\hat{\bm{X}}(k-1)+\bm{w}(k-1) \end{aligned} \tag{4.2.16}\] 其中 \[\Delta\hat{\bm{X}}(k-1)=\bm{X}(k-1)-\hat{\bm{X}}(k-1) \tag{4.2.17}\] \(\Delta\hat{\bm{X}}(k-1)\) 的期望为 \[E\left(\,\Delta\hat{\bm{X}}(k,\ k-1)\,\right)=\bm{\Phi}_{k,\ k-1}E\left(\,\Delta\hat{\bm{X}}(k-1)\,\right)+E\left(\,\bm{w}(k-1)\,\right) \tag{4.2.18}\] 由于 \(E\left(\,\bm{w}(k-1)\,\right)=0\)，所以 \[E\left(\,\Delta\hat{\bm{X}}(k,\ k-1)\,\right)=\bm{\Phi}_{k,\ k-1}E\left(\,\Delta\hat{\bm{X}}(k-1)\,\right) \tag{4.2.19}\] 从上式可以看出，如果 \(\hat{\bm{X}}(k-1)\) 是 \(\bm{X}(k-1)\) 的无偏估计，那么 \(E\left(\,\Delta\hat{\bm{X}}(k-1)\,\right)=0\)，这样就有 \[E\left(\,\Delta\hat{\bm{X}}(k,\ k-1)\,\right)=0 \tag{4.2.20}\] 上面的推导说明，只要滤波 \(\hat{\bm{X}}(k-1)\) 是无偏估计，那么预测 \(\hat{\bm{X}}(k,\ k-1)\) 就一定是无偏估计。关于 \(\hat{\bm{X}}(k-1)\) 的无偏性将在下面的测量更新中给出，所以这里先给出 \[E\left[\,\hat{\bm{X}}(k,\ k-1)\,\right]=E\left[\,x(k)\,\right]=0 \tag{4.2.21}\]

根据方差的定义，\(\hat{\bm{X}}(k,\ k-1)\) 的方差为 \[\bm{D}_{\hat{X}}(k,\ k-1)=E\left(\,\Delta\hat{\bm{X}}(k,\ k-1)\,\Delta\hat{\bm{X}}^{\mathrm{T}}(k,\ k-1)\,\right) \tag{4.2.22}\] 将式 (4.2.16) 代入上式有 \[\begin{aligned} \bm{D}_{\hat{X}}(k,\ k-1)=&E\left\{\left[\,\bm{\Phi}_{k,\ k-1}\Delta\hat{\bm{X}}(k-1)+\bm{w}(k-1)\,\right] \left[\,\bm{\Phi}_{k,\ k-1}\Delta\hat{\bm{X}}(k-1)+\bm{w}(k-1)\,\right]^{\mathrm{T}}\right\}\\ =&\bm{\Phi}_{k,\ k-1}E\left[\,\Delta\hat{\bm{X}}(k-1)\,\Delta\hat{\bm{X}}^{\mathrm{T}}(k-1)\,\right]\bm{\Phi}_{k,\ k-1}^{\mathrm{T}} +E\left[\,\bm{w}(k-1)\,\bm{w}^{\mathrm{T}}(k-1)\,\right]\\ &\quad+E\left[\,\bm{w}(k-1)\,\Delta\hat{\bm{X}}^{\mathrm{T}}(k-1)\,\right]\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\\ &\quad+\bm{\Phi}_{k,\ k-1}E\left[\,\Delta\hat{\bm{X}}(k-1)\,\bm{w}^{\mathrm{T}}(k-1)\,\right] \end{aligned} \tag{4.2.23}\]

上式中的后两项 \(E\left[\,\bm{w}(k-1)\,\Delta\hat{\bm{X}}^{\mathrm{T}}(k-1)\,\right]\) 和 \(E\left[\,\Delta\hat{\bm{X}}(k-1)\,\bm{w}^{\mathrm{T}}(k-1)\,\right]\) 表示 \(\Delta\hat{\bm{X}}(k-1)\) 与 \(\bm{w}(k-1)\) 的协方差。在第 3 章的线性离散系统的随机模型中已经给出了系统噪声 \(\bm{w}(k-1)\) 与 \(\bm{X}(j)\ (j\leq k-1)\) 不相关，并且它们与观测值也不相关，又由于 \(\hat{\bm{X}}(k-1)\) 是由观测值 \(\bm{Z}(1)\)，\(\bm{Z}(2)\)，…，\(\bm{Z}(k-1)\) 估计得到，所以这里的 \(\bm{w}(k-1)\) 与 \(\bm{X}(k-1)\) 和 \(\hat{\bm{X}}(k-1)\) 都不相关，这意味着式 (4.2.23) 中的后两项为零，因此有

\[\bm{D}_{\hat{X}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}E\left[\,\Delta\hat{\bm{X}}(k-1)\,\Delta\hat{\bm{X}}^{\mathrm{T}}(k-1)\,\right]\bm{\Phi}_{k,\ k-1}^{\mathrm{T}} +E\left[\,\bm{w}(k-1)\,\bm{w}^{\mathrm{T}}(k-1)\,\right] \tag{4.2.24}\]

上式中的 \(E\left[\,\Delta\hat{\bm{X}}(k-1)\,\Delta\hat{\bm{X}}^{\mathrm{T}}(k-1)\,\right]\) 为滤波 \(\hat{\bm{X}}(k-1)\) 的方差 \(\bm{D}_{\hat{X}}(k-1)\)，\(E\left[\,\bm{w}(k-1)\,\bm{w}^{\mathrm{T}}(k-1)\,\right]\) 为 \(\bm{w}(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{4.2.25}\]

2. 滤波的测量更新

已知观测方程 \[\bm{Z}(k)=\bm{H}_k\bm{X}(k)+\bm{\Delta}(k) \tag{4.2.26}\] 若已知 \(\bm{X}(k)\) 的期望 \(E\left(\,\bm{X}(k)\,\right)\) 和方差 \(\mathrm{Var}\left(\,\bm{X}(k)\,\right)\)，根据第 2 章的最小方差估计，可得到 \(\bm{X}(k)\) 的最小方差估计为 \[\hat{\bm{X}}(k)=E\left(\,\bm{X}(k)\,\right)+\bm{D}_{XZ}\bm{D}_Z^{-1}\left(\,\bm{Z}(k)-E(\bm{Z}(k)\,)\,\right) \tag{4.2.27}\] 其中，\(\bm{D}_{XZ}\) 为观测值 \(\bm{Z}(k)\) 与 \(\bm{X}(k)\) 的协方差，\(\bm{D}_Z\) 为观测值 \(\bm{Z}(k)\) 的方差。

在上面的时间预测中，得到了 \(\hat{\bm{X}}(k,\ k-1)\) 和 \(\bm{D}_{\hat{X}}(k,\ k-1)\)，这为 \(\bm{X}(k)\) 提供了先验信息。利用这些先验信息，可设 \[E\left(\,\bm{X}(k)\,\right)=\hat{\bm{X}}(k,\ k-1) \tag{4.2.28}\] \[\mathrm{Var}\left(\,\bm{X}(k)\,\right)=\bm{D}_{\hat{X}}(k,\ k-1) \tag{4.2.29}\] 根据式 (4.2.26) 和式 (4.2.28)，得到 \(E\left[\,\bm{Z}(k)\,\right]\) \[E\left[\,\bm{Z}(k)\,\right]=\bm{H}_k\hat{\bm{X}}(k,\ k-1) \tag{4.2.30}\] 根据式 (4.2.26) 和误差传播定律，可得 \(\bm{Z}(k)\) 的方差为 \[\bm{D}_Z=\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\bm{H}_k^{\mathrm{T}}+\bm{D}_{\Delta}(k) \tag{4.2.31}\] \(\bm{Z}(k)\) 与 \(\bm{X}(k)\) 的协方差为 \[\bm{D}_{XZ}=\bm{D}_{\hat{X}}(k,\ k-1)\bm{H}_k^{\mathrm{T}} \tag{4.2.32}\] 将 \(E(\bm{X}(k))\)、\(E\left[\,\bm{Z}(k)\,\right]\)、\(\bm{D}_Z\) 和 \(\bm{D}_{XZ}\) 代入式 (4.2.27) 有 \[\begin{aligned} \hat{\bm{X}}(k)=&\hat{\bm{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}}+\bm{D}_{\Delta}(k)\right)^{-1}\\ &\cdot\left[\,\bm{Z}(k)-\bm{H}_k\hat{\bm{X}}(k,\ k-1)\,\right] \end{aligned} \tag{4.2.33}\] 如果将上式中的 \(\bm{H}_k\hat{\bm{X}}(k,\ k-1)\) 可以看作对观测值 \(\bm{Z}(k)\) 的预测，记为 \(\bm{Z}(k,\ k-1)\) \[\bm{Z}(k,\ k-1)=\bm{H}_k\hat{\bm{X}}(k,\ k-1) \tag{4.2.34}\] 并设预测观测值与实际观测值的差异为 \[\bm{V}_Z(k,\ k-1)=\bm{Z}(k)-\bm{Z}(k,\ k-1) \tag{4.2.35}\] \(\bm{V}(k,\ 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} \tag{4.2.36}\] 那么，式 (4.2.33) 为 \[\hat{\bm{X}}(k)=\hat{\bm{X}}(k,\ k-1)+\bm{K}_k\bm{V}(k,\ k-1) \tag{4.2.37}\] 式 (4.2.37) 即为 Kalman 滤波的测量更新，就此得到了滤波 \(\hat{\bm{X}}(k)\)。在式 (4.2.37) 中，\(\bm{K}_k\) 乘以新息 \(\bm{V}(k,\ k-1)\) 后对预测 \(\hat{\bm{X}}(k,\ k-1)\) 进行修正，\(\bm{K}_k\) 的大小决定了观测值 \(\bm{Z}(k)\) 在多大程度上影响滤波值 \(\hat{\bm{X}}(k)\)，所以 \(\bm{K}_k\) 也被称为增益矩阵。

下面证明 \(\bm{V}_Z(k,\ k-1)\) 的期望为零，\(\hat{\bm{X}}(k)\) 是对 \(\bm{X}(k)\) 的无偏估计，并推导 \(\hat{\bm{X}}(k)\) 的方差 \(\bm{D}_{\hat{X}}(k)\)。

\(\bm{V}_Z(k,\ k-1)\) 的期望为 \[E\left[\,\bm{V}_Z(k,\ k-1)\,\right]=E\left[\,\bm{Z}(k)\,\right]-\bm{H}_kE\left[\,\hat{\bm{X}}(k,\ k-1)\,\right] \tag{4.2.38}\] 根据观测方程有 \[E\left[\,\bm{Z}(k)\,\right]=\bm{H}_kE\left[\,\bm{X}(k)\,\right] \tag{4.2.39}\] 在时间预测中，有 \(E\left[\,\hat{\bm{X}}(k,\ k-1)\,\right]=E\left[\,\bm{X}(k)\,\right]\)，所以式 (4.2.38) 为 \[E\left[\,\bm{V}(k,\ k-1)\,\right]=0 \tag{4.2.40}\] 根据式 (4.2.37) 和式 (4.2.40)，\(\hat{\bm{X}}(k)\) 的期望为 \[E\left[\,\hat{\bm{X}}(k)\,\right]=E\left[\,\hat{\bm{X}}(k,\ k-1)\,\right] \tag{4.2.41}\] 又由于 \(E\left[\,\hat{\bm{X}}(k,\ k-1)\,\right]=E\left[\,\bm{X}(k)\,\right]\)，得到 \[E\left[\,\hat{\bm{X}}(k)\,\right]=E\left[\,\bm{X}(k)\,\right] \tag{4.2.42}\] 显然，\(\hat{\bm{X}}(k)\) 是 \(\bm{X}(k)\) 的无偏估计。

从以上的证明看出，对一步预测 \(\hat{\bm{X}}(k,\ k-1)\) 的无偏性证明是在 \(\hat{\bm{X}}(k-1)\) 为 \(\bm{X}(k-1)\) 的无偏估计的条件下得到的，进而推导得到了滤波 \(\hat{\bm{X}}(k)\) 的无偏性。在式 (4.2.4) 中我们给定的初始条件有 \(E\left[\,\hat{\bm{X}}(0)\,\right]=E\left[\,\bm{X}(0)\,\right]\)，也就是说，如果从一开始初值 \(\hat{\bm{X}}(0)\) 是 \(t_0\) 时刻状态的无偏值，那么递推得到 \(\hat{\bm{X}}(1,\ 0)\)，\(\hat{\bm{X}}(1)\cdots\hat{\bm{X}}(k-1)\)，\(\hat{\bm{X}}(k,\ k-1)\)，\(\hat{\bm{X}}(k)\) 都是无偏估计。

滤波 \(\hat{\bm{X}}(k)\) 的估计误差为 \[\Delta\hat{\bm{X}}(k)=\bm{X}(k)-\hat{\bm{X}}(k) \tag{4.2.43}\] 根据式 (4.2.37) 和式 (4.2.16) 得 \[\begin{aligned} \Delta\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)\\ &=\Delta\hat{\bm{X}}(k,\ k-1)-\bm{K}_k\left[\,\bm{H}_k\bm{X}(k)+\bm{\Delta}(k)-\bm{H}_k\hat{\bm{X}}(k,\ k-1)\,\right]\\ &=\Delta\hat{\bm{X}}(k,\ k-1)-\bm{K}_k\left[\,\bm{H}_k\Delta\hat{\bm{X}}(k,\ k-1)+\bm{\Delta}(k)\,\right]\\ &=\left[\,\bm{I}-\bm{K}_k\bm{H}_k\,\right]\Delta\hat{\bm{X}}(k,\ k-1)-\bm{K}_k\bm{\Delta}(k) \end{aligned} \tag{4.2.44}\] \(\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] \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\}\\ =&\left(\bm{I}{-}\bm{K}_k\bm{H}_k\right)E\left[\Delta\hat{\bm{X}}(k,\!k{-}1)\,\Delta\hat{\bm{X}}^{\mathrm{T}}(k,\!k{-}1)\right] \left(\bm{I}{-}\bm{K}_k\bm{H}_k\right)^{\mathrm{T}}\\ &\quad{+}\bm{K}_kE\left[\bm{\Delta}(k)\bm{\Delta}^{\mathrm{T}}(k)\right]\bm{K}_k^{\mathrm{T}}\\ &\quad-\left(\bm{I}{-}\bm{K}_k\bm{H}_k\right)E\left[\Delta\hat{\bm{X}}(k,\!k{-}1)\bm{\Delta}^{\mathrm{T}}(k)\right]\bm{K}_k^{\mathrm{T}}\\ &\quad{-}\bm{K}_kE\left[\bm{\Delta}(k)\Delta\hat{\bm{X}}^{\mathrm{T}}(k,\!k{-}1)\right]\left(\bm{I}{-}\bm{K}_k\bm{H}_k\right)^{\mathrm{T}} \end{aligned} \tag{4.2.45}\]

上式中的 \(E\left[\,\Delta\hat{\bm{X}}(k,\ k-1)\,\Delta\hat{\bm{X}}^{\mathrm{T}}(k,\ k-1)\,\right]\) 为 \(\hat{\bm{X}}(k,\ k-1)\) 的方差，\(E\left[\,\bm{\Delta}(k)\,\bm{\Delta}^{\mathrm{T}}(k)\,\right]\) 为观测值的噪声方差，最后两项是 \(\Delta\hat{\bm{X}}(k,\ k-1)\) 与 \(\bm{\Delta}(k)\) 的协方差，由于 \(\bm{\Delta}(k)\) 是观测噪声，与 \(\Delta\hat{\bm{X}}(k,\ k-1)\) 无关，所以最后两项为零。这样就得到

\[\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}} +\bm{K}_k\bm{D}_{\Delta}(k)\bm{K}_k^{\mathrm{T}} \tag{4.2.46}\] 将式 (4.2.36) 代入上式，并利用矩阵的恒等式关系可以得到 \[\bm{D}_{\hat{X}}(k)=\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\bm{D}_{\hat{X}}(k,\ k-1) \tag{4.2.47}\]

3. 滤波公式汇总

现将推导得到的 Kalman 滤波基础公式汇总。

已知 \(t_0\) 时刻状态为 \(\hat{\bm{X}}(0)\)，方差为 \(\bm{D}_{\hat{X}}(0)\)，Kalman 滤波的递推公式为 \[\begin{aligned} \hat{\bm{X}}(k,\ k-1)&=\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1) \tag{4.2.48}\\ \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{4.2.49}\\ \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{4.2.50}\\ \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{4.2.51}\\ \bm{D}_{\hat{X}}(k)&=\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\bm{D}_{\hat{X}}(k,\ k-1) \tag{4.2.52} \end{aligned}\]

基于最小二乘准则的推导

1. 滤波的时间预测

与基于最小方差准则的 Kalman 滤波推导不同，在基于最小二乘准则的推导中，待估计参数为非随机量，并在时间预测时，将 \(\hat{\bm{X}}(k-1)\) 看做 \(\bm{X}(k-1)\) 的“虚拟观测值”，与实际观测值一起建立观测方程，按照最小二乘估计方法，构建法方程推导得到 Kalman 滤波。

首先推导已知 \(\hat{\bm{X}}(k-1)\) 和 \(\bm{D}_{\hat{X}}(k-1)\)，估计 \(\hat{\bm{X}}(k,\ k-1)\) 和 \(\bm{D}_{\hat{X}}(k,\ k-1)\) 的递推式。

如果将 \(\hat{\bm{X}}(k-1)\) 看做是 \(\bm{X}(k-1)\) 的“虚拟”观测值，那么观测方程为 \[\hat{\bm{X}}(k-1)=\bm{X}(k-1)+\bm{\Delta}_{\hat{X}(k-1)} \tag{4.2.53}\] 其中 \(\bm{\Delta}_{\hat{X}(k-1)}\) 为虚拟观测值 \(\hat{\bm{X}}(k-1)\) 的噪声，\(E(\bm{\Delta}_{\hat{X}(k-1)})=0\)，方差为 \(\bm{D}_{\hat{X}}(k,\ k-1)\)。接着将状态方程 (4.2.1) 表示为

\[\bm{0}=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)-\bm{X}(k)+\bm{w}(k-1) \tag{4.2.54}\] 并将方程左边的 \(\bm{0}\) 也视为此方程的“虚拟观测值”，噪声为 \(\bm{w}(k-1)\)，并且 \[\begin{aligned} E\left[\,\bm{w}(k)\,\right]&=\bm{0}\\ \mathrm{Cov}\left[\,\bm{w}(k),\ \bm{w}(j)\,\right]&=\bm{D}_w(k)\delta(k-j) \end{aligned} \tag{4.2.55}\] \(\bm{\Delta}_{\hat{X}(k-1)}\) 与 \(\bm{w}(k-1)\) 相互独立。

将式 (4.2.53) 和式 (4.2.54) 联立 \[\begin{bmatrix}\hat{\bm{X}}(k-1)\\ \bm{0}\end{bmatrix} =\begin{bmatrix}\bm{I} & \bm{0}\\ \bm{\Phi}_{k,\ k-1} & -\bm{I}\end{bmatrix} \begin{bmatrix}\bm{X}(k-1)\\ \bm{X}(k)\end{bmatrix} +\begin{bmatrix}\bm{\Delta}_{\hat{X}(k-1)}\\ \bm{w}(k-1)\end{bmatrix} \tag{4.2.56}\] 上式对应的误差方程为 \[-\begin{bmatrix}\bm{\Delta}_{\hat{X}(k-1)}\\ \bm{w}(k-1)\end{bmatrix} =\begin{bmatrix}\bm{I} & \bm{0}\\ \bm{\Phi}_{k,\ k-1} & -\bm{I}\end{bmatrix} \begin{bmatrix}\bm{X}(k-1)\\ \bm{X}(k)\end{bmatrix} -\begin{bmatrix}\hat{\bm{X}}(k-1)\\ \bm{0}\end{bmatrix} \tag{4.2.57}\] 根据最小二乘准则有 \[\begin{bmatrix}\bm{\Delta}_{\hat{X}(k-1)}\\ \bm{w}(k-1)\end{bmatrix}^{\mathrm{T}} \begin{bmatrix}\bm{D}_{\hat{X}}^{-1}(k-1) & \\ & \bm{D}_w^{-1}(k-1)\end{bmatrix} \begin{bmatrix}\bm{\Delta}_{\hat{X}(k-1)}\\ \bm{w}(k-1)\end{bmatrix}=\min \tag{4.2.58}\] 设对 \(\bm{X}(k-1)\) 和 \(\bm{X}(k)\) 的最小二乘估计分别为 \(\hat{\bm{X}}_s(k-1)\) 和 \(\hat{\bm{X}}(k,\ k-1)\)，根据最小二乘估计，法方程为 \[\begin{bmatrix} \bm{D}_{\hat{X}}^{-1}(k-1)+\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\bm{D}_w^{-1}(k-1)\bm{\Phi}_{k,\ k-1} & -\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\bm{D}_w^{-1}(k-1)\\[4pt] -\bm{D}_w^{-1}(k-1)\bm{\Phi}_{k,\ k-1} & \bm{D}_w^{-1}(k-1) \end{bmatrix} \begin{bmatrix}\hat{\bm{X}}_s(k-1)\\ \hat{\bm{X}}(k,\ k-1)\end{bmatrix} =\begin{bmatrix}\bm{D}_{\hat{X}}^{-1}(k-1)\hat{\bm{X}}(k-1)\\ \bm{0}\end{bmatrix} \tag{4.2.59}\] 将上式的第二式左乘 \(\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\) 再与第一式相加，即可解得到 \[\hat{\bm{X}}_s(k-1)=\hat{\bm{X}}(k-1) \tag{4.2.60}\] 将 \(\hat{\bm{X}}_s(k-1)=\hat{\bm{X}}(k-1)\) 代入式 (4.2.59)，得到 \[\hat{\bm{X}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}_s(k-1)=\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1) \tag{4.2.61}\] 法方程的系数矩阵的逆矩阵即为 \(\hat{\bm{X}}_s(k-1)\) 和 \(\hat{\bm{X}}(k,\ k-1)\) 协方差矩阵，所以 \[\begin{aligned} &\begin{bmatrix} \bm{D}_{\hat{X}_s}(k-1) & \mathrm{Covar}\left[\,\hat{\bm{X}}_s(k-1),\ \hat{\bm{X}}(k,\ k-1)\,\right]\\[4pt] \mathrm{Covar}\left[\,\hat{\bm{X}}(k,\ k-1),\ \hat{\bm{X}}_s(k-1)\,\right] & \bm{D}_{\hat{X}}(k,\ k-1) \end{bmatrix}\\[6pt] &=\left[\begin{array}{cc} \bm{D}_{\hat{X}}^{-1}(k-1)+\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\bm{D}_w^{-1}(k-1)\bm{\Phi}_{k,\ k-1} & -\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\bm{D}_w^{-1}(k-1)\\[4pt] -\bm{D}_w^{-1}(k-1)\bm{\Phi}_{k,\ k-1} & \bm{D}_w^{-1}(k-1) \end{array}\right]^{-1} \end{aligned} \tag{4.2.62}\] 其中矩阵右下角的 \(\bm{D}_{\hat{X}}(k,\ k-1)\) 为 \(\hat{\bm{X}}(k,\ k-1)\) 的方差矩阵。根据附录 A.9 的矩阵的分块求逆和矩阵恒等式，可得 \[\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{4.2.63}\] 综合上述，式 (4.2.61) 和式 (4.2.63) 即为已知 \(\hat{\bm{X}}(k-1)\) 和 \(\bm{D}_{\hat{X}}(k-1)\)，估计 \(\hat{\bm{X}}(k,\ k-1)\) 和 \(\bm{D}_{\hat{X}}(k,\ k-1)\) 的递推式，也就是 Kalman 滤波的时间预测。

2. 滤波的测量更新

下面推导已知 \(\hat{\bm{X}}(k,\ k-1)\) 和 \(\bm{D}_{\hat{X}}(k,\ k-1)\)，在有观测值 \(\bm{Z}(k)\) 后对 \(\bm{X}(k)\) 的估计，即对 \(\hat{\bm{X}}(k,\ k-1)\) 的测量更新。

设在 \(t_k\) 时刻有观测方程 \[\bm{Z}(k)=\bm{H}_k\bm{X}(k)+\bm{\Delta}(k) \tag{4.2.64}\] 观测噪声 \(\bm{\Delta}(k)\) 的方差为 \(\bm{D}_{\Delta}(k)\)。同时，将 \(\hat{\bm{X}}(k,\ k-1)\) 看作是对 \(\bm{X}(k)\) 的虚拟观测值 \[\hat{\bm{X}}(k,\ k-1)=\bm{X}(k)+\bm{\Delta}_X(k,\ k-1) \tag{4.2.65}\] 虚拟观测噪声为 \(\bm{\Delta}_X(k,\ k-1)\)，且 \(E\left[\,\bm{\Delta}_X(k,\ k-1)\,\right]=0\)，方差为 \(\bm{D}_{\hat{X}}(k,\ k-1)\)。

联立式 (4.2.64) 和 (4.2.65) 的观测方程为 \[\begin{bmatrix}\hat{\bm{X}}(k,\ k-1)\\ \bm{Z}(k)\end{bmatrix} =\begin{bmatrix}\bm{I}\\ \bm{H}_k\end{bmatrix}\bm{X}(k) +\begin{bmatrix}\bm{\Delta}_X(k,\ k-1)\\ \bm{\Delta}(k)\end{bmatrix} \tag{4.2.66}\] 其中 \(\bm{\Delta}_X(k,\ k-1)\) 与 \(\bm{\Delta}(k)\) 不相关，所以观测噪声的方差阵为 \[\begin{bmatrix}\bm{D}_{\hat{X}}(k,\ k-1) & \bm{0}\\ \bm{0} & \bm{D}_{\Delta}(k)\end{bmatrix} \tag{4.2.67}\] 根据式 (4.2.66)，误差方程为 \[-\begin{bmatrix}\bm{\Delta}_X(k,\ k-1)\\ \bm{\Delta}(k)\end{bmatrix} =\begin{bmatrix}\bm{I}\\ \bm{H}_k\end{bmatrix}\bm{X}(k) -\begin{bmatrix}\hat{\bm{X}}(k,\ k-1)\\ \bm{Z}(k)\end{bmatrix} \tag{4.2.68}\] 最小二乘准则为 \[\begin{bmatrix}\bm{\Delta}_X(k,\ k-1)\\ \bm{\Delta}(k)\end{bmatrix}^{\mathrm{T}} \begin{bmatrix}\bm{D}_{\hat{X}}(k,\ k-1) & \bm{0}\\ \bm{0} & \bm{D}_{\Delta}(k)\end{bmatrix}^{-1} \begin{bmatrix}\bm{\Delta}_X(k,\ k-1)\\ \bm{\Delta}(k)\end{bmatrix}=\min \tag{4.2.69}\] 根据第 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{4.2.70}\] 法方程的系数矩阵的逆也是 \(\hat{\bm{X}}(k)\) 的方差矩阵 \[\bm{D}_{\hat{X}}(k)=\left[\,\bm{D}_{\hat{X}}^{-1}(k,\ k-1)+\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\,\bm{H}_k\,\right]^{-1} \tag{4.2.71}\] 设 \[\begin{aligned} \bm{W}_k&=\bm{D}_{\hat{X}}^{-1}(k)\\ \bm{W}_{k,\ k-1}&=\bm{D}_{\hat{X}}^{-1}(k,\ k-1) \end{aligned} \tag{4.2.72}\] 式 (4.2.71) 成为 \[\bm{W}_k=\bm{W}_{k,\ k-1}+\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\,\bm{H}_k \tag{4.2.73}\] 或者 \[\bm{W}_{k,\ k-1}=\bm{W}_k-\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\,\bm{H}_k \tag{4.2.74}\] 那么式 (4.2.70) 为 \[\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{4.2.75}\] 进而 \[\hat{\bm{X}}(k)=\bm{W}_k^{-1}\bm{W}_{k,\ k-1}\hat{\bm{X}}(k,\ k-1)+\bm{W}_k^{-1}\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\bm{Z}(k) \tag{4.2.76}\] 将式 (4.2.74) 代入式 (4.2.76) 得 \[\begin{aligned} \hat{\bm{X}}(k)&=\bm{W}_k^{-1}\left[\,\bm{W}_k-\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\,\bm{H}_k\,\right]\hat{\bm{X}}(k,\ k-1) +\bm{W}_k^{-1}\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\bm{Z}(k)\\ &=\hat{\bm{X}}(k,\ k-1)+\bm{W}_k^{-1}\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k) \left[\,\bm{Z}(k)-\bm{H}_k\hat{\bm{X}}(k,\ k-1)\,\right] \end{aligned} \tag{4.2.77}\] 设 \[\bm{K}_k=\bm{W}_k^{-1}\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k) \tag{4.2.78}\] 将式 (4.2.73) 代入上式得 \[\bm{K}_k=\left(\bm{W}_{k,\ k-1}+\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\,\bm{H}_k\right)^{-1}\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k) \tag{4.2.79}\] 根据附录中式 (A-53)，可得 \[\bm{K}_k=\bm{W}_{k,\ k-1}^{-1}\bm{H}_k^{\mathrm{T}}\left[\,\bm{D}_{\Delta}(k)+\bm{H}_k\bm{W}_{k,\ k-1}^{-1}\bm{H}_k^{\mathrm{T}}\,\right]^{-1} \tag{4.2.80}\] 将式 (4.2.72) 代入上式得到 \[\bm{K}_k=\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}}\left[\,\bm{D}_{\Delta}(k)+\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}}\,\right]^{-1} \tag{4.2.81}\]

以上从最小二乘准则推导得到了 Kalman 滤波的测量更新。从 Kalman 滤波的最小二乘推导可以看出 Kalman 滤波与最小二乘的关系，即 Kalman 滤波也是最小二乘估计，它是将预测值 \(\hat{\bm{X}}(k,\ k-1)\) 视为虚拟观测值，在“广义最小二乘”准则上得到的。

Kalman 滤波除了以上的基于最小方差准则和最小二乘准则的推导，“投影法”也可以推导得到 Kalman 滤波递推公式。“投影法”也是 R. E. Kalman 在发表 Kalman 滤波的论文中所使用的方法，它是通过估计向量之间的正交性推导得到的，这里不再赘述。

Kalman 滤波器的递推公式和相关说明

现将 Kalman 滤波器的递推公式总结如表 4.1 所示。从表 4.1 可以看出，Kalman 滤波递推公式主要分为两步：时间预测和测量更新。时间预测是利用系统运动变化规律获得预测状态。对最小方差估计来说，这个状态的信息就是先验信息，在获得了观测值后，观测值对这个先验信息再进行修正。对最小二乘估计来说，预测状态被作为虚拟观测值，它与实际观测值一起对状态进行估计，最后得到递推的 Kalman 滤波公式。

表 4.1 中的 Kalman 滤波公式是基于模型 (4.2.1)～(4.2.5) 得到，在应用中，有时系统噪声向量 \(\bm{w}(k-1)\) 与状态向量 \(\bm{X}(k-1)\) 的维数不一样时，状态方程为 \[\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{4.2.82}\] 式 (4.2.82) 是状态方程更一般的情况，这时可设新的系统噪声为 \[\underset{n\times q}{\bm{w}'(k-1)}=\underset{n\times q}{\bm{\varGamma}_{k-1}}\ \underset{q\times 1}{\bm{w}(k-1)} \tag{4.2.83}\] 其方差为 \[\bm{D}_{w'}(k-1)=\bm{\varGamma}_{k-1}\bm{D}_w(k-1)\,\bm{\varGamma}_{k-1}^{\mathrm{T}}\] 在做了系统噪声置换后，将 \(\bm{D}_{w'}(k-1)\) 代替表 4.1 中 \(\bm{D}_w(k-1)\) 的进行时间预测。

1. 方差计算公式的等价性和差异

表 4.1 中的增益矩阵的第二个计算式可以由式 (4.2.78) 得到，此公式常用于相关证明。

表 4.1 中滤波方差的三个计算公式在理论上是等价的，其中第三个公式可由式 (4.2.73) 得到。虽然滤波方差的三个计算公式在理论上是等价的，但在实际应用中的计算效果却不同。从公式上看，式 ① 的计算步骤最少也最简单，但它有减法运算，并且表达式不对称，在递推中由于计算误差的积累容易导致方差矩阵失去对称性和正定性。反之，式 ② 能保持较好的对称性和正定性，式 ② 也被称为“Joseph update”。当状态的方差较大甚至无穷大时，可以采用式 ③ 来传递方差，第 5 章将要介绍的信息滤波就是采用这种方法来传递方差的。

2. 有控制输入的 Kalman 滤波递推公式

有时候系统还有控制输入部分，如在例 3.1 中，弹簧还受到外力 \(F(t)\) 的作用，对系统来说，这样的外力是确定性的控制输入，那么状态方程更一般地表达为 \[\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 1}{\bm{\Omega}(k-1)}+\underset{n\times 1}{\bm{w}(k-1)} \tag{4.2.84}\] 其中，\(\bm{\Omega}(k-1)\) 为确定性的控制输入或与状态和系统噪声无关的非随机部分，按上面的方法，同样可以推导得到。时间预测为 \[\hat{\bm{X}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}\hat{\bm{X}}(k-1)+\bm{\Omega}(k-1) \tag{4.2.85}\] 新息为 \[\bm{V}(k,\ k-1)=\bm{Z}(k)-\bm{H}_k\hat{\bm{X}}(k,\ k-1) \tag{4.2.86}\] 由于确定性部分不影响方差的计算，所以方差的计算公式均与表 4.1 中无 \(\bm{\Omega}(k-1)\) 项的估计公式一样。

3. 估计状态改正量的 Kalman 滤波递推公式

在现实应用中，为了准确地描述载体的运动规律，需要考虑某些量的误差或偏差，如惯性导航器件陀螺和加速度计的零偏和比例因子等，在估价时可将这些偏差和这些偏差对运动载体状态的影响一起作为状态量来估计，估计得到的是原来状态的改正量。下面给出状态改正量的 Kalman 滤波递推公式。

状态方程为 \[\bm{X}(k)=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{w}(k-1) \tag{4.2.87}\] 设 \[\begin{cases} \bm{X}(k)=\bm{X}^{*}(k)+\bm{x}(k)\\ \bm{X}(k-1)=\bm{X}^{*}(k-1)+\bm{x}(k-1) \end{cases} \tag{4.2.88}\] 并且 \[\bm{X}^{*}(k)=\bm{\Phi}\bm{X}^{*}(k-1) \tag{4.2.89}\] 将式 (4.2.88) 和式 (4.2.89) 代入式 (4.2.87)，得 \[\bm{x}(k)+\bm{X}^{*}(k)=\bm{\Phi}_{k,\ k-1}\left[\,\bm{X}^{*}(k-1)+\bm{x}(k-1)\,\right]+\bm{w}(k-1) \tag{4.2.90}\] 所以 \[\bm{x}(k)=\bm{\Phi}_{k,\ k-1}\bm{x}(k-1)+\bm{w}(k-1) \tag{4.2.91}\] 根据式 (4.2.88)，观测方程为 \[\bm{Z}(k)=\bm{H}_k\bm{X}^{*}(k)+\bm{H}_k\bm{x}(k)+\bm{\Delta}(k) \tag{4.2.92}\] 设 \[\bm{z}(k)=\bm{Z}(k)-\bm{H}_k\bm{X}^{*}(k) \tag{4.2.93}\] 那么， \[\bm{z}(k)=\bm{H}_k\bm{x}(k)+\bm{\Delta}(k) \tag{4.2.94}\] 基于式 (4.2.91) 的状态方程和式 (4.2.94) 的观测方程，滤波的递推为 \[\hat{\bm{x}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}\hat{\bm{x}}(k-1) \tag{4.2.95}\] \[\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{4.2.96}\] 这样估计得到的是状态的改正量 \(\hat{\bm{x}}(k,\ k-1)\) 和 \(\hat{\bm{x}}(k)\)。以状态的改正量进行滤波递推的优点是改正量的数值一般较小，便于计算并保证数值的准确性。由于以上的推导只做了数值置换，所以并不影响状态的随机特性，所以状态滤波的方差计算仍然与表 4.1 中给出的递推公式一样。

在得到状态的改正量后，即可得到状态估计 \[\hat{\bm{X}}(k,\ k-1)=\bm{\Phi}_{k,\ k-1}\bm{X}^{*}(k-1)+\bm{\Phi}_{k,\ k-1}\hat{\bm{x}}(k-1) \tag{4.2.97}\] \[\hat{\bm{X}}(k)=\bm{X}^{*}(k)+\hat{\bm{x}}(k) \tag{4.2.98}\]

Kalman 滤波器的直观解释和递推流程

1. Kalman 滤波器的直观解释

为了解释 Kalman 滤波器是如何在预测信息和观测信息进行“折中”的，这里用前文图 4.2 中提出的温度估计的问题来说明。

已知 \(t_0\) 时刻的温度 \(\bm{X}(0)=20.0^{\circ}\mathrm{C}\)，\(\sigma_x(0)=0.1^{\circ}\mathrm{C}\)，温度（状态）的变化规律可以描述为 \[\bm{X}(k)=\bm{X}(k-1)+\bm{w}(k-1) \tag{4.2.99}\] 系统噪声 \(\bm{w}(k-1)\) 表示在预测中没有考虑到或者不确定的因素，根据经验可设 \(\sigma_w^2(k-1)=(0.3^{\circ}\mathrm{C})^2\)。

观测方程为 \[\bm{Z}(k)=\bm{X}(k)+\bm{\Delta}(k) \tag{4.2.100}\] 观测噪声的中误差为 \(\sigma_{\Delta}(k)=0.2^{\circ}\mathrm{C}\)。在此问题中，\(\bm{Z}(1)=20.7^{\circ}\mathrm{C}\)，并且 \(\bm{\Phi}_{k,\ k-1}=1\) 和 \(\bm{H}_k=1\)。

首先，根据状态方程对 \(t_1\) 时刻的温度进行预测 \[\hat{X}(1,\ 0)=X(0)=20.0\] 预测方差为 \[\begin{aligned} D_{\hat{X}}(1,\ 0)&=\sigma_{\hat{X}}^2(1,\ 0)\\ &=\bm{\Phi}_{1,\ 0}\bm{D}_{\hat{X}}(0)\,\bm{\Phi}_{1,\ 0}^{\mathrm{T}}+D_w(0)\\ &=\sigma_{\hat{X}}^2(0)+\sigma_w^2(0)\\ &=0.10 \end{aligned}\] 对 \(t_1\) 时刻的预测温度进行测量更新 \[\begin{aligned} \bm{K}_1&=D_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}}\left(\bm{H}_kD_{\hat{X}}(k,\ k-1)\,\bm{H}_k^{\mathrm{T}}+D_{\Delta}(k)\right)^{-1}\\ &=\frac{\sigma_{\hat{X}}^2(1,\ 0)}{\sigma_{\hat{X}}^2(1,\ 0)+\sigma_{\Delta}^2(1)}\\ &=\frac{5}{7} \end{aligned}\qquad\text{①}\] \[\begin{aligned} \hat{X}(1)&=\hat{X}(1,\ 0)+K_1\left[\,Z(1)-H_1\hat{X}(1,\ 0)\,\right]\\ &=20.5 \end{aligned}\qquad\text{②}\] \[\begin{aligned} D_{\hat{X}}(1)&=\sigma_{\hat{X}}^2(1)\\ &=\left(1-K_1\right)\sigma_{\hat{X}}^2(1,\ 0)\\ &=0.03 \end{aligned}\qquad\text{③}\] 从式①增益矩阵的计算看到，增益矩阵 \(\bm{K}_1\) 由预测 \(\hat{X}(1,\ 0)\) 和观测 \(Z(1)\) 的方差共同决定，现将 \(\bm{K}_1\) 代入式②得到 \[\begin{aligned} \hat{X}(1)&=\frac{\sigma_{\Delta}^2(1)\,\hat{X}(1,\ 0)}{\sigma_{\hat{X}}^2(1,\ 0)+\sigma_{\Delta}^2(1)} +\frac{\sigma_{\hat{X}}^2(1,\ 0)\,Z(1)}{\sigma_{\hat{X}}^2(1,\ 0)+\sigma_{\Delta}^2(1)}\\ &=\left(1-K_1\right)\hat{X}(1,\ 0)+K_1Z(1) \end{aligned}\qquad\text{④}\] 显然，\(\hat{X}(1)\) 是 \(\hat{X}(1,\ 0)\) 和 \(Z(1)\) 的加权平均值，\(\hat{X}(1,\ 0)\) 和 \(Z(1)\) 对 \(\hat{X}(1)\) 的影响取决于各自方差的大小。当 \(\sigma_{\hat{X}}^2(1,\ 0)\) 越大，\(K_1\) 越大，观测值 \(Z(1)\) 对滤波影响越大，反之，\(\sigma_{\Delta}^2(1)\) 越大，\(K_1\) 越小，\(\hat{X}(1,\ 0)\) 就对滤波的影响越大。同样，将 \(\bm{K}_1\) 代入式③得到 \[\begin{aligned} \sigma_{\hat{X}}^2(1)&=\frac{\sigma_{\hat{X}}^2(1,\ 0)\,\sigma_{\Delta}^2(1)}{\sigma_{\hat{X}}^2(1,\ 0)+\sigma_{\Delta}^2(1)}\\ &=\frac{1}{\dfrac{1}{\sigma_{\Delta}^2(1)}+\dfrac{1}{\sigma_{\hat{X}}^2(1,\ 0)}} \end{aligned}\] 即 \[\frac{1}{\sigma_{\hat{X}}^2(1)}=\frac{1}{\sigma_{\Delta}^2(1)}+\frac{1}{\sigma_{\hat{X}}^2(1,\ 0)}\qquad\text{⑤}\] 显然，\(\hat{X}(1)\) 的权为 \(\hat{X}(1,\ 0)\) 的权和观测噪声权之和，所以这时滤波 \(\hat{X}(1)\) 的方差比观测噪声方差和预测方差都要小，Kalman 滤波就是这样达到“去噪”效果的。

下面通过船位平面位置的变化来直观地解释 Kalman 滤波的时间预测和观测值如何确定船位以及它的递推过程。在海图作业中，我们可以以船位为基准，根据航向、航速和海流等要素推算得下一个船位。但依靠这些要素去推算船位会将每一次的推算误差全部传递到下一次推算的船位上，误差的不断累积导致船位的推算与实际位置偏差很大，因此需用仪器对船体进行观测，利用观测值对每一次的船位推算进行修正。

以船的平面坐标为状态，有 \[\hat{\bm{X}}(k)=\left[\begin{array}{l}E(k)\\ N(k)\end{array}\right] \tag{4.2.101}\] 并假设可以对平面坐标进行观测，所以有 \[\bm{H}=\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}\]

Kalman 滤波对船位的递推过程可以用图 4.3 来表示。预测船位，量测船位和滤波估计（测量更新）船位分别用 \(\Diamond\)，\(\triangle\) 和 \(\bigstar\) 来表示。在 \(k-1\) 时刻滤波估计船位 \(\hat{\bm{X}}(k-1)\) 在点 \(A\)，一步预测得到 \(B\)：\(\hat{\bm{X}}(k,\ k-1)\)；通过量测传感器得到量测船位 \(C\)：\(\bm{Z}(k)\)。\(\overrightarrow{BC}\) 代表 \(k\) 时刻从量测中获得的新息向量 \(\bm{V}_z(k)=\bm{Z}(k)-\hat{\bm{X}}(k,\ k-1)\)，\(\overrightarrow{BD}\) 为 \(k\) 时刻增益矩阵与新息的乘积 \(\bm{K}_k\bm{V}_z(k)\)，即对预测船位 \(\hat{\bm{X}}(k,\ k-1)\) 的修正量。从平面位置上看，滤波估计的船位一定在预测船位和观测船位的连线上，并且

(1) 如果观测误差 \(\bm{D}_{\Delta}(k)\) 越小，滤波估计船位就越靠近量测船位 \(\triangle\)，反之就更靠近预测船位。这是由于 \(\bm{D}_{\Delta}(k)\) 越小，\(\bm{K}_k\) 就较大，这样滤波就受到了更多的观测值的影响。相反，\(\bm{D}_{\Delta}(k)\) 越大，滤波值就更接近预测。

(2) 考虑理想的情况：如果不存在系统噪声，经过一段时间的滤波递推后，\(\bm{D}_{\hat{X}}(k,\ k-1)\) 趋近于零，\(\bm{K}_k\) 也趋近于零，这样滤波 \(\hat{\bm{X}}(k)\) 就不再受观测值的影响，完全由预测值 \(\hat{\bm{X}}(k,\ k-1)\) 决定，这时滤波 \(\bigstar\) 与预测重合；反正，如果没有观测噪声，那么 \(\hat{\bm{X}}(k)\) 就完全由观测值决定，这时滤波 \(\bigstar\) 必然与量测 \(\triangle\) 重合。

2. Kalman 滤波的特点

从以上的推导和分析可以看出，Kalman 滤波算法有如下特点：

(1) 增益矩阵 \(\bm{K}_k\) 随着系统噪声 \(\bm{D}_w(k)\) 增大而增大，随着观测噪声 \(\bm{D}_{\Delta}(k)\) 的增大而减小。

(2) Kalman 滤波在时间域内进行递推，计算过程是一个不断“预测-修正”的过程。虽然 \(\hat{\bm{X}}(k)\) 是由从 \(t_1\) 时刻开始，利用所有的观测值 \(\bm{Z}(1)\)，\(\bm{Z}(2)\)，…，\(\bm{Z}(k-1)\)，和 \(\bm{Z}(k)\) 共同估计得到的，但在计算时，不需要存储大量数据占用内存，因此，这种方法非常便于实时处理。

(3) 滤波器的增益矩阵和滤波方差都与观测值无关，因此可以预先离线算出，从而减少实时在线计算量，也可以此来预先分析滤波的行为，如分析是否收敛和收敛的速度。

(4) Kalman 滤波将估计的信号看作在白噪声作用下的一个随机线性系统的输出，输入和输出是由状态方程和观测方程在时间域内给出，不仅适合平稳序列，也适用于非平稳序列，因此应用范围十分广泛。

3. Kalman 滤波器的递推流程

图 4.4 给出了 Kalman 滤波器的计算流程：

(1) 根据动态系统的运动规律建立微分方程，并且根据经验给出随机过程的均方值矩阵 \(\bm{D}_e(t)\)。如果微分方程是非线性的，取近似值为 \(\bm{X}^{*}(t_{k-1})\)，将其线性化，得到线性的微分方程。

(2) 解微分方程，得到离散化后的状态方程和系统噪声方差矩阵。

(3) 时间预测。

(4) 输入观测值 \(\bm{Z}(k)\)，建立观测方程，根据经验确定观测噪声方差。如果观测方程是非线性方程，取近似值为 \(\bm{X}^{*}(t_k)\)，将其线性化。

(5) 测量更新，得到并输出状态的滤波值 \(\hat{\bm{X}}(k)\) 和方差 \(\bm{D}_{\hat{X}}(k)\)。如果没有新的观测值，即结束递推，否则回到 (1)。

如果系统的微分方程和观测方程是线性函数，就可以跳过 (1) 和 (4)，只进行 (2)(3)(5) 的计算就可以了。

在进行 Kalman 滤波计算时，计算机不需要保存历史观测值及其相关信息，只需要输出 \(\hat{\bm{X}}(k)\) 和 \(\bm{D}_{\hat{X}}(k)\)，并将它们传递到下一个时刻，当对 \(t_{k+1}\) 时刻的状态预测完成后，即可释放 \(t_k\) 时刻的滤波结果，进行 \(t_{k+1}\) 时刻的滤波计算。与“批处理”相比，Kalman 滤波是占用内存较少并且高效的算法。

Kalman 滤波与最小二乘估计的关系

在第 2 章中，我们学习了最小二乘估计和递推的最小二乘估计。在本章 Kalman 滤波的推导中，若将预测信息 \(\hat{\bm{X}}(k,\ k-1)\) 看做虚拟观测值，利用“广义最小二乘”也可推导得到 Kalman 滤波。这里分析和给出 Kalman 滤波与第 2 章中的最小估计和递推的最小二乘估计的关系。

1. Kalman 滤波与递推的最小二乘估计之间的关系

若状态方程为：\(\dot{\bm{X}}(t)=0\)，即状态为随机参数，那么离散化后的状态方程为 \[\bm{X}(k)=\bm{X}(k-1) \tag{4.2.102}\] 这时的滤波递推公式为 \[\hat{\bm{X}}(k,\ k-1)=\hat{\bm{X}}(k-1) \tag{4.2.103}\] \[\bm{D}_{\hat{X}}(k,\ k-1)=\bm{D}_{\hat{X}}(k-1) \tag{4.2.104}\] \[\bm{K}_k=\bm{D}_{\hat{X}}(k-1)\,\bm{H}_k^{\mathrm{T}}\left(\bm{H}_k\bm{D}_{\hat{X}}(k-1)\,\bm{H}_k^{\mathrm{T}}+\bm{D}_{\Delta}(k)\right)^{-1} \tag{4.2.105}\] \[\hat{\bm{X}}(k)=\hat{\bm{X}}(k-1)+\bm{K}_k\left[\,\bm{Z}(k)-\bm{H}_k\hat{\bm{X}}(k-1)\,\right] \tag{4.2.106}\] \[\bm{D}_{\hat{X}}(k)=\left(\bm{I}-\bm{K}_k\bm{H}_k\right)\bm{D}_{\hat{X}}(k-1) \tag{4.2.107}\] 可以看出，当状态方程为 \(\dot{\bm{X}}(t)=0\) 时，Kalman 滤波就退化为在第 2 章中介绍的递推的最小二乘。也正是由于 \(\dot{\bm{X}}(t)=0\)，所以把递推的最小二乘称为“静态滤波”。

2. Kalman 滤波与“snapshot”最小二乘估计之间的关系

在“snapshot”最小二乘估计中，只用当前观测值来估计参数，并不考虑参数的先验信息，估计的参数与历史观测值无关，那么可以认为参数的先验信息未知或者其方差无穷大，即 \(\bm{D}_{\hat{X}}(k,\ k-1)=\infty\)，也就是 \(\bm{W}_{k,\ k-1}=0\)，那么式 (4.2.73) 成为 \[\bm{W}_k=\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\bm{H}_k \tag{4.2.108}\] 和式 (4.2.75) 成为 \[\bm{W}_k\hat{\bm{X}}(k)=\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\bm{Z}(k) \tag{4.2.109}\] 将式 (4.2.108) 代入式 (4.2.109)，并求解 \(\hat{\bm{X}}(k)\) 得到 \[\hat{\bm{X}}(k)=\left(\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}\bm{H}_k\right)^{-1}\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}\bm{Z}(k) \tag{4.2.110}\] 和 \[\bm{D}_{\hat{X}}(k)=\left(\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(k)\bm{H}_k\right)^{-1} \tag{4.2.111}\] 可见，当不考虑参数的先验信息时，Kalman 滤波就退化成为了“snapshot”最小二乘估计。

算例分析

例 4.1如图 4.5 所示，小车在直线轨道上沿着箭头方向匀加速地移动，加速度近似为 \(a=10.0\,\mathrm{m/s}^2\)。小车的运动可以描述为 \[\dot{v}(t)=a+e(t) \tag{4.3.1}\] 其中，\(v(t)\) 为小车运动的速度；\(e(t)\) 为白噪声，均方值为 \(D_e(t)=(0.3)^2\,(\mathrm{m}^2/\mathrm{s}^3)\)。设小车的位移为 \(r(t)\)，在 \(t_0\) 时刻 \(\left[\begin{array}{ll}r(0) & v(0)\end{array}\right]^{\mathrm{T}}=\left[\begin{array}{ll}0.0 & 0.0\end{array}\right]^{\mathrm{T}}\)，方差分别为 \(\sigma_r^2(0)=(1.0\,\mathrm{m})^2\) 和 \(\sigma_v^2(0)=(1.0\,\mathrm{m/s})^2\)。在 \(\Delta t=1\,\mathrm{s}\) 后的 \(t_1\) 时刻，观测到小车的位移为 \(Z(1)=5.7\,\mathrm{m}\)，观测噪声的方差为 \(\sigma_{\Delta}^2(1)=(0.2\,\mathrm{m})^2\)。用 Kalman 滤波求小车在 \(t_1\) 的位移和速度，以及它们的方差。

解：根据运动方程，此问题中的状态为 \[\bm{X}(t)=\left[\begin{array}{l}r(t)\\ v(t)\end{array}\right] \tag{4.3.2}\] 初值为 \[\bm{X}(0)=\left[\begin{array}{ll}0.0 & 0.0\end{array}\right]^{\mathrm{T}}\ ;\quad \bm{D}_{\bm{X}}(0)=\begin{bmatrix}(1.0\,\mathrm{m})^2 & \\ & (1.0\,\mathrm{m/s})^2\end{bmatrix}\] 小车的运动规律可描述为 \[\begin{bmatrix}\dot{r}(t)\\ \dot{v}(t)\end{bmatrix} =\begin{bmatrix}0 & 1\\ 0 & 0\end{bmatrix}\begin{bmatrix}r(t)\\ v(t)\end{bmatrix} +\begin{bmatrix}0\\ 1\end{bmatrix}a+\begin{bmatrix}0\\ 1\end{bmatrix}e(t)\] 在此问题中 \[\bm{A}=\begin{bmatrix}0 & 1\\ 0 & 0\end{bmatrix},\quad \bm{B}u(\tau)=\begin{bmatrix}0\\ 1\end{bmatrix}a,\quad \bm{C}=\begin{bmatrix}0\\ 1\end{bmatrix}\] 根据例 3.7，可知此问题的状态转移矩阵为 \[\bm{\Phi}_{k,\ k-1}=\begin{bmatrix}1 & t_k-t_{k-1}\\ 0 & 1\end{bmatrix}\] 离散化的状态方程为 \[\bm{X}(k)=\bm{\Phi}(t_k,\ t_{k-1})\bm{X}(k-1)+\int_{t_{k-1}}^{t_k}\left[\,\bm{\Phi}(t_k,\ \tau)\bm{B}u(\tau)\,\right]\mathrm{d}\tau +\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\bm{C}(\tau)e(\tau)\,\mathrm{d}\tau \tag{4.3.3}\] 设 \(\Delta t=t_k-t_{k-1}\)，并将已知条件代入上式 \[\bm{X}(k)=\begin{bmatrix}1 & \Delta t\\ 0 & 1\end{bmatrix}\bm{X}(k-1) +a\int_{t_{k-1}}^{t_k}\begin{bmatrix}t_k-\tau\\ 1\end{bmatrix}\mathrm{d}\tau+\bm{w}(k-1)\] 积分得到状态方程为 \[\bm{X}(k)=\begin{bmatrix}1 & \Delta t\\ 0 & 1\end{bmatrix}\bm{X}(k-1) +a\begin{bmatrix}\dfrac{\Delta t^2}{2}\\[6pt] \Delta t\end{bmatrix}+\bm{w}(k-1)\] 其中 \[\bm{w}(k-1)=\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\bm{C}(\tau)e(\tau)\,\mathrm{d}\tau \tag{4.3.4}\] 从例 3.7 知道此问题中的系统噪声的方差为 \[\bm{D}_w(k-1)=(0.3)^2\times\begin{bmatrix}\dfrac{(\Delta t)^3}{3} & \dfrac{(\Delta t)^2}{2}\\[8pt] \dfrac{(\Delta t)^2}{2} & \Delta t\end{bmatrix} \tag{4.3.5}\] 观测方程为 \[\bm{Z}(k)=\left[\begin{array}{ll}1 & 0\end{array}\right]\begin{bmatrix}r(t)\\ v(t)\end{bmatrix}+\Delta(k)\] 根据 Kalman 滤波基础方程，对 \(t_1\) 时刻状态时间预测为 \[\hat{\bm{X}}(1,\ 0)=\begin{bmatrix}5\\ 10\end{bmatrix}\] \[\bm{D}_{\hat{X}}(1,\ 0)=\begin{bmatrix}2.03 & 1.05\\ 1.05 & 1.09\end{bmatrix}\] 增益矩阵为 \[\bm{K}_1=\begin{bmatrix}0.98\\ 0.50\end{bmatrix}\] 新息为 \[\bm{V}(1,\ 0)=0.70\] 最后，\(t_1\) 时刻的状态滤波值为 \[\hat{\bm{X}}(1)=\begin{bmatrix}5.7\\ 10.4\end{bmatrix}\] 其方差为 \[\bm{D}_{\hat{X}}(1)=\begin{bmatrix}0.04 & 0.02\\ 0.02 & 0.56\end{bmatrix}\]

例 4.2船舶在海面上航行，设船舶在 WGS-84 坐标系下的坐标为 \(\bm{P}(t)=\left[\begin{array}{lll}X(t) & Y(t) & Z(t)\end{array}\right]^{\mathrm{T}}\)，考虑到船舶行驶速度较平稳，将船舶的运动表示为：\(\ddot{\bm{P}}(t)=\bm{e}(t)\)，\(\bm{e}(t)\) 为白噪声过程，并且 \(\mathrm{Cov}\left[\,\bm{e}(t),\ \bm{e}(\tau)\,\right]=\bm{D}_e\delta(t-\tau)\)，其中

\[\bm{D}_e(t)=\mathrm{diag}\left(\begin{array}{lll}\sigma_{e_x}^2 & \sigma_{e_y}^2 & \sigma_{e_z}^2\end{array}\right) =\mathrm{diag}\left(\begin{array}{lll}0.1^2 & 0.1^2 & 0.1^2\end{array}\right)\] 此外，船舶装载了 GPS 设备，利用 GPS 实时地（每隔 \(1\,\mathrm{s}\)）估计了船舶的坐标。现将 GPS 估计的船舶坐标作为观测值 \(\bm{Z}_P(t_k)=\left[\begin{array}{lll}Z_X(t_k) & Z_Y(t_k) & Z_Z(t_k)\end{array}\right]^{\mathrm{T}}\)，测噪声方差为 \(\bm{D}_{\Delta}(k)\)，用 Kalman 滤波对船舶的坐标进行估计。

在本问题中，为了方便起见，将港口码头设为坐标原点（WGS-84 坐标系进行平移）。船舶在码头附近出发，初始状态为 \[\bm{X}(t_0)=\left[\begin{array}{llllll}X(t_0) & Y(t_0) & Z(t_0) & \dot{X}(t_0) & \dot{Y}(t_0) & \dot{Z}(t_0)\end{array}\right]^{\mathrm{T}} =\left[\begin{array}{llllll}0 & 0 & 0 & 0 & 0 & 0\end{array}\right]^{\mathrm{T}}\] 初始方差为 \[\bm{D}_X(t_0)=\mathrm{diag}\left(\left[\begin{array}{llllll}1 & 1 & 1 & 1 & 1 & 1\end{array}\right]\times 10^2\right)\] 观测值来自于 GPS 的标准单点定位结果。一般情况下，单点定位结果的三维坐标是相关的，为了简单起见，这里假设 \(\left[\begin{array}{lll}Z_X(t_k) & Z_Y(t_k) & Z_Z(t_k)\end{array}\right]^{\mathrm{T}}\) 互不相关，并设其方差为 \[\bm{D}_{\Delta}(k)=\begin{bmatrix}3^2 & 0 & 0\\ 0 & 3^2 & 0\\ 0 & 0 & 3^2\end{bmatrix}\mathrm{m}^2\]

解：根据题意，设状态为 \[\bm{X}(t)=\left[\begin{array}{llllll}X(t) & Y(t) & Z(t) & \dot{X}(t) & \dot{Y}(t) & \dot{Z}(t)\end{array}\right]^{\mathrm{T}} \tag{4.3.6}\] 将船舶的运动规律描述为 \(\ddot{\bm{P}}(t)=\bm{e}(t)\)，那么船舶运动的微分方程为 \[\dot{\bm{X}}(t)=\bm{A}(t)\bm{X}(t)+\bm{C}(t)\bm{e}(t) \tag{4.3.7}\] 其中 \[\bm{A}(t)=\begin{bmatrix} 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix}\ ,\quad \bm{C}(t)=\begin{bmatrix} 0 & 0 & 0\\ 0 & 0 & 0\\ 0 & 0 & 0\\ 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix}\ ,\quad \bm{e}(t)=\begin{bmatrix}e_x(t)\\ e_y(t)\\ e_z(t)\end{bmatrix} \tag{4.3.8}\] 解微分方程，并离散化 \[\bm{X}(k)=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{w}(k-1) \tag{4.3.9}\] 其中 \[\bm{\Phi}_{k,\ k-1}=\begin{bmatrix} 1 & 0 & 0 & t_k-t_{k-1} & 0 & 0\\ 0 & 1 & 0 & 0 & t_k-t_{k-1} & 0\\ 0 & 0 & 1 & 0 & 0 & t_k-t_{k-1}\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \tag{4.3.10}\] 系统噪声为 \[\bm{w}(k-1)=\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\bm{C}(\tau)\bm{e}(\tau)\,\mathrm{d}\tau \tag{4.3.11}\] 其方差为 \[\begin{aligned} \underset{6\times 6}{\bm{D}_w(k-1)}&=\int_{t_{k-1}}^{t_k}\bm{\Phi}(t_k,\ \tau)\bm{C}(\tau)\bm{D}_e(\tau)\bm{C}^{\mathrm{T}}(\tau)\bm{\Phi}^{\mathrm{T}}(t_k,\ \tau)\,\mathrm{d}\tau\\ &=\begin{bmatrix}\dfrac{T^3}{3}\underset{3\times 3}{\bm{D}_e} & \dfrac{T^2}{2}\underset{3\times 3}{\bm{D}_e}\\[10pt] \dfrac{T^2}{2}\underset{3\times 3}{\bm{D}_e} & T\underset{3\times 3}{\bm{D}_e}\end{bmatrix} \end{aligned}\] 其中，\(T=t_k-t_{k-1}=1\,\mathrm{s}\)。

观测值与状态向量的关系为 \[\bm{Z}_P(k)=\bm{H}\bm{X}(k)+\bm{\Delta}(k)\] 其中 \[\bm{Z}_P(t_k)=\left[\begin{array}{lll}Z_X(t_k) & Z_Y(t_k) & Z_Z(t_k)\end{array}\right]^{\mathrm{T}}\] \[\bm{H}=\begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix}\] \(\bm{\Delta}(k)\) 是观测噪声向量，方差已由题目给出。根据以上模型和 Kalman 滤波基本公式，计算得到每个观测时刻船舶状态的滤波值 \[\hat{\bm{X}}(k)=\left[\begin{array}{llllll}\hat{X}(k) & \hat{Y}(k) & \hat{Z}(k) & \hat{\dot{X}}(k) & \hat{\dot{Y}}(k) & \hat{\dot{Z}}(k)\end{array}\right]^{\mathrm{T}}\] 图 4.6 给出了船舶在 XOY 坐标平面中的实际轨迹，即真实轨迹 \((X(k),\ Y(k))\)、观测轨迹 \((X_Z(k),\ Y_Z(k))\) 和滤波估计的轨迹 \((\hat{X}(k),\ \hat{Y}(k))\)。从图中可以看到，观测值有较大的噪声，经过滤波后，估计的轨迹较为平滑并且与真实估计比较接近。

滤波的方差矩阵为 \[\bm{D}_{\hat{X}}(k)=\begin{bmatrix} \sigma_{\hat{X}}^2(k) & \sigma_{\hat{X}\hat{Y}}(k) & \sigma_{\hat{X}\hat{Z}}(k) & \sigma_{\hat{X}\dot{\hat{X}}}(k) & \sigma_{\hat{X}\dot{\hat{Y}}}(k) & \sigma_{\hat{X}\dot{\hat{Z}}}(k)\\ \sigma_{\hat{X}\hat{Y}}(k) & \sigma_{\hat{Y}}^2(k) & \sigma_{\hat{Y}\hat{Z}}(k) & \sigma_{\hat{Y}\dot{\hat{X}}}(k) & \sigma_{\hat{Y}\dot{\hat{Y}}}(k) & \sigma_{\hat{Y}\dot{\hat{Z}}}(k)\\ \sigma_{\hat{X}\hat{Z}}(k) & \sigma_{\hat{Y}\hat{Z}}(k) & \sigma_{\hat{Z}}^2(k) & \sigma_{\hat{Z}\dot{\hat{X}}}(k) & \sigma_{\hat{Z}\dot{\hat{Y}}}(k) & \sigma_{\hat{Z}\dot{\hat{Z}}}(k)\\ \sigma_{\hat{X}\dot{\hat{X}}}(k) & \sigma_{\hat{Y}\dot{\hat{X}}}(k) & \sigma_{\hat{Z}\dot{\hat{X}}}(k) & \sigma_{\dot{\hat{X}}}^2(k) & \sigma_{\dot{\hat{X}}\dot{\hat{Y}}}(k) & \sigma_{\dot{\hat{X}}\dot{\hat{Z}}}(k)\\ \sigma_{\hat{X}\dot{\hat{Y}}}(k) & \sigma_{\hat{Y}\dot{\hat{Y}}}(k) & \sigma_{\hat{Z}\dot{\hat{Y}}}(k) & \sigma_{\dot{\hat{X}}\dot{\hat{Y}}}(k) & \sigma_{\dot{\hat{Y}}}^2(k) & \sigma_{\dot{\hat{Y}}\dot{\hat{Z}}}(k)\\ \sigma_{\hat{X}\dot{\hat{Z}}}(k) & \sigma_{\hat{Y}\dot{\hat{Z}}}(k) & \sigma_{\hat{Z}\dot{\hat{Z}}}(k) & \sigma_{\dot{\hat{X}}\dot{\hat{Z}}}(k) & \sigma_{\dot{\hat{Y}}\dot{\hat{Z}}}(k) & \sigma_{\dot{\hat{Z}}}^2(k) \end{bmatrix}\]

图 4.7 给出了滤波的中误差 \(\sigma_{\hat{X}}\) 和 \(\sigma_{\hat{Y}}\) 以及测值对状态的增益部分：\(\bm{K}_k\bm{V}(k,\ k-1)\)。从图中看到，由于初始状态不确定，设置的初始状态的方差较大，但是经过一段时间的滤波后，\(\bm{D}_{\hat{X}}(k)\) 能“摆脱”初始状态的影响，中误差收敛到 \(0.8\,\mathrm{m}\) 附近并保持稳定，数值 \(0.8\) 是系统噪声方差和观测值噪声方差的“折中”。观察增益部分 \(\bm{K}_k\bm{V}(k,\ k-1)\)，随着滤波的递推，\(\bm{K}_k\bm{V}(k,\ k-1)\) 对 \(\hat{X}(k-1)\) 和 \(\hat{Y}(k-1)\) 的增益都迅速减小，并逐步趋于稳定，维持在 \(\pm 1\,\mathrm{m}\) 以内。

例 4.3两个弹簧分别固定在两个竖直的墙面 \(A\) 和 \(B\) 上（如图 4.8），并被质点 \(m\) 链接，质点 \(m\) 的质量为 \(M\)。在墙面 \(A\) 处的 \(P\) 点有一观测仪器，\(P\) 点距离质点的竖直距离为 \(h\)。仪器可观测到水平运动的质点 \(m\) 到 \(P\) 点的距离 \(\rho\) 和在此方向的速度 \(\dot{\rho}\)。设质点到 \(O\) 点的水平距离和运动速度为状态变量，\(\bm{X}=\left[\begin{array}{ll}x & v\end{array}\right]^{\mathrm{T}}\)，得到质点 \(m\) 的运动方程为 \[\ddot{x}=\dot{v}=-(k_1+k_2)(x-R)/M \tag{4.3.12}\] 上式中，\(k_1\) 和 \(k_2\) 为两个弹簧的弹簧系数，\(R\) 为质点 \(m\) 处于静止平衡状态时距离 \(O\) 点的距离。已知在 \(t_0\) 时刻质点的状态，在 \(\Delta t=1\,\mathrm{s}\) 后的 \(t_1\) 时刻观测到 \(\rho(1)\) 和 \(\dot{\rho}(1)\)。用 Kalman 滤波估计质点 \(m\) 在 \(t_1\) 时刻的状态和方差。

解算本题所需要的常量和已知值见表 4.2。

解：由于 \(R\) 为常数，并不影响以下的估计结果，为了简单起见，这里设 \(R=0\)，并设 \[\omega^2=\frac{k_1+k_2}{M} \tag{4.3.13}\] 那么 \[\omega=1.02\] 式 (4.3.12) 为 \[\ddot{x}+\omega^2x=0 \tag{4.3.14}\] 由此得到一阶微分方程为 \[\begin{bmatrix}\dot{x}\\ \dot{v}\end{bmatrix} =\begin{bmatrix}0 & 1\\ -\omega^2 & 0\end{bmatrix}\begin{bmatrix}x\\ v\end{bmatrix} \tag{4.3.15}\] 由于此系统没有噪声，所以 \(\bm{D}_e(t)=0\)。

解微分方程，并设 \(\Delta t=t_k-t_{k-1}\)，状态转移矩阵为 \[\bm{\Phi}_{k,\ k-1}=\begin{bmatrix}\cos\omega\Delta t & \dfrac{\sin\omega\Delta t}{\omega}\\[8pt] -\omega\sin\omega\Delta t & \cos\omega\Delta t\end{bmatrix} \tag{4.3.16}\] 将 \(\omega=1.02\) 和 \(\Delta t=1\,\mathrm{s}\) 代入转移矩阵 \[\bm{\Phi}_{k,\ k-1}=\begin{bmatrix}0.52 & 0.83\\ -0.87 & 0.52\end{bmatrix}\] 根据状态方程和初始值，可以预测得到 \[\begin{bmatrix}\hat{x}(1,\ 0)\\ \hat{v}(1,\ 0)\end{bmatrix} =\bm{\Phi}_{1,\ 0}\begin{bmatrix}x(0)\\ v(0)\end{bmatrix} =\begin{bmatrix}1.57\\ -2.61\end{bmatrix}\] 状态方程中并没有系统噪声，所以在估计中 \(\bm{D}_w(k-1)=0\)。时间预测的方差为 \[\bm{D}_{\hat{X}}(1,\ 0)=\bm{\Phi}_{1,\ 0}\bm{D}_{\bm{X}}(0)\bm{\Phi}_{1,\ 0}^{\mathrm{T}} =\begin{bmatrix}0.34 & -0.38\\ -0.38 & 0.83\end{bmatrix}\] 观测方程为 \[\begin{bmatrix}\rho(k)\\ \dot{\rho}(k)\end{bmatrix} =\begin{bmatrix}\sqrt{x^2(k)+h^2}\\[6pt] \dfrac{x(k)v(k)}{\sqrt{x^2(k)+h^2}}\end{bmatrix} +\begin{bmatrix}\Delta_{\rho}\\ \Delta_{\dot{\rho}}\end{bmatrix} \tag{4.3.17}\] 显然观测方程为非线性方程，需要将其线性化。设 \(\bm{X}=\left[\begin{array}{ll}x & v\end{array}\right]^{\mathrm{T}}\) 在 \(t_k\) 时的近似值为 \(\bm{X}^{*}=\left[\begin{array}{ll}x^{*} & v^{*}\end{array}\right]^{\mathrm{T}}\)，将观测方程用泰勒级数展开并略去高阶项，得到 \[\begin{bmatrix}\rho(k)\\ \dot{\rho}(k)\end{bmatrix}-\begin{bmatrix}\rho^{*}(k)\\ \dot{\rho}^{*}(k)\end{bmatrix} =\bm{H}_k\begin{bmatrix}x(k)-x^{*}\\ v(k)-v^{*}\end{bmatrix}+\begin{bmatrix}\Delta_{\rho}\\ \Delta_{\dot{\rho}}\end{bmatrix} \tag{4.3.18}\] 其中 \[\begin{bmatrix}\rho^{*}(k)\\ \dot{\rho}^{*}(k)\end{bmatrix} =\begin{bmatrix}\sqrt{(x^{*})^2+h^2}\\[6pt] \dfrac{x^{*}v^{*}}{\sqrt{(x^{*})^2+h^2}}\end{bmatrix}\ ,\quad \bm{H}_k=\begin{bmatrix} \dfrac{x^{*}}{\sqrt{(x^{*})^2+h^2}} & 0\\[10pt] \dfrac{v^{*}}{\sqrt{(x^{*})^2+h^2}}-\dfrac{v^{*}(x^{*})^2}{\sqrt{\left[\,(x^{*})^2+h^2\,\right]^3}} & \dfrac{x^{*}}{\sqrt{(x^{*})^2+h^2}} \end{bmatrix}\]

新息为 \[\bm{V}(k,\ k-1)=\begin{bmatrix}\rho(k)\\ \dot{\rho}(k)\end{bmatrix}-\begin{bmatrix}\rho^{*}(k)\\ \dot{\rho}^{*}(k)\end{bmatrix} -\bm{H}_k\begin{bmatrix}\hat{x}(k,\ k-1)-x^{*}\\ \hat{v}(k,\ k-1)-v^{*}\end{bmatrix} \tag{4.3.19}\] 如果将上式中的近似值 \(\bm{X}^{*}=\left[\begin{array}{ll}x^{*} & v^{*}\end{array}\right]^{\mathrm{T}}\) 取值为预测值 \(x^{*}=\hat{x}(k,\ k-1)\)，\(v^{*}=\hat{v}(k,\ k-1)\)；代入式 (4.3.19)，那么新息为 \[\bm{V}(k,\ k-1)=\begin{bmatrix}\rho(k)\\ \dot{\rho}(k)\end{bmatrix}-\begin{bmatrix}\rho^{*}(k)\\ \dot{\rho}^{*}(k)\end{bmatrix}\] 在 \(t_1\) 时刻 \[\begin{bmatrix}\rho^{*}(1)\\ \dot{\rho}^{*}(1)\end{bmatrix}=\begin{bmatrix}5.62\\ -0.73\end{bmatrix}\ ,\quad \bm{H}_1=\begin{bmatrix}0.28 & 0\\ -0.43 & 0.28\end{bmatrix}\] \[\bm{V}(1,\ 0)=\begin{bmatrix}\rho(k)\\ \dot{\rho}(k)\end{bmatrix}-\begin{bmatrix}\rho^{*}(k)\\ \dot{\rho}^{*}(k)\end{bmatrix} =\begin{bmatrix}-0.06\\ 2.04\end{bmatrix}\] 增益矩阵为 \[\begin{aligned} \bm{K}_1&=\bm{D}_{\hat{X}}(1,\ 0)\bm{H}_1^{\mathrm{T}} \left(\bm{H}_1\bm{D}_{\hat{X}}(1,\ 0)\bm{H}_1^{\mathrm{T}}+\bm{D}_{\Delta}(1)\right)^{-1}\\ &=\begin{bmatrix}0.26 & -1.02\\ 0.23 & 1.80\end{bmatrix} \end{aligned}\] 滤波估计为 \[\hat{\bm{X}}(1)=\hat{\bm{X}}(1,\ 0)+\bm{K}_1\bm{V}(1,\ 0)=\begin{bmatrix}-0.52\\ 1.04\end{bmatrix}\] 滤波方差为 \[\bm{D}_{\hat{X}}(1)=\left(\bm{I}-\bm{K}_1\bm{H}_1\right)\bm{D}_{\hat{X}}(1,\ 0) =\begin{bmatrix}\sigma_{\hat{x}}^2 & \sigma_{\hat{x}\hat{v}}\\ \sigma_{\hat{x}\hat{v}} & \sigma_{\hat{v}}^2\end{bmatrix}\] 其中 \(\sigma_{\hat{x}}=0.24\,\mathrm{m}\)，\(\sigma_{\hat{v}}=0.38\,\mathrm{m/s}\)，

线性离散系统的最优预测与平滑

在前面介绍的 Kalman 滤波器中，状态方程进行时间预测是用观测值 \(\bm{Z}(1)\)，\(\bm{Z}(2)\)，…\(\bm{Z}(k-1)\) 对 \(\bm{X}(k)\) 的估计 \[\hat{\bm{X}}(k,\ k-1)=E\left[\,\bm{X}(k)\mid\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\,\right] \tag{4.4.1}\] 在获得观测值 \(\bm{Z}(k)\) 后对预测值 \(\hat{\bm{X}}(k,\ k-1)\) 进行更新，得到滤波 \(\hat{\bm{X}}(k)\) \[\hat{\bm{X}}(k)=E\left[\,\bm{X}(k)\mid\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\bm{Z}(k)\,\right] \tag{4.4.2}\] 每一步预测后总伴随有观测值的更新。在这里，将式 (4.4.1) 的预测扩展到更一般的情况：在 \(k\) 时刻估计 \(l\ (l>k)\) 时刻的状态，记为 \(\hat{\bm{X}}(l,\ k)\)，这样的滤波估计称为预测。此外，还可以用 \(k\) 时刻所有的观测值估计过去某一个时刻 \(j\ (j<k)\) 的状态，这样的估值称为平滑 \(\hat{\bm{X}}(j,\ k)\)。

线性离散系统的最优预测

线性离散系统 \[\bm{X}(k)=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{w}(k-1) \tag{4.4.3}\] \[\bm{Z}(k)=\bm{H}_k\bm{X}(k)+\bm{\Delta}(k) \tag{4.4.4}\] 其中，\(\bm{\Phi}_{k,\ k-1}\) 为 \(t_{k-1}\) 时刻至 \(t_k\) 时刻的状态转移矩阵；\(\bm{w}(k-1)\) 系统噪声；\(\bm{H}_k\) 为量测矩阵；\(\bm{\Delta}(k)\) 为量测噪声序列。随机模型为 \[\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{4.4.5}\] 此外，已知 \(\hat{\bm{X}}(0)\)，并且有 \[\begin{aligned} E\left[\,\hat{\bm{X}}(0)\,\right]&=E\left[\,\bm{X}(t_0)\,\right]\\ \mathrm{Var}\left[\,\hat{\bm{X}}(0)\,\right]&=\bm{D}_{\hat{X}}(0) \end{aligned} \tag{4.4.6}\] \[\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{4.4.7}\] 根据 Kalman 滤波基础公式 \[\hat{\bm{X}}(k)=\hat{\bm{X}}(k,\ k-1)+\bm{K}_k\bm{V}(k,\ k-1)\] 当无观测值 \(\bm{Z}(k)\) 时，\(\bm{Z}(k)\) 的方差无穷大，\(\bm{D}_{\Delta}(k)\rightarrow\infty\)，那么 \(\bm{K}_k\rightarrow 0\)，所以 \[\hat{\bm{X}}(k)=\hat{\bm{X}}(k,\ k-1)\] 这意味着无观测值 \(\bm{Z}(k)\) 时，最优滤波 \(\hat{\bm{X}}(k)\) 就是 \(\hat{\bm{X}}(k,\ k-1)\)。下面从一般情况来证明预测就是在无观测值或者观测值不可用时的最优估计。

设有观测数据 \(\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\bm{Z}(k)\)，对 \(l\ (l>k)\) 时刻的状态 \(\bm{X}(l)\) 进行估计的最小方差估计 \(\hat{\bm{X}}(l,\ k)\) 为 \[\hat{\bm{X}}(l,\ k)=E\left[\,\bm{X}(l)\mid\bm{Z}(1),\ \bm{Z}(2),\ \cdots,\ \bm{Z}(k)\,\right] \tag{4.4.8}\] \(k\) 时刻到 \(l\) 时刻的状态方程为 \[\bm{X}(l)=\bm{\Phi}_{l,\ k}\bm{X}(k)+\sum_{i=k+1}^{l}\bm{\Phi}_{l,\ i}\bm{w}(i-1) \tag{4.4.9}\] 将式 (4.4.9) 代入式 (4.4.8) 可得 \[\begin{aligned} \hat{\bm{X}}(l,\ k)&=E\left[\left.\left(\bm{\Phi}_{l,\ k}\bm{X}(k)+\sum_{i=k+1}^{l}\bm{\Phi}_{l,\ i}\bm{w}(i-1)\right)\right|\bm{Z}(1),\ \bm{Z}(2),\ \cdots,\ \bm{Z}(k)\right]\\ &=E\left[\,\bm{\Phi}_{l,\ k}\bm{X}(k)\mid\bm{Z}(1),\ \cdots,\ \bm{Z}(k)\,\right] +E\left[\left.\sum_{i=k+1}^{l}\bm{\Phi}_{l,\ i}\bm{w}(i-1)\right|\bm{Z}(1),\ \cdots,\ \bm{Z}(k)\right]\\ &=\bm{\Phi}_{l,\ k}E\left[\,\bm{X}(k)\mid\bm{Z}(1),\ \cdots,\ \bm{Z}(k)\,\right] +\sum_{i=k+1}^{l}\bm{\Phi}_{l,\ k}E\left[\,\bm{w}(i-1)\mid\bm{Z}(1),\ \cdots,\ \bm{Z}(k)\,\right] \end{aligned} \tag{4.4.10}\] 随机变量 \(\bm{w}(k)\)，\(\bm{w}(k+1)\cdots\) 与 \(\bm{Z}(1)\)，…，\(\bm{Z}(k)\) 相互独立的。考虑到 \(\bm{w}(k)\)，\(\bm{w}(k+1)\cdots\) 的期望为零，则有 \[\sum_{i=k+1}^{l}\bm{\Phi}_{l,\ i}E\left[\,\bm{w}(i-1)\mid\bm{Z}(1),\ \cdots,\ \bm{Z}(k)\,\right]=0 \tag{4.4.11}\] 那么，式 (4.4.10) 为 \[\hat{\bm{X}}(l,\ k)=\bm{\Phi}_{l,\ k}E\left[\,\bm{X}(k)\mid\bm{Z}(1),\ \cdots,\ \bm{Z}(k)\,\right] \tag{4.4.12}\] 其中 \(E\left[\,\bm{X}(k)\mid\bm{Z}(1),\ \cdots,\ \bm{Z}(k)\,\right]\) 是 \(\bm{X}(k)\) 的最小方差估计，也是滤波值 \(\hat{\bm{X}}(k)\)，所以 \[\hat{\bm{X}}(l,\ k)=\bm{\Phi}_{l,\ k}\hat{\bm{X}}(k) \tag{4.4.13}\] 上式为 \(\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\bm{Z}(k)\) 对 \(\bm{X}(l)\ (l>k)\) 的最小方差估计，即对 \(\bm{X}(l)\) 的最优预测值。式 (4.4.13) 也可以表达为下面的递推表达式 \[\begin{aligned} \hat{\bm{X}}(l,\ k)&=\bm{\Phi}_{l,\ l-1}\bm{\Phi}_{l-1,\ k}\hat{\bm{X}}(k)\\ &=\bm{\Phi}_{l,\ l-1}\hat{\bm{X}}(l-1,\ k) \end{aligned} \tag{4.4.14}\]

下面推导预测 \(\hat{\bm{X}}(l,\ k)\) 的方差矩阵。预测误差可表示为 \[\begin{aligned} \Delta\hat{\bm{X}}(l,\ k)&=\bm{X}(l)-\hat{\bm{X}}(l,\ k)\\ &=\bm{\Phi}_{l,\ k}\hat{\bm{X}}(k)+\sum_{i=k+1}^{l}\bm{\Phi}_{l,\ i}\bm{w}(i-1)-\bm{\Phi}_{l,\ k}\hat{\bm{X}}(k)\\ &=\bm{\Phi}_{l,\ k}\left[\,\bm{X}(k)-\hat{\bm{X}}(k)\,\right]+\sum_{i=k+1}^{l}\bm{\Phi}_{l,\ i}\bm{w}(i-1)\\ &=\bm{\Phi}_{l,\ k}\Delta\hat{\bm{X}}(k)+\sum_{i=k+1}^{l}\bm{\Phi}_{l,\ i}\bm{w}(i-1) \end{aligned} \tag{4.4.15}\] 显然 \(\Delta\hat{\bm{X}}(l,\ k)\) 的误差来源于 \(k\) 时刻的滤波误差和从 \(k\) 时刻起累积的系统噪声。由于 \(\Delta\hat{\bm{X}}(k)\) 与系统噪声 \(\bm{w}(i-1)\ (i=k+1,\ \cdots,\ l)\) 不相关，所以可得 \(\hat{\bm{X}}(l,\ k)\) 的方差阵为 \[\begin{aligned} \bm{D}_{\hat{X}}(l,\ k)&=\bm{\Phi}_{l,\ k}E\left[\,\bm{\Delta}\bm{X}(k)\bm{\Delta}\bm{X}^{\mathrm{T}}(k)\,\right]\bm{\Phi}_{l,\ k}^{\mathrm{T}} +\sum_{i=k+1}^{l}\bm{\Phi}_{l,\ i}E\left[\,\bm{w}(i-1)\bm{w}(i-1)\,\right]\bm{\Phi}_{l,\ k}^{\mathrm{T}}\\ &=\bm{\Phi}_{l,\ k}\bm{D}_{\hat{X}}(k)\,\bm{\Phi}_{l,\ k}^{\mathrm{T}} +\sum_{i=k+1}^{l}\bm{\Phi}_{l,\ i}\bm{D}_w(i-1)\,\bm{\Phi}_{l,\ i}^{\mathrm{T}} \end{aligned} \tag{4.4.16}\] 上式可以计算得到 \(\hat{\bm{X}}(l,\ k)\) 的方差阵，但不是递推式，下面对式 (4.2.15) 做递推变化，使 \(\bm{D}_{\hat{X}}(l,\ k)\) 可做递推计算。

\(\Delta\hat{\bm{X}}(l,\ k)\) 可表示为 \[\begin{aligned} \Delta\hat{\bm{X}}(l,\ k)&=\bm{\Phi}_{l,\ k}\Delta\hat{\bm{X}}(k)+\sum_{i=k+1}^{l}\bm{\Phi}_{l,\ i}\bm{w}(i-1)\\ &=\bm{\Phi}_{l,\ l-1}\bm{\Phi}_{l-1,\ k}\Delta\hat{\bm{X}}(k)+\bm{\Phi}_{l,\ l}\bm{w}(l-1) +\bm{\Phi}_{l,\ l-1}\sum_{i=k+1}^{l-1}\bm{\Phi}_{l,\ i}\bm{w}(i-1)\\ &=\bm{\Phi}_{l,\ l-1}\left[\bm{\Phi}_{l-1,\ k}\Delta\hat{\bm{X}}(k)+\sum_{i=k+1}^{l-1}\bm{\Phi}_{l,\ i}\bm{w}(i-1)\right] +\bm{\Phi}_{l,\ l}\bm{w}(l-1)\\ &=\bm{\Phi}_{l,\ l-1}\Delta\hat{\bm{X}}(l-1,\ k)+\bm{w}(l-1) \end{aligned} \tag{4.4.17}\] 可见，\(\hat{\bm{X}}(l,\ k)\) 的预测误差可以由 \(\hat{\bm{X}}(l-1,\ k)\) 的误差递推得到。由于 \(\Delta\hat{\bm{X}}(l-1,\ k)\) 与 \(\bm{w}(l-1)\) 不相关，因此 \[\bm{D}_{\hat{X}}(l,\ k)=\bm{\Phi}_{l,\ l-1}\bm{D}_{\hat{X}}(l-1,\ k)\,\bm{\Phi}_{l,\ l-1}^{\mathrm{T}}+\bm{D}_w(l-1) \tag{4.4.18}\] 上式中 \(\bm{D}_{\hat{X}}(l-1,\ k)\) 为 \(\hat{\bm{X}}(l-1,\ k)\) 的方差矩阵。这样就得到了对最优预测 \(\hat{\bm{X}}(l,\ k)\) 的方差阵 \(\bm{D}_{\hat{X}}(l,\ k)\) 的递推公式。

综合以上推导，式 (4.4.13)、(4.4.14)、(4.4.16) 和 (4.4.18) 为 \(k\) 时刻对 \(l\ (l>k)\) 时刻状态的最优预测。

线性离散系统的最优平滑

Kalman 平滑是指观测值 \(\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\bm{Z}(k)\) 对过去时刻的状态 \(\bm{X}(j)\ (j<k)\) 的最优估计。可见，平滑是事后对状态的估计。与滤波相比，平滑使用了更多的观测值来估计状态，所以状态估计的噪声被减小了；从时间序列图示上看，状态估计随时间变化更为平稳，看上去更加“平滑”了，所以把这种估计称为平滑。在实际应用中，平滑能在不同程度上减小估计的方差（MSE），方差减小的程度与系统的结构和随机噪声有关。对于稳定的动态系统，与滤波的方差比较，平滑能将方差减小一半左右。平滑还可以用于某些不具有稳定性的动态系统，并且能够明显地降低方差。

根据 \(k\) 和 \(j\) 具体的变化情况，最优平滑分为如下三类：固定区间平滑、固定滞后平滑、固定点平滑，这三种平滑与滤波的关系如图 4.9 所示。

(1) 固定区间平滑（Fixed-Interval Smoothing）：

利用固定的时间区间 \(\left[\,t_0,\ t_m\,\right]\) 内所得到的观测值 \(\bm{Z}^M=\left[\begin{array}{llll}\bm{Z}(1) & \bm{Z}(2) & \cdots & \bm{Z}(M)\end{array}\right]^{\mathrm{T}}\) 依次估计这个区间中每个时刻的状态 \(\bm{X}(j)\ (j=1,\ 2,\ \cdots,\ M)\)，这种估计方法称为固定区间平滑，平滑的输出为 \(\hat{\bm{X}}_S(j,\ M)\)。如图 4.9 所示的阴影部分，时间 \(t_0\) 和 \(t_M\) 是固定，利用在这个时间段内所有的观测值对状态依次进行估计。

(2) 固定滞后平滑（Fixed-Lag Smoothing）：

时间区间的 \(t_m\) 不固定，如果 \(t_k\) 为当前时间，利用 \(\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\bm{Z}(k)\) 对过去 \(t_j\)（\(t_j=t_k-l\cdot\Delta t\)）时刻的状态 \(\bm{X}(j)\) 进行估计，估计结果记为 \(\hat{\bm{X}}_S(k-l,\ k)\)，其中 \(l\)（正整数）为某个固定的时间滞后值，\(\Delta t\) 为采样间隔。如图 4.9 所示，若从 \(t_0\) 时刻开始连续观测，当 \(t_1\) 时，可得到平滑 \(\hat{\bm{X}}_S(0,\ l)\)，随着时间观测值不断增加，依次得到 \(\hat{\bm{X}}_S(1,\ l+1)\cdots\hat{\bm{X}}_S(k-l,\ k)\)，\(\hat{\bm{X}}_S(k-l+1,\ k+1)\cdots\)。如果固定滞后的时间较短，如 1 2 秒，可以认为 \(\hat{\bm{X}}_S(k-l,\ k)\) 为“准”实时估计。

(3) 固定点平滑（Fixed-Point Smoothing）：

时间区间的 \(t_m\) 不固定，总是对过去某个时间点 \(t_j\) 的 \(\bm{X}(j)\) 的状态进行估计。如图 4.9 所示，利用观测 \(\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\bm{Z}(k)\) 对 \(\bm{X}(j)\) 的估计记为 \(\hat{\bm{X}}_S(j,\ k)\)。同样，利用 \(\bm{Z}(1)\bm{Z}(2)\cdots\bm{Z}(k-1)\bm{Z}(k)\bm{Z}(k+1)\) 对 \(\bm{X}(j)\) 的估计记为 \(\hat{\bm{X}}_S(j,\ k+1)\)。固定点平滑常用于对过程中某一时刻的状态估计，如估计人造卫星轨道在某一个特定时刻的状态。

自 Kalman 滤波被提出以来，各国学者提出了许多基于 Kalman 滤波的平滑方法，以上三类平滑中的每一类都有不同的平滑算法，但是在实际应用中要考虑数值计算的稳定性、计算的复杂程度和计算内存的大小，所以不是所有的方法都实用。下面分别介绍这三类平滑方法的实用算法。

1. 固定区间平滑

首先介绍三通道固定区间平滑。这种平滑方法是最早的基于 Kalman 滤波的平滑方法，此方法比较直观，从中我们可以看到平滑的基本思路。

三通道平滑方法如图 4.10 所示。从左向右的输出是随着时间向前（forward）的滤波输出：\(\hat{\bm{X}}_{[f]}(j)\) 和方差 \(\bm{D}_{[f],\hat{X}}(j)\)，它的递推正如前面介绍的 Kalman 滤波递推公式。从右向左的输出是随着时间点向后（backward）的向后滤波输出 \(\hat{\bm{X}}_{[b]}(j+1)\)。这里的下标 \([f]\) 表示随时间向前递推，\([b]\) 表示时间向后递推。当向前滤波和向后滤波相遇到达同一时间点 \(j\) 时，向后滤波对 \(j\) 时的时间预测为： \[\hat{\bm{X}}_{[b]}(j,\ j+1)=\bm{\Phi}_{j+1,\ j}^{-1}\hat{\bm{X}}_{[b]}(j+1) \tag{4.4.19}\] \[\bm{D}_{[b],\hat{X}}(j,\ j+1)=\bm{\Phi}_{j+1,\ j}^{-1}\bm{D}_{[b],\hat{X}}(j+1)\left(\bm{\Phi}_{j+1,\ j}^{-1}\right)^{\mathrm{T}} +\bm{\Phi}_{j+1,\ j}^{-1}\bm{D}_w(j)\left(\bm{\Phi}_{j+1,\ j}^{-1}\right)^{\mathrm{T}} \tag{4.4.20}\]

如果将 \(\hat{\bm{X}}_{[b]}(j,\ j+1)\) 看作是对 \(\bm{X}(j)\) 状态的观测，\(\hat{\bm{X}}_{[f]}(j)\) 看作是对状态 \(\bm{X}(j)\) 的预测，根据 Kalman 滤波观测对预测的更新公式有 \[\hat{\bm{X}}_S(j)=\hat{\bm{X}}_{[f]}(j)+\bm{K}_j\left[\,\hat{\bm{X}}_{[b]}(j,\ j+1)-\hat{\bm{X}}_{[f]}(j)\,\right] \tag{4.4.21}\] 其中 \[\bm{K}_j=\bm{D}_{[f],\hat{X}}(j)\left[\,\bm{D}_{[f],\hat{X}}(j)+\bm{D}_{[b],\hat{X}}(j,\ j+1)\,\right]^{-1} \tag{4.4.22}\] 方差为 \[\begin{aligned} \bm{D}_{[s],\hat{X}}(j)&=\left[\,\bm{I}-\bm{K}_j\,\right]\bm{D}_{[f],\hat{X}}(j)\\ &=\bm{D}_{[f],\hat{X}}(j)-\bm{D}_{[f],\hat{X}}(j)\left[\,\bm{D}_{[f],\hat{X}}(j)+\bm{D}_{[b],\hat{X}}(j,\ j+1)\,\right]^{-1}\bm{D}_{[f],\hat{X}}(j) \end{aligned} \tag{4.4.23}\] 观察式 (4.4.23)，固定区间平滑的方差比常规滤波的方差减小了 \[\bm{D}_{[f],\hat{X}}(j)\left[\,\bm{D}_{[f],\hat{X}}(j)+\bm{D}_{[b],\hat{X}}(j,\ j+1)\,\right]^{-1}\bm{D}_{[f],\hat{X}}(j)\]

三通道平滑的步骤如下：

(1) 向前滤波：以 \(\hat{\bm{X}}(0)\) 为初始值，用常规滤波计算 \(\hat{\bm{X}}_{[f]}(j)\) 和 \(\bm{D}_{[f],\hat{X}}(j)\)（\(j=0,\ 1,\ \cdots,\ M\)）并存储；

(2) 向后滤波：以 \(\hat{\bm{X}}_{[f]}(M)\) 和 \(\bm{D}_{[f],\hat{X}}(M)\) 作为初始值，依次向后滤波计算 \(\hat{\bm{X}}_{[b]}(j,\ j+1)\) \(\bm{D}_{[b],\hat{X}}(j,\ j+1)\)（\(j=M-1,\ \cdots,\ 0\)）并存储；

(3) 平滑计算：根据式 (4.4.21) 式 (4.4.23) 依次得到 \(\hat{\bm{X}}_S(j)\) 和 \(\bm{D}_{[s],\hat{X}}(j)\)（\(j=M-1,\ \cdots,\ 1,\ 0\)）。

为了说明滤波与平滑的区别，现模拟状态为标量（一维）情况下的动态系统，并对此动态系统在时间段 \([\,t_j,\ t_m\,]\)（\(m=100\)）内进行了观测。用向前滤波、向后滤波和三通道平滑方法对状态进行估计，并计算了各自的方差，结果如图 4.11 中所示。图中的第一和第二幅图分别为向前滤波 \(\hat{\bm{X}}_{[f]}(j)\) 和向后滤波 \(\hat{\bm{X}}_{[b]}(j)\)，从图中看出，\(\hat{\bm{X}}_{[f]}(j)\) 和 \(\hat{\bm{X}}_{[b]}(j)\) 都在真值附近波动，它们的估计方差相差不大。图中第三幅为固定区间平滑值 \(\hat{\bm{X}}_S(j)\)，显然平滑值比滤波值看上去更加“平滑”了，其估计方差也更小了，这是因为平滑值在估计中使用了更多观测值，从而达到了更好的去噪效果。

由于三通道固定区间平滑不仅需要向前滤波，还需要向后滤波并存储滤波结果，在实际应用中受到限制。在这里不加证明地给出应用较好的二通道固定区间平滑公式（Rauch-Tung-Striebel Two-Pass Smoother）。二通道平滑只有向前滤波和平滑两个过程的计算：第一个通道为向前滤波 \(\hat{\bm{X}}_{[f]}(j)\) 和方差 \(\bm{D}_{[f],\hat{X}}(j)\)；第二个通道为向后的平滑计算，直接利用向前滤波的结果，平滑的初始值为 \[\hat{\bm{X}}_S(M)=\hat{\bm{X}}_{[f]}(M) \tag{4.4.24}\] \[\bm{D}_{[s],\hat{X}}(M)=\bm{D}_{[f],\hat{X}}(M) \tag{4.4.25}\] 接下来的递推公式为 \[\hat{\bm{X}}_S(j)=\hat{\bm{X}}_{[f]}(j)+\bm{A}_j\left[\,\hat{\bm{X}}_S(j+1)-\hat{\bm{X}}_{[f]}(j+1,\ j)\,\right] \tag{4.4.26}\] 其中，\(j=M-1,\ \cdots,\ 0\)。由于 \(\hat{\bm{X}}_{[f]}(k)\) 即为 Kalman 滤波 \(\hat{\bm{X}}(k)\)，可将上式表示为 \[\hat{\bm{X}}_S(j)=\hat{\bm{X}}(j)+\bm{A}_j\left[\,\hat{\bm{X}}_S(j+1)-\hat{\bm{X}}(j+1,\ j)\,\right] \tag{4.4.27}\] 其中， \[\bm{A}_j=\bm{D}_{\hat{X}}(j)\bm{\Phi}_{j+1,\ j}^{\mathrm{T}}\bm{D}_{\hat{X}}^{-1}(j+1,\ j) \tag{4.4.28}\] 平滑的方差为 \[\bm{D}_{[s],\hat{X}}(j)=\bm{D}_{\hat{X}}(j)+\bm{A}_j\left[\,\bm{D}_{[s],\hat{X}}(j+1)-\bm{D}_{[f],\hat{X}}(j+1,\ j)\,\right]\bm{A}_j^{\mathrm{T}} \tag{4.4.29}\]

以上的二通道固定区间平滑公式是一组递推公式，计算更简单高效。

2. 固定滞后平滑

如图 4.9 所示，固定滞后平滑与滤波比较，估计值较当前观测值总有一个时间上的滞后。设观测值采样间隔为 \(\Delta t\)，固定滞后值 \(l\)，那么滞后时间为 \(\Delta t_{lag}=l\Delta t\)。用 \(\bm{Z}^k=\left[\,\bm{Z}(1),\ \bm{Z}(2),\ \cdots,\ \bm{Z}(k)\,\right]^{\mathrm{T}}\) 来估计状态 \(\bm{X}(k-l)\)，记此滤波估计值为 \(\hat{\bm{X}}_S(k-l,\ k)\)。如果滞后时间较短，可以认为是“准”实时估计。与固定区间平滑一样，固定滞后平滑的方法也有多种，有些平滑方法在数学上成立，但在实际应用中由于数值计算不稳定而限制了其应用，这里介绍数值计算较稳定的方法——Biswas-Mahalanabis 固定区间滞后平滑。

现将状态扩展为 \[\underset{(l+1)n\times 1}{\bm{X}_{BM}(k)}=\begin{bmatrix}\bm{X}(k)\\ \bm{X}(k-1)\\ \bm{X}(k-2)\\ \vdots\\ \bm{X}(k-l)\end{bmatrix},\ (k=l,\ l+1,\ \cdots) \tag{4.4.30}\] 其中，\(\bm{X}_{BM}(k)\) 为 \((l+1)\times n\) 元素的列向量。从时刻 \(t_l\) 到时刻 \(t_k\) 的扩展状态如图 4.12 所示。

扩展后的状态方程为 \(\bm{X}_{BM}(k)=\left[\begin{array}{c}\bm{X}(k)\\ \bm{X}(k-1)\\ \bm{X}(k-2)\\ \vdots\\ \bm{X}(k-l+1)\\ \bm{X}(k-l)\end{array}\right]\) \[=\begin{bmatrix} \bm{\Phi}_{k,\ k-1} & 0 & 0 & \cdots & 0 & 0\\ \bm{I} & 0 & 0 & \cdots & 0 & 0\\ 0 & \bm{I} & 0 & \cdots & 0 & 0\\ 0 & 0 & \bm{I} & \cdots & 0 & 0\\ \vdots & \vdots & \vdots & & \vdots & \vdots\\ 0 & 0 & 0 & \cdots & \bm{I} & 0 \end{bmatrix} \begin{bmatrix}\bm{X}(k-1)\\ \bm{X}(k-2)\\ \bm{X}(k-3)\\ \vdots\\ \bm{X}(k-l)\\ \bm{X}(k-l-1)\end{bmatrix} +\begin{bmatrix}\bm{w}(k-1)\\ \bm{0}\\ \bm{0}\\ \vdots\\ \bm{0}\\ \bm{0}\end{bmatrix} \tag{4.4.31}\] 状态噪声方差为 \[\bm{D}_{w,\ BM}(k-1)=\begin{bmatrix} \bm{D}_w(k-1) & \bm{0} & \bm{0} & \cdots & \bm{0}\\ \bm{0} & \bm{0} & \bm{0} & \cdots & \bm{0}\\ \bm{0} & \bm{0} & \bm{0} & \cdots & \bm{0}\\ \vdots & \vdots & \vdots & \ddots & \vdots\\ \bm{0} & \bm{0} & \bm{0} & \cdots & \bm{0} \end{bmatrix} \tag{4.4.32}\] 观测方程为 \[\bm{Z}(k)=\bm{H}_{k,\ BM}\bm{X}_{BM}(k)+\bm{\Delta}(k) \tag{4.4.33}\] 其中 \[\bm{H}_{k,\ BM}=\begin{bmatrix}\bm{H}_k & \bm{0} & \bm{0} & \cdots & \bm{0}\end{bmatrix} \tag{4.4.34}\] 观测值方差为 \(\bm{D}_{\Delta}(k)\)。

基于以上的线性离散系统模型，根据 Kalman 滤波递推公式，可以求得 \(\hat{\bm{X}}_{BM}(k)\) \[\hat{\bm{X}}_{BM}(k)=\begin{bmatrix}\hat{\bm{X}}(k)\\ \hat{\bm{X}}_S(k-1,\ k)\\ \hat{\bm{X}}_S(k-2,\ k)\\ \vdots\\ \hat{\bm{X}}_S(k-l,\ k)\end{bmatrix} \tag{4.4.35}\] \(\hat{\bm{X}}_{BM}(k)\) 中的第一个子向量 \(\hat{\bm{X}}(k)\) 即为 \(\bm{X}(k)\) 的滤波，接下来依次为时间滞后为 \(\Delta t\) 的平滑值 \(\hat{\bm{X}}_S(k-1,\ k)\)、时间滞后为 \(2\Delta t\) 的平滑值 \(\hat{\bm{X}}_S(k-2,\ k)\)。依次类推，\(\hat{\bm{X}}_S(k-l,\ k)\) 为时间滞后 \(l\times\Delta t\) 的平滑。为了求时间滞后 \(l\times\Delta t\) 的平滑，至少有观测值 \(\bm{Z}_{l,\ BM}=\left[\begin{array}{lllll}\bm{Z}_1 & \bm{Z}_2 & \cdots & \bm{Z}_{l-1} & \bm{Z}_l\end{array}\right]\) 后，才能得到第一个时间滞后为 \(l\times\Delta t\) 的状态平滑 \(\hat{\bm{X}}_S(0,\ l)\)，在有观测值 \(\bm{Z}(l+1)\) 后，可得到平滑 \(\hat{\bm{X}}_S(1,\ l+1)\)，依次类推。

3. 固定点平滑

这里不加推导地给出用观测值 \(\bm{Z}^k=\left[\,\bm{Z}^{\mathrm{T}}(1),\ \bm{Z}^{\mathrm{T}}(2),\ \cdots,\ \bm{Z}^{\mathrm{T}}(k)\,\right]^{\mathrm{T}}\) 对 \(j\ (j<k)\) 时刻的状态 \(\bm{X}(j)\) 进行估计的递推公式 [Grewal M S, 1988]。设固定点平滑为 \(\hat{\bm{X}}_S(j,\ k)\)，那么 \[\hat{\bm{X}}_S(j,\ k)=\hat{\bm{X}}_S(j,\ k-1)+\bm{B}_k\bm{K}_k\left(\bm{Z}(k)-\bm{H}_k\hat{\bm{X}}(k,\ k-1)\right) \tag{4.4.36}\] 其中 \[\bm{B}_k=\bm{B}_{k-1}\bm{D}_{\hat{X}}(k-1)\bm{\Phi}_{k,\ k-1}^{\mathrm{T}}\bm{D}_{\hat{X}}^{-1}(k,\ k-1) \tag{4.4.37}\] \[\bm{D}_{\hat{X}_S}(j,\ k)=\bm{D}_{\hat{X}_S}(j,\ k-1) +\bm{B}_k\left[\,\bm{D}_{\hat{X}}(k)-\bm{D}_{\hat{X}}(k,\ k-1)\,\right]\bm{B}_k^{\mathrm{T}} \tag{4.4.38}\] 其中，\(\bm{K}_k\) 为 Kalman 滤波中的增益矩阵。在平滑的初始时刻，也就是 \(k=j\) 时，\(\hat{\bm{X}}_S(j,\ j)=\hat{\bm{X}}(j)\)；当 \(k=j+1\) 时，就开始了对 \(\bm{X}(j)\) 的平滑。利用上面的递推公式，可以得到 \(\hat{\bm{X}}_S(j,\ j+1)\)，\(\hat{\bm{X}}_S(j,\ j+2)\cdots\hat{\bm{X}}_S(j,\ k)\)，\(\hat{\bm{X}}_S(j,\ k+1)\cdots\)。

线性连续系统的 Kalman 滤波

线性连续系统的 Kalman 滤波基础方程

有时为了理论研究等目的，还需要得到连续系统模型下的结果，如可以通过连续型 Kalman 滤波结果来分析滤波结果是否“稳定”。本节将推导和给出连续系统模型下 Kalman 滤波基础方程，连续性的 Kalman 滤波也称为Kalman-Bucy 滤波。

当时间变化 \(\Delta t\) 很小时，微分方程可以用差分方程来表示，所以连续系统也可以看作是当采样周期趋于零的离散系统的极限，因此可以利用差分方法建立离散系统模型与连续系统模型之间的关系，然后利用离散系统的 Kalman 滤波来推导连续系统 Kalman 滤波。

设线性连续系统模型为 \[\begin{aligned} \dot{\bm{X}}(t)&=\bm{A}(t)\bm{X}(t)+\bm{C}(t)\bm{e}(t)\\ \bm{Z}(t)&=\bm{H}(t)\bm{X}(t)+\bm{\Delta}(t) \end{aligned} \tag{4.5.1}\] 系统噪声 \(\bm{e}(t)\) 和观测噪声 \(\bm{\Delta}(t)\) 互不相关，且均为零均值白噪声过程，并已知 \[\begin{gathered} E\left[\,\bm{e}(t)\bm{e}(\tau)\,\right]=\bm{D}_e(t)\delta(t-\tau)\\ E\left[\,\bm{\Delta}(t)\bm{\Delta}(\tau)\,\right]=\bm{D}_{\Delta}(t)\delta(t-\tau)\\ E\left[\,\bm{e}(t)\bm{\Delta}(\tau)\,\right]=0\\ E\left[\,\hat{\bm{X}}(t_0)\,\right]=E\left[\,\bm{X}(t_0)\,\right]\\ \bm{D}_{\hat{X}}(t_0)=\mathrm{Var}\left[\,\bm{X}(t_0)\,\right] \end{gathered}\] 根据 3.2.1 节，状态转移矩阵满足 \[\dot{\bm{\Phi}}(t+\Delta t,\ t)=\bm{A}(t+\Delta t)\bm{\Phi}(t+\Delta t,\ t) \tag{4.5.2}\] 当 \(\Delta t\rightarrow 0\)，式 (4.5.2) 可表达为 \[\frac{\bm{\Phi}(t+\Delta t,\ t)-\bm{\Phi}(t,\ t)}{\Delta t}=\bm{A}(t+\Delta t)\bm{\Phi}(t+\Delta t,\ t) \tag{4.5.3}\] 由于 \(\Delta t\rightarrow 0\)，得到 \[\bm{\Phi}(t+\Delta t,\ t)=\bm{I}+\bm{A}(t)\Delta t \tag{4.5.4}\] 令 \(t_{k+1}=t+\Delta t\)，\(t_k=t\)，那么 \[\bm{\Phi}(k+1,\ k)=\bm{I}+\bm{A}(t_k)\Delta t \tag{4.5.5}\] 根据 3.3.2 节有 \[\begin{aligned} \bm{D}_w(k)&=\bm{C}(t_k)\bm{D}_e(t_k)\bm{C}^{\mathrm{T}}(t_k)\Delta t\\ \bm{D}_{\Delta}(k)&=\bm{D}_{\Delta}(t_k)/\Delta t \end{aligned} \tag{4.5.6}\] 因此时间预测的方差为 \[\bm{D}_{\hat{X}}(k+1,\ k)=\bm{\Phi}(k+1,\ k)\bm{D}_{\hat{X}}(k)\bm{\Phi}^{\mathrm{T}}(k,\ k-1)+\bm{C}(t_k)\bm{D}_e(t_k)\bm{C}^{\mathrm{T}}(t_k)\Delta t \tag{4.5.7}\] 将 \(\bm{\Phi}(k+1,\ k)=\bm{I}+\bm{A}(t_k)\Delta t\) 代入式 (4.5.7)，得到 \[\begin{aligned} \bm{D}_{\hat{X}}(k+1,\ k)=&\left[\,\bm{I}+\bm{A}(t_k)\Delta t\,\right]\bm{D}_{\hat{X}}(k)\left[\,\bm{I}+\bm{A}(t_k)\Delta t\,\right]^{\mathrm{T}} +\bm{C}(t_k)\bm{D}_e(t_k)\bm{C}^{\mathrm{T}}(t_k)\Delta t\\ =&\ \bm{D}_{\hat{X}}(k)+\left[\,\bm{A}(t_k)\bm{D}_{\hat{X}}(k)+\bm{D}_{\hat{X}}(k)\bm{A}^{\mathrm{T}}(t_k) +\bm{C}(t_k)\bm{D}_e(t_k)\bm{C}^{\mathrm{T}}(t_k)\right.\\ &\quad\left.{}+\bm{A}(t_k)\bm{D}_{\hat{X}}(k)\bm{A}^{\mathrm{T}}(t_k)\Delta t\,\right]\Delta t+O(\Delta t^2) \end{aligned} \tag{4.5.8}\] 其中 \(\sigma(\Delta t^2)\) 为 \(\Delta t\) 的二阶项。从上式看出，如果 \(\Delta t\rightarrow 0\)，有 \[\bm{D}_{\hat{X}}(k+1,\ k)\rightarrow\bm{D}_{\hat{X}}(k) \tag{4.5.9}\] 将 \[\bm{D}_{\hat{X}}(k)=\left[\,\bm{I}-\bm{K}_k\bm{H}_k\,\right]\bm{D}_{\hat{X}}(k,\ k-1) \tag{4.5.10}\] 代入式 (4.5.8)，两边同时减去 \(\bm{D}_{\hat{X}}(k,\ k-1)\) 后除以 \(\Delta t\) \[\begin{aligned} &\frac{\bm{D}_{\hat{X}}(k+1,\ k)-\bm{D}_{\hat{X}}(k,\ k-1)}{\Delta t} =\bm{A}(t_k)\bm{D}_{\hat{X}}(k,\ k-1)+\bm{D}_{\hat{X}}(k,\ k-1)\bm{A}^{\mathrm{T}}(t_k)\\ &\quad+\bm{C}(t_k)\bm{D}_e(t_k)\bm{C}^{\mathrm{T}}(t_k) -\frac{\bm{K}_k\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)}{\Delta t}\\ &\quad-\bm{A}(t_k)\bm{K}_k\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\bm{A}^{\mathrm{T}}(t_k)\Delta t+O(\Delta t^2) \end{aligned} \tag{4.5.11}\] 其中 \(\dfrac{\bm{K}_k}{\Delta t}\) 为 \[\frac{\bm{K}_k}{\Delta t}=\bm{D}_{\hat{X}}(k,\ k-1)\bm{H}_k^{\mathrm{T}} \left[\,\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1)\times\bm{H}_k^{\mathrm{T}}\Delta t+\bm{D}_{\Delta}(t_k)\,\right]^{-1} \tag{4.5.12}\] 当 \(\Delta t\rightarrow 0\)，有 \[\lim_{\Delta t\rightarrow 0}\left[\frac{\bm{K}_k}{\Delta t}\right]=\bm{D}_{\hat{X}}(k,\ k-1)\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(t) \tag{4.5.13}\]

将式 (4.5.12) 代入式 (4.5.11)，并考虑 \(\Delta t\rightarrow 0\)，得到 \[\begin{aligned} \frac{\bm{D}_{\hat{X}}(k+1,\ k)-\bm{D}_{\hat{X}}(k,\ k-1)}{\Delta t} =&\ \bm{A}(t)\bm{D}_{\hat{X}}(k,\ k-1)+\bm{D}_{\hat{X}}(k,\ k-1)\bm{A}^{\mathrm{T}}(t)\\ &+\bm{C}(t)\bm{D}_e(t)\bm{C}^{\mathrm{T}}(t)\\ &-\bm{D}_{\hat{X}}(k,\ k-1)\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(t)\bm{H}_k\bm{D}_{\hat{X}}(k,\ k-1) \end{aligned} \tag{4.5.14}\] 考虑式 (4.5.9)，有 \[\bm{D}_{\hat{X}}(k,\ k-1)\rightarrow\bm{D}_{\hat{X}}(k-1) \tag{4.5.15}\] 所以式 (4.5.14) 为 \[\begin{aligned} \frac{\bm{D}_{\hat{X}}(k+1)-\bm{D}_{\hat{X}}(k)}{\Delta t} =&\ \bm{A}(t_k)\bm{D}_{\hat{X}}(k)+\bm{D}_{\hat{X}}(k)\bm{A}^{\mathrm{T}}(t_k)\\ &+\bm{C}(t_k)\bm{D}_e(t_k)\bm{C}^{\mathrm{T}}(t_k) -\bm{D}_{\hat{X}}(k)\bm{H}_k^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(t_k)\bm{H}_k\bm{D}_{\hat{X}}(k) \end{aligned} \tag{4.5.16}\] 由于 \(t_{k+1}=t+\Delta t\)，\(t_k=t\)，上式为 \[\begin{aligned} \dot{\bm{D}}_{\hat{X}}(t_k)=&\ \bm{A}(t_k)\bm{D}_{\hat{X}}(t_k)+\bm{D}_{\hat{X}}(t_k)\bm{A}^{\mathrm{T}}(t_k) +\bm{C}(t_k)\bm{D}_e(t_k)\bm{C}^{\mathrm{T}}(t_k)\\ &-\bm{D}_{\hat{X}}(t_k)\bm{H}^{\mathrm{T}}(t_k)\bm{D}_{\Delta}^{-1}(t_k)\bm{H}(t_k)\bm{D}_{\hat{X}}(t_k) \end{aligned} \tag{4.5.17}\] 去掉时间下标 \(k\)，并设 \[\bm{K}(t)=\bm{D}_{\hat{X}}(t)\bm{H}^{\mathrm{T}}(t)\bm{D}_{\Delta}^{-1}(t) \tag{4.5.18}\] 式 (4.5.17) 为 \[\dot{\bm{D}}_{\hat{X}}(t)=\bm{A}(t)\bm{D}_{\hat{X}}(t)+\bm{D}_{\hat{X}}(t)\bm{A}^{\mathrm{T}}(t) +\bm{C}(t)\bm{D}_e(t)\bm{C}^{\mathrm{T}}(t)-\bm{K}(t)\bm{D}_{\Delta}(t)\bm{K}^{\mathrm{T}}(t) \tag{4.5.19}\] \(\bm{K}(t)\) 就是连续 Kalman 滤波的增益矩阵，式 (4.5.19) 也称为矩阵黎卡提微分方程。显然，矩阵黎卡提微分方程是一个非线性微分方程。

下面推导连续系统的状态的 Kalman 滤波 \(\hat{\bm{X}}(t)\)。

将 \(\bm{\Phi}(t+\Delta t,\ t)\approx\bm{I}_n+\bm{A}(t)\Delta t\) 代入滤波方程，得 \[\begin{aligned} \hat{\bm{X}}(t+\Delta t)=&\left[\,\bm{I}+\bm{A}(t)\Delta t\,\right]\hat{\bm{X}}(t)\\ &+\bm{K}(t+\Delta t)\left\{\bm{Z}(t+\Delta t) -\bm{H}(t+\Delta t)\left[\,\bm{I}+\bm{A}(t)\Delta t\,\right]\hat{\bm{X}}(t)\right\} \end{aligned} \tag{4.5.20}\] 将上式两端同时减 \(\hat{\bm{X}}(t)\) 并除以 \(\Delta t\)，得 \[\begin{aligned} \frac{\hat{\bm{X}}(t+\Delta t)-\hat{\bm{X}}(t)}{\Delta t} =&\ \bm{A}(t)\hat{\bm{X}}(t)+\frac{\bm{K}(t+\Delta t)}{\Delta t}\left[\,\bm{Z}(t+\Delta t)\right.\\ &\left.{}-\bm{H}(t+\Delta t)\left[\,\bm{I}+\bm{A}(t)\Delta t\,\right]\hat{\bm{X}}(t)\,\right] \end{aligned} \tag{4.5.21}\] 当 \(\Delta t\rightarrow 0\)，对上式取极限并考虑式 (4.5.13) 得到 \[\dot{\hat{\bm{X}}}(t)=\bm{A}(t)\hat{\bm{X}}(t)+\bm{D}_{\hat{X}}(t)\bm{H}^{\mathrm{T}}(t)\bm{D}_{\Delta}^{-1}(t)\left[\,\bm{Z}(t)-\bm{H}(t)\hat{\bm{X}}(t)\,\right] \tag{4.5.22}\] 由于 \(\bm{K}(t)=\bm{D}_{\hat{X}}(t)\bm{H}^{\mathrm{T}}(t)\bm{D}_{\Delta}^{-1}(t)\)，式 (4.5.22) 为 \[\dot{\hat{\bm{X}}}(t)=\bm{A}(t)\hat{\bm{X}}(t)+\bm{K}(t)\left[\,\bm{Z}(t)-\bm{H}(t)\hat{\bm{X}}(t)\,\right] \tag{4.5.23}\] 综合上述推导，便得到了连续 Kalman 滤波基本方程。现在将线性连续系统的 Kalman 滤波方程汇总，见表 4.3。

线性连续系统的 Kalman 滤波方程是一组一阶微分方程，其初始条件为 \(E\left[\,\bm{X}(t_0)\,\right]=E\left[\,\hat{\bm{X}}(t_0)\,\right]\) 和 \(\mathrm{Var}\left[\,\bm{X}(t_0)\,\right]=\bm{D}_{\hat{X}}(t_0)\)。对以上的结果，做以下说明：

(1) 式 (4.5.23) 说明连续 Kalman 滤波是观测值新息 \(\left[\,\bm{Z}(t)-\bm{H}(t)\hat{\bm{X}}(t)\,\right]\) 作用下的一个随机线性系统。

(2) 滤波方差 \(\bm{D}_{\hat{X}}(t)\) 可由矩阵黎卡提微分方程 (4.5.19) 离线解出。方程中的前两项 \(\bm{A}(t)\bm{D}_{\hat{X}}(t)+\bm{D}_{\hat{X}}(t)\bm{A}^{\mathrm{T}}(t)\) 考虑的是式 (4.5.1) 中动态方程 \(\bm{A}(t)\bm{X}(t)\) 部分的误差，即系统在无外部输入时的误差；第三项 \(\bm{C}(t)\bm{D}_e(t)\bm{C}^{\mathrm{T}}(t)\) 考虑的是系统噪声的方差，它使 \(\bm{D}_{\hat{X}}(t)\) 增大；最后一项 \(\bm{D}_{\hat{X}}(t)\bm{H}^{\mathrm{T}}(t)\bm{D}_{\Delta}^{-1}(t)\bm{H}(t)\bm{D}_{\hat{X}}(t)\) 是加入观测值后使滤波不确定性减小的部分。如果滤波结果是稳态的，初始方差偏差较大，那么随着时间的递推，滤波方差逐渐减小并趋于稳定值，观测值的方差 \(\bm{D}_{\Delta}(t)\) 越小，\(\bm{D}_{\hat{X}}(t)\) 下降的速度就越快；反之，\(\bm{D}_{\Delta}(t)\) 越大，\(\bm{D}_{\hat{X}}(t)\) 下降的速度就越慢。

(3) 式 (4.5.23) 可以改写为 \[\dot{\hat{\bm{X}}}(t)=\left[\,\bm{A}(t)-\bm{K}(t)\bm{H}(t)\,\right]\hat{\bm{X}}(t)+\bm{K}(t)\bm{Z}(t) \tag{4.5.24}\] 设上式的解为 \[\hat{\bm{X}}(t)=\bm{\Psi}(t,\ t_0)\hat{\bm{X}}(t_0)+\int_{t_0}^{t}\bm{\Psi}(t,\ \tau)\bm{K}(\tau)\bm{Z}(\tau)\,\mathrm{d}\tau \tag{4.5.25}\] 其中 \(\bm{\Psi}(t,\ t_0)\) 是微分方程 (4.5.24) 的状态转移矩阵。当 \(\hat{\bm{X}}(t_0)=0\) 时，有 \[\hat{\bm{X}}(t)=\int_{t_0}^{t}\bm{\Psi}(t,\ \tau)\bm{K}(\tau)\bm{Z}(\tau)\,\mathrm{d}\tau \tag{4.5.26}\] 也就是说，当 \(\hat{\bm{X}}(t_0)=0\) 时，滤波 \(\hat{\bm{X}}(t)\) 可以表示为观测值的一个特殊线性变换。当无观测值可用时，\(\bm{K}(t)\rightarrow 0\)，式 (4.5.23) 为 \[\dot{\hat{\bm{X}}}(t)=\bm{A}(t)\hat{\bm{X}}(t) \tag{4.5.27}\] 式 (4.5.19) 为 \[\dot{\bm{D}}_{\hat{X}}(t)=\bm{A}(t)\bm{D}_{\hat{X}}(t)+\bm{D}_{\hat{X}}(t)\bm{A}^{\mathrm{T}}(t)+\bm{C}(t)\bm{D}_e(t)\bm{C}^{\mathrm{T}}(t) \tag{4.5.28}\] 这时的连续 Kalman 滤波即为线性连续系统的最小方差估计。

算例分析

例 4.4有观测方程和状态方程 \[\begin{aligned} \dot{X}(t)&=e(t) & \mathrm{Cov}\left[\,e(t)\,\right]&=D_e\\ Z(t)&=X(t)+\Delta(t) & \mathrm{Cov}\left[\,\Delta(t)\,\right]&=D_{\Delta} \end{aligned} \tag{4.5.29}\] 和初值 \(X(0)\)、\(D_{\hat{X}}(0)\)。分析当 \(t\) 增加时 \(D_{\hat{X}}(t)\) 的变化。

解：在此问题中 \(\bm{A}(t)=0\)，\(\bm{C}(t)=1\)，\(\bm{H}(t)=1\)，根据连续型 Kalman 滤波基本公式 \[\dot{D}_{\hat{X}}(t)=D_e-\frac{D_{\hat{X}}^2(t)}{D_{\Delta}} \tag{4.5.30}\] 解微分方程 \[\int\frac{\mathrm{d}D_{\hat{X}}(t)}{\alpha^2-D_{\hat{X}}^2(t)}=\frac{1}{2\alpha}\ln\left(\frac{\alpha+D_{\hat{X}}(t)}{\alpha-D_{\hat{X}}(t)}\right) \tag{4.5.31}\] 解此微分方程 \[D_{\hat{X}}(t)=\alpha\left[\frac{D_{\hat{X}}(0)\cosh(\beta t)+\alpha\sinh(\beta t)} {D_{\hat{X}}(0)\sinh(\beta t)+\alpha\cosh(\beta t)}\right] \tag{4.5.32}\] 其中 \[\alpha=\sqrt{D_{\Delta}D_e}\ ,\quad \beta=\sqrt{\frac{D_e}{D_{\Delta}}} \tag{4.5.33}\] 从式 (4.5.32) 分析得到：

(1) 若 \(D_{\hat{X}}(0)=0\)，则 \(\lim\limits_{t\rightarrow\infty}D_{\hat{X}}(t)=\alpha\tanh(\beta t)=\alpha\)；

(2) 若 \(D_{\hat{X}}(0)=\alpha\)，则 \(D_{\hat{X}}(t)=\alpha\)；

(3) 若 \(D_{\hat{X}}(0)=\infty\)，则 \(\lim\limits_{t\rightarrow\infty}D_{\hat{X}}(t)=\alpha\coth(\beta t)=\alpha\)。

\(D_{\hat{X}}(t)\) 随 \(t\) 的变化如图 4.13 所示。

可以看到，在此问题中，无论初始的状态方差为何值，经过一段时间的滤波，\(D_{\hat{X}}(t)\) 逐渐趋于一个稳定值，\(D_{\hat{X}}(\infty)\) 为一常数，即 \(\dot{D}_{\hat{X}}(\infty)=0\)。

在定常系统中，\(\bm{A}(t)\)，\(\bm{C}(t)\)，\(\bm{H}(t)\)，\(\bm{D}_e(t)\) 和 \(\bm{D}_{\Delta}\) 均为常量，如果滤波可以达到稳态，就有 \[\bm{A}\bm{D}_{\hat{X}}(\infty)+\bm{D}_{\hat{X}}(\infty)\bm{A}^{\mathrm{T}}+\bm{C}\bm{D}_e\bm{C}^{\mathrm{T}} -\bm{D}_{\hat{X}}(\infty)\bm{H}^{\mathrm{T}}\bm{D}_{\Delta}\bm{H}\bm{D}_{\hat{X}}(\infty)=0 \tag{4.5.34}\] 式 (4.5.34) 称为代数黎卡提方程。根据式 (4.5.18)，这时的增益矩阵为 \[\bm{K}(\infty)=\bm{D}_{\hat{X}}(\infty)\bm{H}^{\mathrm{T}}\bm{D}_{\Delta}^{-1} \tag{4.5.35}\] 例 4.4 的系统随着 \(t\) 增加，\(\bm{D}_{\hat{X}}(t)\) 和 \(\bm{K}(t)\) 趋于常数矩阵，这样的 Kalman 滤波器是稳态的。如若已知例 4.4 的滤波是稳态的，就可以提前根据式 (4.5.34) 和式 (4.5.35) 计算 \(\bm{D}_{\hat{X}}(\infty)\) 和 \(\bm{K}(\infty)\) \[\bm{D}_{\hat{X}}(\infty)=\sqrt{D_{\Delta}D_e} \tag{4.5.36}\] \[\bm{K}(\infty)=\sqrt{D_{\Delta}D_e}\,\bm{H}^{\mathrm{T}}\bm{D}_{\Delta}^{-1} \tag{4.5.37}\] 这样就避免了实时计算增益矩阵和方差矩阵，从而大大减少在线计算的负担，便于工程应用。

例 4.5有动态方程和观测方程： \[\begin{aligned} \dot{\bm{X}}(t)&=\begin{bmatrix}0 & 1\\ 0 & 0\end{bmatrix}\bm{X}(t)+\begin{bmatrix}0\\ 1\end{bmatrix}e(t)\\[4pt] \bm{Z}(t)&=\left[\begin{array}{ll}1 & 0\end{array}\right]\bm{X}(t)+\Delta(t) \end{aligned} \tag{4.5.38}\] \(e(t)\) 和 \(\Delta(t)\) 都是一维的零均值噪声，方差和初值分别为： \[\begin{gathered} \mathrm{Cov}\left[\,e(t)e(\tau)\,\right]=4\delta(t-\tau)\ ,\quad \mathrm{Cov}\left[\,\Delta(t)\Delta(\tau)\,\right]=2\delta(t-\tau)\\ E\left[\,\bm{X}(t_0)\,\right]=\bm{0}\ ,\quad \mathrm{Var}\left[\,\bm{X}(t_0)\,\right]=\begin{bmatrix}1 & 0\\ 0 & 0\end{bmatrix} \end{gathered} \tag{4.5.39}\] 且 \(e(t)\) 与 \(\Delta(t)\) 互不相关，试求 Kalman 滤波方程。

解：已知 \[\begin{gathered} \bm{A}(t)=\begin{bmatrix}0 & 1\\ 0 & 0\end{bmatrix}\ ,\quad \bm{C}(t)=\begin{bmatrix}0\\ 1\end{bmatrix}\ ,\quad \bm{H}(t)=\left[\begin{array}{ll}1 & 0\end{array}\right]\\[4pt] D_e(t)=4\ ,\quad D_{\Delta}(t)=2\ ,\quad \bm{X}(t_0)=\bm{0}\ ,\quad \mathrm{Var}\left[\,\bm{X}(t_0)\,\right]=\begin{bmatrix}1 & 0\\ 0 & 0\end{bmatrix} \end{gathered} \tag{4.5.40}\] 将以上参数代入线性连续系统的 Kalman 滤波公式，得： \[\dot{\hat{\bm{X}}}(t)=\begin{bmatrix}0 & 1\\ 0 & 0\end{bmatrix}\hat{\bm{X}}(t) +\bm{K}(t)\left[\,\bm{Z}(t)-\left[\begin{array}{ll}1 & 0\end{array}\right]\hat{\bm{X}}(t)\,\right] \tag{4.5.41}\] \[\bm{K}(t)=\frac{1}{2}\bm{D}_{\hat{X}}(t)\begin{bmatrix}0\\ 1\end{bmatrix} \tag{4.5.42}\] \[\begin{aligned} \dot{\bm{D}}_{\hat{X}}(t)=&\begin{bmatrix}0 & 1\\ 0 & 0\end{bmatrix}\bm{D}_{\hat{X}}(t) +\bm{D}_{\hat{X}}(t)\begin{bmatrix}0 & 0\\ 1 & 0\end{bmatrix} +4\begin{bmatrix}0\\ 1\end{bmatrix}\left[\begin{array}{ll}0 & 1\end{array}\right]\\ &-\frac{1}{2}\bm{D}_{\hat{X}}(t)\begin{bmatrix}0\\ 1\end{bmatrix} \left[\begin{array}{ll}1 & 0\end{array}\right]\bm{D}_{\hat{X}}(t) \end{aligned} \tag{4.5.43}\]

假设 \[\bm{D}_{\hat{X}}(t)=\begin{bmatrix}\sigma_1^2(t) & \sigma_{12}(t)\\ \sigma_{21}(t) & \sigma_2^2(t)\end{bmatrix}\ ,\quad \dot{\bm{D}}_{\hat{X}}(t)=\begin{bmatrix}\dot{\sigma}_1^2(t) & \dot{\sigma}_{12}(t)\\ \dot{\sigma}_{21}(t) & \dot{\sigma}_2^2(t)\end{bmatrix}\] 并将式 (4.5.43) 计算的结果代入上式： \[\begin{bmatrix}\dot{\sigma}_1^2(t) & \dot{\sigma}_{12}(t)\\ \dot{\sigma}_{21}(t) & \dot{\sigma}_2^2(t)\end{bmatrix} =\begin{bmatrix}2\sigma_{12}(t)-\frac{1}{2}\left[\,\sigma_1^2(t)\,\right]^2 & \sigma_2^2(t)-\frac{1}{2}\sigma_1^2(t)\sigma_{12}(t)\\[6pt] \sigma_2^2(t)-\frac{1}{2}\sigma_1^2(t)\sigma_{12}(t) & 4-\frac{1}{2}\left[\,\sigma_{12}(t)\,\right]^2\end{bmatrix} \tag{4.5.44}\] 将上式展开，有 \[\begin{aligned} \dot{\sigma}_1^2(t)&=2\sigma_{12}(t)-\frac{1}{2}\left[\,\sigma_1^2(t)\,\right]^2\ , &\sigma_1^2(0)&=1\\[4pt] \dot{\sigma}_{12}(t)&=\sigma_2^2(t)-\frac{1}{2}\sigma_1^2(t)\sigma_{12}(t)=\dot{\sigma}_{21}(t)\ , &\sigma_{12}(0)&=0\\[4pt] \dot{\sigma}_2^2(t)&=4-\frac{1}{2}\left[\,\sigma_{12}(t)\,\right]^2\ , &\sigma_2^2(0)&=0 \end{aligned} \tag{4.5.45}\] 解上述微分方程组，求得方差 \(\bm{D}_{\hat{X}}(t)\) 后，得增益矩阵为： \[\bm{K}(t)=\frac{1}{2}\begin{bmatrix}\sigma_1^2(t) & \sigma_{12}(t)\\ \sigma_{21}(t) & \sigma_2^2(t)\end{bmatrix}\begin{bmatrix}1\\ 0\end{bmatrix} =\begin{bmatrix}\sigma_1^2(t)/2\\ \sigma_{12}(t)/2\end{bmatrix} \tag{4.5.46}\] 于是，滤波方程为 \[\dot{\hat{\bm{X}}}(t)=\begin{bmatrix}0 & 1\\ 0 & 0\end{bmatrix}\hat{\bm{X}}(t) +\begin{bmatrix}\sigma_1^2(t)/2\\ \sigma_{12}(t)/2\end{bmatrix} \left[\,\bm{Z}(t)-\left[\begin{array}{ll}1 & 0\end{array}\right]\hat{\bm{X}}(t)\,\right] \tag{4.5.47}\] 由本例可以看出，线性连续系统 Kalman 滤波方程的求解问题归结为矩阵黎卡提微分方程的求解问题。即使是一个简单的定常系统，求解矩阵黎卡提微分方程的解析解都很困难，所以一般需要借助计算机求数值解。

Kalman 滤波的稳定性

对任何控制系统而言，稳定性是系统正常工作的前提，本节介绍的是 Kalman 滤波的稳定性概念和判断滤波系统稳定性的条件。

随机线性系统的可控性和可测性

随机线性系统的可控性与我们在第 3 章中介绍的可控性不同。随机线性系统的可控性是指系统的随机噪声影响系统状态的能力，第 3 章中介绍的可控性是指系统的确定性输入影响系统状态的能力。设随机线性离散系统为 \[\bm{X}(k)=\bm{\Phi}_{k,\ k-1}\bm{X}(k-1)+\bm{\varGamma}_{k-1}\bm{w}(k-1) \tag{4.6.1}\] \[\bm{Z}(k)=\bm{H}_k\bm{X}(k)+\bm{\Delta}(k) \tag{4.6.2}\] 随机模型为 \[\begin{gathered} E\left[\,\bm{w}(k)\,\right]=\bm{0}\ ,\quad E\left[\,\bm{w}(k)\bm{w}^{\mathrm{T}}(j)\,\right]=\bm{D}_w(k)\delta(k-j)\\ E\left[\,\bm{\Delta}(k)\,\right]=\bm{0}\ ,\quad E\left[\,\bm{\Delta}(k)\bm{\Delta}^{\mathrm{T}}(j)\,\right]=\bm{D}_{\Delta}(k)\delta(k-j)\\ E\left[\,\bm{w}(k)\bm{\Delta}(j)\,\right]=0 \end{gathered} \tag{4.6.3}\] 其中 \(\bm{D}_w(k)\) 和 \(\bm{D}_{\Delta}(k)\) 均为正定矩阵。对于连续 Kalman 滤波，系统模型为 \[\begin{aligned} \dot{\bm{X}}(t)&=\bm{A}(t)\bm{X}(t)+\bm{C}(t)\bm{e}(t)\\ \bm{Z}(t)&=\bm{H}(t)\bm{X}(t)+\bm{\Delta}(t) \end{aligned} \tag{4.6.4}\] 系统噪声 \(\bm{e}(t)\) 和观测噪声 \(\bm{\Delta}(t)\) 互不相关，且均为零均值白噪声过程，并且 \[\begin{gathered} E\left[\,\bm{e}(t)\bm{e}^{\mathrm{T}}(\tau)\,\right]=\bm{D}_e(t)\delta(t-\tau)\\ E\left[\,\bm{\Delta}(t)\bm{\Delta}^{\mathrm{T}}(\tau)\,\right]=\bm{D}_{\Delta}(t)\delta(t-\tau)\\ E\left[\,\bm{e}(t)\bm{\Delta}^{\mathrm{T}}(\tau)\,\right]=0 \end{gathered} \tag{4.6.5}\] 其中 \(\bm{D}_e(t)\) 和 \(\bm{D}_{\Delta}(t)\) 为正定矩阵。

1. 可控性

(1) 随机线性离散系统的可控性：

对于离散线性系统而言，可控性矩阵为 \[\bm{W}_C(k-N+1,\ k)=\sum_{i=k-N+1}^{k}\bm{\Phi}_{k,\ i}\bm{\varGamma}_{i-1}\bm{D}_w(i-1)_{i,\ i-1}\bm{\varGamma}_{i-1}^{\mathrm{T}}\bm{\Phi}_{k,\ i}^{\mathrm{T}} \tag{4.6.6}\] 离散线性系统随机一致完全可控的充要条件是：存在正整数 \(N\) 和 \(\beta_1>0\)，\(\beta_2>0\)，使得 \[\beta_1\bm{I}<\bm{W}_C(k-N+1,\ k)<\beta_2\bm{I} \tag{4.6.7}\] 离散线性系统随机完全可控的充分必要条件是：存在正整数 \(N\)，使可控性矩阵正定 \[\bm{W}_C(k-N+1,\ k)>0 \tag{4.6.8}\] 如果系统是定常系统，式 (4.6.6) 可以表示为 \[\bm{W}_C(k-N+1,\ k)=\sum_{i=1}^{N}\bm{\Phi}^{N-i}\bm{\varGamma}\bm{D}_w\left(\bm{\Phi}^{N-i}\bm{\varGamma}\right)^{\mathrm{T}}\] 设 \(\bm{D}_w^{1/2}\) 为正定矩阵的 \(\bm{D}_w\) 的平方根矩阵，即 \(\bm{D}_w=\bm{D}_w^{1/2}\bm{D}_w^{1/2}\)，式 (4.6.6) 可进一步表示为 \[\bm{W}_C=\left[\begin{array}{llll}\bm{\varGamma}\bm{D}_w^{1/2} & \bm{\Phi}\bm{\varGamma}\bm{D}_w^{1/2} & \cdots & \bm{\Phi}^{N-1}\bm{\varGamma}\bm{D}_w^{1/2}\end{array}\right] \begin{bmatrix}\bm{D}_w^{1/2}\bm{\varGamma}^{\mathrm{T}}\\ \bm{D}_w^{1/2}\bm{\varGamma}^{\mathrm{T}}\bm{\Phi}^{\mathrm{T}}\\ \vdots\\ \bm{D}_w^{1/2}\bm{\varGamma}^{\mathrm{T}}\left(\bm{\Phi}^{N-1}\right)^{\mathrm{T}}\end{bmatrix} \tag{4.6.9}\] 设 \[\bm{C}=\left[\begin{array}{llll}\bm{\varGamma}\bm{D}_w^{1/2} & \bm{\Phi}\bm{\varGamma}\bm{D}_w^{1/2} & \cdots & \bm{\Phi}^{N-1}\bm{\varGamma}\bm{D}_w^{1/2}\end{array}\right] \tag{4.6.10}\] 那么 \[\bm{W}_C(k-N+1,\ k)=\bm{C}\bm{C}^{\mathrm{T}} \tag{4.6.11}\] 根据附录 C.3 知，\(\bm{W}_C(k-N+1,\ k)\) 正定的充要条件是 \(\bm{C}\) 为行满秩矩阵，即 \[\mathrm{rank}(\bm{C})=n \tag{4.6.12}\] \(\bm{C}\) 还可以表示为 \[\bm{C}=\left[\begin{array}{llll}\bm{\varGamma} & \bm{\Phi}\bm{\varGamma} & \cdots & \bm{\Phi}^{N-1}\bm{\varGamma}\end{array}\right]\bm{D}_w^{1/2} \tag{4.6.13}\] 因为 \(\bm{D}_w^{1/2}\) 为正定矩阵，所以只要 \[\mathrm{rank}\left[\begin{array}{llll}\bm{\varGamma} & \bm{\Phi}\bm{\varGamma} & \cdots & \bm{\Phi}^{N-1}\bm{\varGamma}\end{array}\right]=n \tag{4.6.14}\] 就可得到 \(\bm{W}_C(k-N+1,\ k)>0\)，那么离散线性系统就是随机完全可控的。

(2) 随机线性连续系统的可控性：

对于连续系统，可控性矩阵为 \[\bm{W}_C(t_0,\ t)=\int_{t_0}^{t}\bm{\Phi}(t,\ \tau)\bm{C}(\tau)\bm{D}_e(\tau)\bm{\Phi}^{\mathrm{T}}(t,\ \tau)\bm{C}^{\mathrm{T}}(\tau)\,\mathrm{d}\tau \tag{4.6.15}\] 连续线性系统的随机一致完全可控的充要条件是：对于任意初始时刻 \(t_0\)，存在 \(t>t_0\)，\(\beta_1>0\) 和 \(\beta_2>0\)，使 \[\beta_1\bm{I}<\bm{W}_C(t_0,\ t)<\beta_2\bm{I} \tag{4.6.16}\] 连续线性系统随机完全可控的充要条件是：对于任意初始时刻 \(t_0\)，存在 \(t>t_0\) 使得 \(\bm{W}_C(t_0,\ t)\) 正定 \[\bm{W}_C(t_0,\ t)>0 \tag{4.6.17}\] 可以证明，当 \(\bm{D}_{\Delta}(t)\rightarrow\infty\) 且 \(\bm{D}_{\hat{X}}(0)=0\) 时，\(\bm{D}_{\hat{X}}(t)=\bm{W}_C(t_0,\ t)\)。

2. 可测性

(1) 随机线性离散系统的可测性：

对于离散系统，可测性矩阵为 \[\bm{W}_O(k-N+1,\ k)=\sum_{j=k-N+1}^{k}\bm{\Phi}_{j,\ k}^{\mathrm{T}}\bm{H}_j^{\mathrm{T}}\bm{D}_{\Delta}^{-1}(j)\bm{H}_j\bm{\Phi}_{j,\ k} \tag{4.6.18}\] 离散线性系统随机一致完全可测的充要条件是：存在正整数 \(N\) 和 \(\alpha_1>0\)，\(\alpha_2>0\)，使得 \[\alpha_1\bm{I}<\bm{W}_O(k-N+1,\ k)<\alpha_2\bm{I} \tag{4.6.19}\] 离散线性系统随机完全可测的充要条件是：存在正整数 \(N\)，使可测性矩阵正定，即满足 \[\bm{W}_O(k-N+1,\ k)>0 \tag{4.6.20}\] 可以证明，对于时不变系统而言，\(\bm{W}_O(k-N+1,\ k)\) 正定等价于 \[\mathrm{rank}\begin{bmatrix}\bm{H}\\ \bm{H}\bm{\Phi}\\ \vdots\\ \bm{H}\bm{\Phi}^{N-1}\end{bmatrix}=n \tag{4.6.21}\] 这与系统为确定性输入的完全可测性的判断标准是一样的。

(2) 随机线性连续系统的可测性：

对于连续系统而言，可测性矩阵为 \[\bm{W}_O(t_0,\ t)=\int_{t_0}^{t}\bm{\Phi}^{\mathrm{T}}(t,\ \tau)\bm{H}^{\mathrm{T}}(\tau)\bm{D}_{\Delta}^{-1}(\tau)\bm{H}(\tau)\bm{\Phi}(t,\ \tau)\,\mathrm{d}\tau \tag{4.6.22}\] 连续线性系统的随机一致完全可控的充要条件是：对于任意初始时刻 \(t_0\)，存在 \(t>t_0\)，\(\alpha_1>0\) 和 \(\alpha_2>0\)， \[\alpha_1\bm{I}<\bm{W}_O(t_0,\ t)<\alpha_2\bm{I} \tag{4.6.23}\] 连续线性系统随机完全可测的充要条件是：存在 \(t>t_0\) 使可测性矩阵正定，即 \[\bm{W}_O(t_0,\ t)>0 \tag{4.6.24}\] 由矩阵黎卡提微分方程证明得到，当 \(\bm{D}_e(t)=0\) 且 \(\bm{D}_{\hat{X}}(0)=\infty\) 时，\(\bm{D}_{\hat{X}}^{-1}(t)=\bm{W}_O(t_0,\ t)\)。

Kalman 滤波的稳定性

控制系统的稳定性是指系统受到某种干扰，在干扰消失后，系统恢复到原有运动状态的能力。就 Kalman 滤波而言，在算法启动时必须先给定初始状态，但在工程实践中，初始值通常不能确切知道，只能假定给出。如果滤波的初始值偏差较大，滤波经过一段时间的递推，也能摆脱初值偏差的干扰，逐渐在较小的范围内变化，那么滤波器是稳定的。下面给出滤波稳定性的定义。

现有任意的滤波器初值 \(\hat{\bm{X}}_1(0)\) 和 \(\hat{\bm{X}}_2(0)\)，并设 \(\hat{\bm{X}}_1(k)\) 和 \(\hat{\bm{X}}_2(k)\) 分别表示以 \(\hat{\bm{X}}_1(0)\) 和 \(\hat{\bm{X}}_2(0)\) 为初始值的递推滤波。对于任意给定的正数 \(\varepsilon\)，都可以找到正数 \(\kappa\)，使得对任意满足 \[\left\|\hat{\bm{X}}_1(0)-\hat{\bm{X}}_2(0)\right\|<\kappa \tag{4.6.25}\] 都有 \[\left\|\hat{\bm{X}}_1(k)-\hat{\bm{X}}_2(k)\right\|<\varepsilon \tag{4.6.26}\] 则称滤波器是稳定的。上式中如果 \[\lim_{k\rightarrow\infty}\left\|\hat{\bm{X}}_1(k)-\hat{\bm{X}}_2(k)\right\|=0 \tag{4.6.27}\] 则称滤波器是一致渐进稳定。

在例 4.4 的滤波器中我们看到，无论初始值取何值，滤波总能达到稳态。那么稳态的滤波器是否一定一致渐进稳定的呢？这里设 \(\hat{\bm{X}}_1(k)\) 和 \(\hat{\bm{X}}_2(k)\) 是分别以 \(\hat{\bm{X}}_1(0)\) 和 \(\hat{\bm{X}}_2(0)\) 为初始值的稳态滤波，那么 \[\begin{aligned} \hat{\bm{X}}_1(k)&=(\bm{I}-\bm{K}\bm{H})\bm{\Phi}\hat{\bm{X}}_1(k-1)+\bm{K}\bm{Z}(k)\\ \hat{\bm{X}}_2(k)&=(\bm{I}-\bm{K}\bm{H})\bm{\Phi}\hat{\bm{X}}_2(k-1)+\bm{K}\bm{Z}(k) \end{aligned} \tag{4.6.28}\] 设 \[\bm{\delta}(k)=\hat{\bm{X}}_1(k)-\hat{\bm{X}}_2(k) \tag{4.6.29}\] 将式 (4.6.28) 代入式 (4.6.29)，得到 \[\bm{\delta}(k)=(\bm{I}-\bm{K}\bm{H})\bm{\Phi}\bm{\delta}(k-1) \tag{4.6.30}\] 通过迭代可得 \[\bm{\delta}(k)=\left[\,(\bm{I}-\bm{K}\bm{H})\bm{\Phi}\,\right]^k\bm{\delta}(0) \tag{4.6.31}\] 其中 \[\bm{\delta}(0)=\hat{\bm{X}}_1(0)-\hat{\bm{X}}_2(0) \tag{4.6.32}\] 由于矩阵 \((\bm{I}-\bm{K}\bm{H})\bm{\Phi}\) 是稳定的，所以 \[\lim_{k\rightarrow\infty}\bm{\delta}(k)=0 \tag{4.6.33}\]

这说明，如果 Kalman 滤波器是稳态的，那么它就一定具有稳定性，无论初值如何选取，滤波总能摆脱初值的影响。

对于随机线性连续定常系统，若系统是稳态的，那么滤波方差将随着时间的递推逐渐地趋向稳态值，即 \(t\rightarrow\infty\)，\(\dot{\bm{D}}_{\hat{X}}(\infty)=0\)，根据矩阵黎卡提微分方程，得到 \[\bm{A}\bm{D}_{\hat{X}}(\infty)+\bm{D}_{\hat{X}}(\infty)\bm{A}^{\mathrm{T}}+\bm{C}\bm{D}_e\bm{C}^{\mathrm{T}} -\bm{D}_{\hat{X}}(\infty)\bm{H}^{\mathrm{T}}\bm{D}_{\Delta}^{-1}\bm{H}\bm{D}_{\hat{X}}(\infty)=0 \tag{4.6.34}\] 上式就是在例 4.4 中提到的代数黎卡提方程，其中的 \(\bm{D}_{\hat{X}}(\infty)\) 表示连续型滤波方差的稳态值，\(\bm{A}\bm{D}_{\hat{X}}(\infty)+\bm{D}_{\hat{X}}(\infty)\bm{A}^{\mathrm{T}}\) 和 \(\bm{C}\bm{D}_e\bm{C}^{\mathrm{T}}\) 分别是状态不确定部分和系统噪声引起的 \(\dot{\bm{D}}_{\hat{X}}(\infty)\) 增大部分，\(\bm{D}_{\hat{X}}(\infty)\bm{H}^{\mathrm{T}}\bm{D}_{\Delta}^{-1}\bm{H}\bm{D}_{\hat{X}}(\infty)\) 是加入观测值后使 \(\dot{\bm{D}}_{\hat{X}}(\infty)\) 减小的部分，两部分相互作用，最后使 \(\dot{\bm{D}}_{\hat{X}}(\infty)=0\)。相应的，增益矩阵的稳态值为 \[\bm{K}(\infty)=\bm{D}_{\hat{X}}(\infty)\bm{H}^{\mathrm{T}}\bm{D}_{\Delta}^{-1} \tag{4.6.35}\] 稳态的滤波为 \[\dot{\hat{\bm{X}}}(t)=\bm{A}\hat{\bm{X}}(t)+\bm{K}(\infty)\left[\,\bm{Z}(t)-\bm{H}\hat{\bm{X}}(t)\,\right] \tag{4.6.36}\] 同样，对于随机离散线性系统，当滤波从任意的初始方差 \(\bm{D}_{\hat{X}}(0)\) 开始，\(\bm{D}_{\hat{X}}(k)\) 随着时间的推移也趋于稳态，存在唯一的正定矩阵 \(\bm{D}_{\hat{X}}\)， \[\lim_{k\rightarrow\infty}\bm{D}_{\hat{X}}(k)=\bm{D}_{\hat{X}} \tag{4.6.37}\] 根据式 (4.2.49)，得到 \[\lim_{k\rightarrow\infty}\bm{D}_{\hat{X}}(k,\ k-1)=\bm{\Phi}\bm{D}_{\hat{X}}\bm{\Phi}^{\mathrm{T}}+\bm{D}_w \tag{4.6.38}\] 显然，\(\bm{D}_{\hat{X}}(k,\ k-1)\) 也必然趋于一稳定值，设 \[\lim_{k\rightarrow\infty}\bm{D}_{\hat{X}}(k,\ k-1)=\bm{D}_{\widetilde{X}} \tag{4.6.39}\] 根据滤波的递推关系，容易得到 \[\bm{D}_{\hat{X}}=\bm{D}_{\widetilde{X}}-\bm{D}_{\widetilde{X}}\bm{H}^{\mathrm{T}}\left(\bm{H}\bm{D}_{\widetilde{X}}\bm{H}^{\mathrm{T}}+\bm{D}_{\Delta}\right)^{-1}\bm{H}\bm{D}_{\widetilde{X}} \tag{4.6.40}\] \[\bm{K}=\bm{D}_{\widetilde{X}}\bm{H}^{\mathrm{T}}\left(\bm{H}\bm{D}_{\widetilde{X}}\bm{H}^{\mathrm{T}}+\bm{D}_{\Delta}\right)^{-1} \tag{4.6.41}\] 由式 (4.2.49) 并考虑递推关系可以得到 \[\begin{aligned} \bm{D}_{\hat{X}}(k+1,\ k)=&\ \bm{\Phi}\bm{D}_{\hat{X}}(k,\ k-1)\left[\,\bm{I}-\bm{H}^{\mathrm{T}}\left(\bm{H}\bm{D}_{\hat{X}}(k,\ k-1)\bm{H}^{\mathrm{T}}+\bm{D}_{\Delta}\right)^{-1}\right.\\ &\quad\left.{}\cdot\bm{H}\bm{D}_{\hat{X}}(k,\ k-1)\,\right]\bm{\Phi}^{\mathrm{T}}+\bm{D}_w \end{aligned} \tag{4.6.42}\] 上式也称为黎卡提差分方程。根据黎卡提差分方程，当 \(\bm{D}_{\hat{X}}(k,\ k-1)\) 稳态后有 \[\bm{D}_{\widetilde{X}}=\bm{\Phi}\left[\,\bm{D}_{\widetilde{X}}-\bm{D}_{\widetilde{X}}\bm{H}^{\mathrm{T}}\left(\bm{H}\bm{D}_{\widetilde{X}}\bm{H}^{\mathrm{T}}+\bm{D}_{\Delta}\right)^{-1}\bm{H}\bm{D}_{\widetilde{X}}\,\right]\bm{\Phi}^{\mathrm{T}}+\bm{D}_w \tag{4.6.43}\]

Kalman 滤波稳定的判别条件

Kalman 等人证明了如果线性系统是随机一致完全可控和随机一致完全可测的，那么 Kalman 滤波器是一致渐近稳定的。对随机线性定常系统来说，完全可控和完全可测等价于一致完全可控和一致完全可测，所以只要系统完全可控和完全可测，那么滤波就是一致渐近稳定的。

例 4.6系统的状态方程和观测方程如下所示 \[\begin{bmatrix}X_1(k+1)\\ X_2(k+1)\end{bmatrix} =\begin{bmatrix}1 & T\\ 0 & 1\end{bmatrix}\begin{bmatrix}X_1(k)\\ X_2(k)\end{bmatrix} +\begin{bmatrix}T^2/2\\ T\end{bmatrix}e(k)\] \[Z(k)=\left[\begin{array}{ll}1 & 0\end{array}\right]\begin{bmatrix}X_1(k)\\ X_2(k)\end{bmatrix}+\Delta(k)\] 假设 \(e(k)\) 和 \(\Delta(k)\) 都是均值为零的白噪声，又互不相关，即 \[\begin{aligned} E\left[\,e(k)\,\right]&=E\left[\,\bm{\Delta}(k)\,\right]=0\\ \mathrm{Cov}\left[\,e(k),\ e(j)\,\right]&=\bm{D}_e\delta(k-j)\\ \mathrm{Cov}\left[\,\bm{\Delta}(k),\ \bm{\Delta}(j)\,\right]&=\bm{D}_{\Delta}\delta(k-j) \end{aligned}\] \(\bm{D}_e\) 和 \(\bm{D}_{\Delta}\) 均正定。判断此系统是否具有一致渐近稳定性。

解：此系统为线性时不变系统，\(n=2\)。由系统方程和观测方程知 \[\bm{\Phi}=\begin{bmatrix}1 & T\\ 0 & 1\end{bmatrix}\ ,\quad \bm{\varGamma}=\begin{bmatrix}T^2/2\\ T\end{bmatrix}\ ,\quad \bm{H}=\left[\begin{array}{ll}1 & 0\end{array}\right]\] 下面根据式 (4.6.14) 来判断此系统是否具有随机完全可控性 \[\left[\begin{array}{ll}\bm{\varGamma} & \bm{\Phi}\bm{\varGamma}\end{array}\right] =\begin{bmatrix}\dfrac{T^2}{2} & \dfrac{3T^2}{2}\\[8pt] T & T\end{bmatrix} =T\begin{bmatrix}\dfrac{T}{2} & \dfrac{3T}{2}\\[8pt] 1 & 1\end{bmatrix}\] 显然只要 \(T\neq 0\)，上式就可以满足 \(\mathrm{rank}\left[\begin{array}{ll}\bm{\varGamma} & \bm{\Phi}\bm{\varGamma}\end{array}\right]=2\)。

根据式 (4.6.21) 来判断此系统是否具有随机一致完全可测性 \[\mathrm{rank}\begin{bmatrix}\bm{H}\\ \bm{H}\bm{\Phi}\end{bmatrix} =\begin{bmatrix}1 & 0\\ 1 & T\end{bmatrix}=2\] 所以，此系统既是随机一致完全可测的，也是随机一致完全可控的，可判定此系统是一致渐近稳定的。

例 4.7有观测方程和状态方程 \[\begin{aligned} \dot{X}(t)&=w(t) & \mathrm{Cov}\left[\,e(t)\,\right]&=\bm{D}_e\\[4pt] Z(t)&=X(t)+\Delta(t) & \mathrm{Cov}\left[\,\Delta(t)\,\right]&=\bm{D}_{\Delta} \end{aligned} \tag{4.6.44}\] \(D_e>0\)，\(D_{\Delta}>0\)。判定此系统是否稳定的，如果是稳定的，求稳定后的滤波方差。

解：在此系统中 \[\bm{A}=0\ ,\quad \bm{\Phi}=1\ ,\quad \bm{C}=1\ ,\quad \bm{H}=1 \tag{4.6.45}\] 可控性矩阵为 \[\begin{aligned} \bm{W}_C(t_0,\ t)&=\int_{t_0}^{t}D_e\,\mathrm{d}\tau\\ &=D_e(t-t_0) \end{aligned} \tag{4.6.46}\] 可测性矩阵为 \[\begin{aligned} \bm{W}_O(t_0,\ t)&=\int_{t_0}^{t}D_{\Delta}^{-1}\,\mathrm{d}\tau\\ &=D_{\Delta}^{-1}(t-t_0) \end{aligned} \tag{4.6.47}\] 显然，此系统满足完全可控和完全可测的条件。又由于此系统是定常系统，因此，此系统也是完全一致可控和完全一致可测的，所以这个系统是一致渐近稳定的。根据式 (4.6.34) 有 \[D_e-D_{\hat{X}}(\infty)\,D_{\Delta}^{-1}\,D_{\hat{X}}(\infty)=0\] 在此问题中，上式的各矩阵都是标量，所以 \[D_{\hat{X}}(\infty)=\sqrt{D_eD_{\Delta}}\] 稳态的增益矩阵为 \[K(\infty)=\sqrt{\frac{D_e}{D_{\Delta}}}\]