Kalman 滤波最优状态估计特性的几何解读

看到一些论文和资料, 将 Kalman 滤波置于 Hilbert Space 描述下进行几何解读 ^5-6. 这些几何解释让人有更直观和深刻的理解, 观点也可以更高屋建瓴. 我们借鉴这些资料, 试图更详细地对 Kalman Filter 进行几何意义的解读. 写下来也为了自己学习和理解.

我们先对要讨论的卡尔曼滤波相关内容进行简要回顾.

I. 系统方程

离散时间系统方程

$${\mathbf{x}}_t={\mathbf{F}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+{\mathbf{w}}_t\text{\hspace{1em}(I-1)}$$

描述了系统的当前时刻状态 ${\mathbf{x}}_t$ 由上一时刻状态 ${\mathbf{x}}_{t-1}$ 演变的动力学, 也称为预测方程. 其中

$t$ 是离散时间步, $t=0,1,2,\dots$

${\mathbf{x}}_t$ 为 $n$ 维状态向量;

${\mathbf{F}}_t$ 为系统的状态转移矩阵;

${\mathbf{u}}_t$ 为当前控制输入;

${\mathbf{B}}_t$ 为控制输入矩阵;

${\mathbf{w}}_t$ 为 $n$ 维零均值过程白噪声, 即
$$\mathbf{E}[{\mathbf{w}}_t{\mathbf{w}}_l^{\mathrm{T}}]={\mathbf{Q}}_t{\delta }_{tl}=\{\begin{array}{rlr}& {\mathbf{Q}}_t& (t=l)\\ & 0& (t\ne l)\end{array}\text{\hspace{1em}(I-2)}$$
其中 ${\mathbf{Q}}_t$ 为正定的协方差矩阵.


测量方程

$${\mathbf{y}}_t={\mathbf{H}}_t{\mathbf{x}}_t+{\mathbf{v}}_t\text{\hspace{1em}(I-3)}$$

其中

${\mathbf{y}}_t$ 是 $m$ 维观测向量;

${\mathbf{H}}_t$ 观测矩阵, 描述状态向量到观测向量的变换;

${\mathbf{v}}_t$ 是 $n$ 维零均值的观测白噪声, 即
$$\mathbf{E}[{\mathbf{v}}_t{\mathbf{v}}_t^{\mathrm{T}}]={\mathbf{R}}_t{\delta }_{tl}=\{\begin{array}{rlr}& {\mathbf{R}}_t& (t=l)\\ & 0& (t\ne l)\end{array}\text{\hspace{1em}(I-4)}$$
其中 ${\mathbf{R}}_t$ 为正定的协方差矩阵.

需要说明:

- 我们本文的分析都建立在线性系统的基础上;

- 一步时间内的控制输入提供一个确定的偏置 (后面分析时再讨论);

- 随机变量 ${\mathbf{x}}_t$、${\mathbf{w}}_t$、${\mathbf{v}}_t$ 之间独立不相关.

在系统模型的基础上, 继续回顾对系统状态进行估计的卡尔曼滤波方法.

II. Kalman Filter

卡尔曼滤波算法分为两个阶段, 分别是预测阶段和更新阶段¹.


预测阶段

$${\hat{\mathbf{x}}}_{t|t-1}={\mathbf{F}}_t{\hat{\mathbf{x}}}_{t-1|t-1}+{\mathbf{B}}_t{\mathbf{u}}_t\text{\hspace{1em}(II-1)}$$

$${\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^{\mathrm{T}}+{\mathbf{Q}}_t\text{\hspace{1em}(II-2)}$$

其中

${\hat{\mathbf{x}}}_{t|t-1}$ 是基于预测方程 (II-1) 对未知的真实状态向量 ${\mathbf{x}}_t$ 的预测, 是对真实状态向量的先验估计 (Priori Estimation).

${\mathbf{P}}_{t|t-1}$ 是预测状态的误差协方差矩阵.


更新阶段

$${\hat{\mathbf{x}}}_{t|t}={\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{K}}_t\left({\mathbf{y}}_t-{\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1}\right)\text{\hspace{1em}(II-3)}$$

$${\mathbf{P}}_{t|t}={\mathbf{P}}_{t|t-1}-{\mathbf{K}}_t{\mathbf{H}}_t{\mathbf{P}}_{t|t-1}\text{\hspace{1em}(II-4)}$$

$${\mathbf{K}}_t={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{\mathrm{T}}{\left({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{\mathrm{T}}+{\mathbf{R}}_t\right)}^{-1}\text{\hspace{1em}(II-5)}$$

其中

${\hat{\mathbf{x}}}_{t|t}$ 称为修正的状态估计, 是对状态预测值 ${\hat{\mathbf{x}}}_{t|t-1}$ 的更新, 是对真实状态向量的后验估计 (Posteriori Estimation).

${\tilde{\mathbf{y}}}_t={\mathbf{y}}_t-{\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1}$ 为新息 (Innovation Variable), 也叫测量残差, 是实际传感器测量值与传感器预测值之间的差.

${\mathbf{K}}_t$ 为卡尔曼滤波增益 (Kalman Gain), 是该算法的核心.

${\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{\mathrm{T}}+{\mathbf{R}}_t$ 称为新息协方差矩阵.
${\mathbf{P}}_{t|t}$ 为更新状态的误差协方差矩阵.

卡尔曼滤波的预测阶段的重点是动力学建模, 需要对系统的动力学特性进行准确刻画, 得到的状态预测值 ${\hat{\mathbf{x}}}_{t|t-1}$ 作为更新阶段的先验信息.

预测阶段相对明确和直观, 没有融合与优化. 预测阶段只作为更新阶段的先验输入对待.

卡尔曼滤波多传感器信息的融合、最优状态估计等特性都体现在更新阶段, 卡尔曼滤波器的特性是在更新阶段实现的.

从最优化的角度, 卡尔曼滤波是最优估计, 是一种最小均方误差估计器 (Minimum Mean-Square Error Estimator).

下面我们从几何正交投影角度解释卡尔曼滤波的优化特性、新息以及更新过程的几何意义等.

III. Hilbert Space

我们已经熟悉了欧式空间, 维度是确定和有限的, 内积计算也简单. 我们将要解释处理的卡尔曼滤波器的状态向量或者观测向量的维度并不一定限于某一值, 不同的系统维度变化不同; 另外, 状态变量和观测数据都是被噪声/扰动污染的随机向量. 甚至, 更深入的研究利用 Kalman Filter 进行时间-空间过程的最优估计 (无穷维). 为了得到一致和有力的数学工具, 引入可以描述无限维内积空间的 Hilbert Space 的定义. 当然, 此处我们在处理卡尔曼滤波数据时, 还是基于有限维内积空间中的情形进行考虑.

Hilbert Space²

A Hilbert space is a vector space $\mathcal{H}$ with an inner product $⟨f,g⟩$ such that the norm defined by
$$\Vert f\Vert =\sqrt{⟨f,f⟩}\text{\hspace{1em}(III-1)}$$
turns $\mathcal{H}$ into a complete metric space. If the metric defined by the norm is not complete, then $\mathcal{H}$ is instead known as an inner product space.

已知 $n$ 维实的随机向量
$$\mathbf{x}=\left[\begin{array}{c}{x}_1\\ x_2\\ ⋮\\ x_n\end{array}\right]\in {\mathbb{R}}^n\text{\hspace{1em}(III-2)}$$
其中每个元素 $x_i$ 为随机变量 (随机的标量).

假设 { y i } i ∈ N \{ \mathbf{y}_i \}_{i\in\mathbb{N}} {yi​}i∈N​ 序列组成了 (III-2) 形式的 $n$ 维随机向量的集合. 该集合内任意元素的所有线性组合构成了一个随机向量空间 $\mathcal{H}$. 我们在该空间上构造内积.

假设 $\mathbf{x}$ 和 $\mathbf{y}$ 是随机向量空间 $\mathcal{H}$ 上的任意元素, $\mathbf{x}$ 和 $\mathbf{y}$ 都为 $n$ 维随机向量, 则两者的内积定义为
$$⟨\mathbf{x},\mathbf{y}⟩=\mathbf{E}[{\mathbf{x}}^{\mathrm{T}}\mathbf{y}]=\mathbf{E}\left[\sum _{i=1}^n{x}_i{y}_i\right]=\mathrm{Tr}(\mathbf{E}[\mathbf{x}{\mathbf{y}}^{\mathrm{T}}])\text{\hspace{1em}(III-3)}$$
其中 Tr 即为矩阵的迹 (Trace). 由内积导出的随机向量 $\Vert \mathbf{x}\Vert$ 的范数的定义
$$\begin{array}{rl}\Vert x\Vert & =\sqrt{\mathbf{E}[{\mathbf{x}}^{\mathrm{T}}\mathbf{x}]}\\ & =\sqrt{\mathbf{E}[x_1^2+x_2^2+\cdots +x_n^2]}\\ & =\sqrt{\mathbf{E}[x_1^2]+\mathbf{E}[x_2^2]+\cdots +\mathbf{E}[x_n^2]}\\ & =\sqrt{\mathrm{Trace}(\mathbf{E}[\mathbf{x}{\mathbf{x}}^{\mathrm{T}}])}\end{array}\text{\hspace{1em}(III-4)}$$
关于随机向量空间 $\mathcal{H}$ 的完备性, 此处只能给个大概的参考描述. 随机向量 $\mathbf{x}$ 中独立随机变量 $x_i$ 处于完备概率空间 (完备化得到), 由参考文献可知³, 随机向量 $\mathbf{x}$ 所处内积空间是完备的. 故装备了以上 (III-3) 内积定义的随机向量空间 $\mathcal{H}$ 就是本文需要构建的那个 Hilbert Space.

下文讨论的距离就是式 (III-4) 定义的范数.

IV. Orthogonal Porjection

Orthogonal Decomposition⁴

If $\mathcal{Y}$ is a closed subspace of Hilbert space $\mathcal{H}$, then we have:
$$\mathcal{H}=\mathcal{Y}\oplus {\mathcal{Y}}^{\perp }\text{\hspace{1em}(IV-1)}$$
where Y ⊥ = { z ∈ H : ⟨ z , y ⟩ = 0 , ∀ y ∈ Y } \mathcal{Y}^{\bot} = \{\mathbf{z} \in \mathcal{H}: \langle \mathbf{z}, \mathbf{y} \rangle = 0, \forall \mathbf{y} \in \mathcal{Y}\} Y⊥={z∈H:⟨z,y⟩=0,∀y∈Y}.

In other words, for any $\mathbf{x}\in \mathcal{H}$, $\mathbf{x}$ can be expressed uniquely as
$$\mathbf{x}=\mathbf{y}+\mathbf{z}$$
where $\mathbf{y}\in \mathcal{Y}$ and $\mathbf{z}\in {\mathcal{Y}}^{\perp }$.
Moreover, $\mathbf{y}$ and $\mathbf{z}$ are the unique elements of $\mathcal{Y}$ and ${\mathcal{Y}}^{\perp }$ whose distance to $\mathbf{x}$ is smallest.

Fig.1 正交分解示意图

为了说明正交分解得到的 $\mathbf{y}$ 和 $\mathbf{z}$ 是各自空间内到达 $\mathbf{x}$ 距离最近的点, 我们只需要借用勾股定理 (毕达哥拉斯定理), 如 Fig.1 所示.

简单证明:

$\mathcal{M}$ 上任一不同于正交分解所得向量 $\mathbf{y}$ 的另一向量 ${\mathbf{y}}^{{}^{\prime }}$, 则由勾股定理
$$\Vert \mathbf{x}-{\mathbf{y}}^{{}^{\prime }}\Vert =\Vert \mathbf{x}-\mathbf{y}\Vert +\Vert {\mathbf{y}}^{{}^{\prime }}-\mathbf{y}\Vert >\Vert \mathbf{x}-\mathbf{y}\Vert$$
${\mathcal{M}}^{\perp }$ 上的 $\mathbf{z}$ 同理. 得证. 有这个简单证明, 可以知道正交分解是唯一的.

为下面说明方便, 定义记号如下:

- 假设向量 $\mathbf{x}\in \mathcal{H}$ 和一个闭的线性子空间 $\mathcal{Y}\subset \mathcal{H}$.

- 沿着 ${\mathcal{Y}}^{\perp }$ 投影于 $\mathcal{Y}$ 上的正交投影子 (Orthogonal Projector) 记为 ${\mathcal{P}}_{\mathcal{Y}}(\cdot )$.

- 向量 $\mathbf{x}$ 向子空间 $\mathcal{Y}$ 进行正交投影变换得到投影向量记为 ${\mathbf{x}}_{\mathcal{Y}}$, 即
$${\mathbf{x}}_{\mathcal{Y}}={\mathcal{P}}_{\mathcal{Y}}(\mathbf{x})\text{\hspace{1em}(IV-2)}$$
- 另记距离平方 ${\sigma }_{\mathcal{Y}}(\mathbf{x})=\Vert \mathbf{x}-{\mathbf{x}}_{\mathcal{Y}}{\Vert }^2$.

V. Minimum Variance Estimate

由测量方程
$$\mathbf{y}=\mathbf{Hx}+\mathbf{v}\text{\hspace{1em}(V-1)}$$
估计系统状态 $\hat{\mathbf{x}}=\mathbf{Ky}$ 称为基于测量 $\mathbf{y}$ 的线性估计.

Minimum Variance Linear Estimate⁵

Let $\mathbf{y}$ and $\mathbf{x}$ be random vectors. Assume that ${\left[\mathbf{E}[\mathbf{y}{\mathbf{y}}^{\mathrm{T}}]\right]}^{-1}$ exists. The linear estimate $\hat{\mathbf{x}}$ of $\mathbf{x}$ based on $\mathbf{y}$ that minimizes $\Vert \mathbf{x}-\hat{\mathbf{x}}{\Vert }^2$ is
$$\hat{\mathbf{x}}=\mathbf{E}[\mathbf{x}{\mathbf{y}}^{\mathrm{T}}]{\left[\mathbf{E}[\mathbf{y}{\mathbf{y}}^{\mathrm{T}}]\right]}^{-1}\mathbf{y}\text{\hspace{1em}(V-2)}$$

式 (V-2) 得到的 $\hat{\mathbf{x}}$ 使得下式成立
$$\begin{array}{rl}\hat{\mathbf{x}}& =\underset{\mathbf{m}\in \mathcal{Y}}{\mathrm{argmin}}\Vert \mathbf{x}-\mathbf{m}{\Vert }^2\\ & =\underset{\mathbf{m}\in \mathcal{Y}}{\mathrm{argmin}}\sqrt{\mathbf{E}\left[(\mathbf{x}-\mathbf{m}{)}^{\mathrm{T}}(\mathbf{x}-\mathbf{m})\right]}\end{array}\text{\hspace{1em}(V-3)}$$
根据前面 “Orthogonal Decomposition” 定理可知, 极值条件 (V-3) 等价于估计误差 $\mathbf{x}-\hat{\mathbf{x}}$ 与测量向量 $\mathbf{y}$ 垂直 (内积为零), 即
$$⟨\mathbf{x}-\hat{\mathbf{x}},\mathbf{y}⟩=\mathbf{E}\left[(\mathbf{x}-\hat{\mathbf{x}}{)}^{\mathrm{T}}\mathbf{y}\right]=0\&\hat{\mathbf{x}}=\mathbf{Ky}\text{\hspace{1em}(V-4)}$$
此处 $\mathbf{y}$ 张成子空间 $\mathcal{Y}$. 最小方差估计 $\hat{\mathbf{x}}$ 就是让状态 $\mathbf{x}$ 正交投影到测量 $\mathbf{y}$ 张成的子空间上得到 ${\mathbf{x}}_{\mathcal{Y}}$, 即
$${\mathbf{x}}_{\mathcal{Y}}=\hat{\mathbf{x}}\text{\hspace{1em}(V-5)}$$
将 $\Vert \mathbf{x}-\hat{\mathbf{x}}\Vert$ 视作估计误差, 记为 $\mathbf{ϵ}$.

需要说明,

- 无偏估计情况下, 最小均方误差估计式 (V-3) 和最小方差估计等价;

- Kalman Filter 初值的设定以及迭代过程, 使其是无偏估计.

Minimum Variance Linear Estimate of a Linear Function⁵

The minimum variance linear estimate of a linear function of $\mathbf{x}$ is equal to the linear function of the minimum variance estimate of $\mathbf{x}$. In other words given a matrix $\mathbf{T}$ (a linear function), the minimum variance estimate of $\mathbf{Tx}$ is
$$\mathbf{T}\hat{\mathbf{x}}=\mathbf{T}\mathbf{E}[\mathbf{x}{\mathbf{y}}^{\mathrm{T}}]{\left[\mathbf{E}[\mathbf{y}{\mathbf{y}}^{\mathrm{T}}]\right]}^{-1}\mathbf{y}\text{\hspace{1em}(V-6)}$$

简单证明:

把 $\mathbf{Tx}$ 看作整体替换 “Minimum Variance Linear Estimate” 中式 (V-2) 中的 $\mathbf{x}$,
$$\begin{array}{rl}\widehat{(\mathbf{Tx})}& =\mathbf{E}[(\mathbf{Tx}){\mathbf{y}}^{\mathrm{T}}]{\left[\mathbf{E}[\mathbf{y}{\mathbf{y}}^{\mathrm{T}}]\right]}^{-1}\mathbf{y}\\ & =\mathbf{T}\mathbf{E}[\mathbf{x}{\mathbf{y}}^{\mathrm{T}}]{\left[\mathbf{E}[\mathbf{y}{\mathbf{y}}^{\mathrm{T}}]\right]}^{-1}\mathbf{y}\\ & =\mathbf{T}\hat{\mathbf{x}}\end{array}\text{\hspace{1em}(V-7)}$$
线性转换后的最小方差估计是最小方差估计的线性变换, 即
$${\mathcal{P}}_{\mathcal{Y}}(\mathbf{Tx})=\mathbf{T}{\mathcal{P}}_{\mathcal{Y}}(\mathbf{x})\text{\hspace{1em}(V-8)}$$

推论 V-1:

$[\mathbf{E}[\mathbf{y}{\mathbf{y}}^{\mathrm{T}}]{]}^{-1}$ 存在情况下, 任意向量 $\mathbf{x}$ 和线性变换 $\mathbf{T}$ 满足
- $\mathbf{T}{\mathcal{P}}_{\mathcal{Y}}(\mathbf{x})\in \mathcal{Y}$;

- $\mathbf{Tx}-\mathbf{T}{\mathcal{P}}_{\mathcal{Y}}(\mathbf{x})\in {\mathcal{Y}}^{\perp }$.

证明: 因为 ${\mathcal{P}}_{\mathcal{Y}}(\mathbf{Tx})\in \mathcal{Y}$ 及式 (V-8);
\qquad 因为 $\mathbf{Tx}-{\mathcal{P}}_{\mathcal{Y}}(\mathbf{Tx})\in {\mathcal{Y}}^{\perp }$ 及式 (V-8).

在这一章结束前, 我们补充定义沿着 $\mathcal{Y}$ 投影于 ${\mathcal{Y}}^{\perp }$ 上的正交投影子为 ${\mathcal{P}}_{{\mathcal{Y}}^{\perp }}(\cdot )$.
由 Orthogonal Decomposition 定义, $\forall \mathbf{x}\in \mathcal{H}$
$$\mathbf{x}={\mathcal{P}}_{\mathcal{Y}}(\mathbf{x})+{\mathcal{P}}_{{\mathcal{Y}}^{\perp }}(\mathbf{x})\text{\hspace{1em}(V-9)}$$
考虑线性变换 $\mathbf{T}$ 对 ${\mathcal{P}}_{{\mathcal{Y}}^{\perp }}(\cdot )$ 的影响
$$\begin{array}{rl}\mathbf{Tx}& ={\mathcal{P}}_{\mathcal{Y}}(\mathbf{Tx})+{\mathcal{P}}_{{\mathcal{Y}}^{\perp }}(\mathbf{Tx})\\ \stackrel{(\mathrm{V}-8)}{\Rightarrow }{\mathcal{P}}_{{\mathcal{Y}}^{\perp }}(\mathbf{Tx})& =\mathbf{Tx}-\mathbf{T}{\mathcal{P}}_{\mathcal{Y}}(\mathbf{x})\\ \Rightarrow {\mathcal{P}}_{{\mathcal{Y}}^{\perp }}(\mathbf{Tx})& =\mathbf{T}\left[\mathbf{x}-{\mathcal{P}}_{\mathcal{Y}}(\mathbf{x})\right]\\ \stackrel{(\mathrm{V}-9)}{\Rightarrow }{\mathcal{P}}_{{\mathcal{Y}}^{\perp }}(\mathbf{Tx})& =\mathbf{T}{\mathcal{P}}_{{\mathcal{Y}}^{\perp }}(\mathbf{x})\end{array}\text{\hspace{1em}(V-10)}$$

推论 V-2:
$[\mathbf{E}[\mathbf{y}{\mathbf{y}}^{\mathrm{T}}]{]}^{-1}$ 存在情况下, 任意向量 $\mathbf{x}$ 和线性变换 $\mathbf{T}$, 满足 $\mathbf{T}{\mathcal{P}}_{{\mathcal{Y}}^{\perp }}(\mathbf{x})\in {\mathcal{Y}}^{\perp }$.

证明: 因为 ${\mathcal{P}}_{{\mathcal{Y}}^{\perp }}(\mathbf{Tx})\in {\mathcal{Y}}^{\perp }$ 及式 (V-10).

VI. Kalman Filter 的正交投影

我们使用归纳法证明 Kalman Filter 的几何意义 —— 基于几何正交投影实现最优状态估计.

1. 时刻 $t=0$ 的初值

$${\hat{\mathbf{x}}}_{0|0}=\mathbf{E}[\mathbf{x}]\text{\hspace{1em}(VI-1-1)}$$

$${\mathbf{P}}_{0|0}=\mathbf{E}[(x-{\hat{\mathbf{x}}}_{0|0})(x-{\hat{\mathbf{x}}}_{0|0}{)}^{\mathrm{T}}]\text{\hspace{1em}(VI-1-2)}$$

以上两式的成立使得 Kalman Filter 是无偏估计. ${\mathbf{P}}_{0|0}$ 是正定对称矩阵.

2. 时刻 $t=n-1$ 的假设

假设 $n-1$ 时刻, 给定测量值序列 ${\mathbf{y}}_1,{\mathbf{y}}_2,\dots ,{\mathbf{y}}_{n-1}$关于状态向量 ${\mathbf{x}}_{n-1}$ 的最小均方误差估计是 ${\hat{\mathbf{x}}}_{n-1|n-1}$, 记作 ${\mathbf{x}}_{\mathcal{A}}$. 即
$$\begin{array}{rl}{\mathbf{x}}_{\mathcal{A}}& ={\hat{\mathbf{x}}}_{n-1|n-1}\\ & =\underset{{\hat{\mathbf{x}}}_{n-1}\in \mathcal{A}}{\mathrm{argmin}}\sqrt{\mathbf{E}\left[({\mathbf{x}}_{n-1}-{\hat{\mathbf{x}}}_{n-1}{)}^{\mathrm{T}}({\mathbf{x}}_{n-1}-{\hat{\mathbf{x}}}_{n-1})\right]}\\ & =\underset{{\hat{\mathbf{x}}}_{n-1}\in \mathcal{A}}{\mathrm{argmin}}\Vert {\mathbf{x}}_{n-1}-{\hat{\mathbf{x}}}_{n-1}\Vert \end{array}\text{\hspace{1em}(VI-2-1)}$$
估计误差向量为
$${\mathbf{ϵ}}_{n-1}={\mathbf{x}}_{n-1}-{\hat{\mathbf{x}}}_{n-1|n-1}\text{\hspace{1em}(VI-2-2)}$$
误差协方差矩阵为
$$\begin{array}{rl}{\mathbf{P}}_{n-1|n-1}& =\mathbf{E}[({\mathbf{x}}_{n-1}-{\hat{\mathbf{x}}}_{n-1|n-1})({\mathbf{x}}_{n-1}-{\hat{\mathbf{x}}}_{n-1|n-1}{)}^{\mathrm{T}}]\\ & =\mathbf{E}[{\mathbf{ϵ}}_{n-1}{\mathbf{ϵ}}_{n-1}^{\mathrm{T}}]\end{array}\text{\hspace{1em}(VI-2-3)}$$

Fig.2 上一时刻的正交投影示意图

以 Fig.2 来进行解释, $\mathcal{A}\subset \mathcal{H}$ 是由 ${\mathbf{y}}_1,{\mathbf{y}}_2,\dots ,{\mathbf{y}}_{n-1}$ 张成的子空间. ${\mathbf{x}}_{\mathcal{A}}$ 是 ${\mathbf{x}}_{n-1}$ 沿着 ${\mathcal{A}}^{\perp }$ 向 $\mathcal{A}$ 正交投影得到的向量, 即
$${\mathbf{x}}_{\mathcal{A}}={\mathcal{P}}_{\mathcal{A}}({\mathbf{x}}_{n-1})\text{\hspace{1em}(VI-2-4)}$$
最小状态估计的误差

$${\mathbf{ϵ}}_{n-1}\in {\mathcal{A}}^{\perp }\text{\hspace{1em}(VI-2-5)}$$
由内积的定义式 (III-3) 及式 (V-2-3) 得
$${\mathbf{x}}_{\mathcal{A}}=\underset{{\hat{\mathbf{x}}}_{n-1}\in \mathcal{A}}{\mathrm{argmin}}\mathrm{Tr}({\mathbf{P}}_{n-1|n-1})\text{\hspace{1em}(VI-2-6)}$$
即 $t=n-1$ 时刻的代价函数可以写成
$${\mathbf{J}}_{n-1}=\mathrm{Tr}({\mathbf{P}}_{n-1|n-1})\text{\hspace{1em}(VI-2-7)}$$
这里我们假设了 $t=n-1$ 时刻, 关于估计误差的最小二乘法意义下的最优解, 就是状态向量 ${\mathbf{x}}_{n-1}$ 向由测量向量序列 ${\mathbf{y}}_1,{\mathbf{y}}_2,\dots ,{\mathbf{y}}_{n-1}$ 张成的子空间 $\mathcal{A}$ 投影, 得到的投影向量 ${\mathbf{x}}_{\mathcal{A}}$. 换句话说, Kalman Filter 这个最优估计的滤波过程就是计算正交投影子 ${\mathcal{P}}_{\mathcal{A}}(\cdot )$ 的过程.

3. 时刻 $t=n$ 的证明

原有向量 ${\mathbf{y}}_1,{\mathbf{y}}_2,\dots ,{\mathbf{y}}_{n-1}$ 组成子空间 $\mathcal{A}$. 在此基础上新增测量向量 ${\mathbf{y}}_n$, 由 ${\mathbf{y}}_1,{\mathbf{y}}_2,\dots ,{\mathbf{y}}_{n-1},{\mathbf{y}}_n$ 张成扩展后的子空间 $\mathcal{B}\subset \mathcal{H}$. 即因为新测量 ${\mathbf{y}}_n$ 的加入, $\mathcal{B}$ 是 $\mathcal{A}$ 的扩展, 写成 $\mathcal{A}\subset \mathcal{B}\subset \mathcal{H}$.

Fig.3 所示是 $t=n$ 时刻真实状态向量 ${\mathbf{x}}_n$ 与两个子空间 $\mathcal{A}$ 及 $\mathcal{B}$ 之间的正交投影关系的描述, 梳理这里面的几何关系来解释 Kalman Filter 过程是我们的目标.

3.1. 预测推导

${\mathbf{x}}_{\mathcal{A}}$ 是上一时刻状态向量在子空间 $\mathcal{A}$ 中的投影向量, 是上一时刻的最优估计值 (我们这个归纳法的假设条件).

由预测阶段的系统方程 (II-1) 可知
$${\hat{\mathbf{x}}}_{n|n-1}={\mathbf{F}}_n{\mathbf{x}}_{\mathcal{A}}+{\mathbf{B}}_n{\mathbf{u}}_n\text{\hspace{1em}(VI-3-1-1)}$$
在 $t=n$ 时刻, ${\mathbf{B}}_n{\mathbf{u}}_n$ 是控制输入中明确的已知的部分, 可能存在的输入扰动通过白噪声 ${\mathbf{w}}_t$ 建模 (参考式 (I-1)). 所以式 (VI-3-1-1) 中的 ${\mathbf{B}}_n{\mathbf{u}}_n$ 在当前时刻看作是常向量, 提供给整个线性系统一个整体的偏移. 这个整体偏移不会对我们的优化和几何描述产生影响, 故为了简化描述, 我们省略这个外部输入的向量项 ${\mathbf{B}}_n{\mathbf{u}}_n$.

原始的预测方程 (I-1) 也可以简化为

$${\mathbf{x}}_t={\mathbf{F}}_t{\mathbf{x}}_{t-1}+{\mathbf{w}}_t\text{\hspace{1em}(VI-3-1-2)}$$
式 (VI-3-1-1) 就可以简写为
$${\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}\triangleq {\hat{\mathbf{x}}}_{n|n-1}={\mathbf{F}}_n{\mathbf{x}}_{\mathcal{A}}\text{\hspace{1em}(VI-3-1-3)}$$

${\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}$ 为根据上一时刻的最优状态估计, 利用系统动力学预测的当前时刻的先验估计. 由前面的定理 “Minimum Variance Linear Estimate of a Linear Function” 可知, ${\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}$ 是系统状态 ${\mathbf{x}}_n$ 基于 1 到 $t=n-1$ 时刻测量 ${\mathbf{y}}_1,{\mathbf{y}}_2,\dots ,{\mathbf{y}}_{n-1}$ 得到的最优估计 (预测).

由推论 V-1, ${\mathbf{x}}_n$ 正交投影于 $\mathcal{A}$ 得到 ${\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}\in \mathcal{A}$, 且由定理 “Orthogonal Decomposition” 可知
$${\mathbf{x}}_n-{\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}\in {\mathcal{A}}^{\perp }\text{\hspace{1em}(VI-3-1-4)}$$
对应于 ${\hat{\mathbf{x}}}_{n|n-1}$ 的误差协方差矩阵
$$\begin{array}{rl}& {\mathbf{P}}_{n|n-1}\\ =& \mathbf{E}[({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n-1})({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n-1}{)}^{\mathrm{T}}]\\ =& \mathbf{E}[({\mathbf{F}}_n{\mathbf{x}}_{n-1}+{\mathbf{w}}_t-{\mathbf{F}}_n{\hat{\mathbf{x}}}_{n-1|n-1})({\mathbf{F}}_n{\mathbf{x}}_{n-1}+{\mathbf{w}}_t-{\mathbf{F}}_n{\hat{\mathbf{x}}}_{n-1|n-1}{)}^{\mathrm{T}}]\\ =& \mathbf{E}[({\mathbf{F}}_n({\mathbf{x}}_{n-1}-{\hat{\mathbf{x}}}_{n-1|n-1})+{\mathbf{w}}_n)({\mathbf{F}}_n({\mathbf{x}}_{n-1}-{\hat{\mathbf{x}}}_{n-1|n-1})+{\mathbf{w}}_n{)}^{\mathrm{T}}]\\ =& {\mathbf{F}}_n{\mathbf{P}}_{n-1|n-1}{\mathbf{F}}_n^{\mathrm{T}}+{\mathbf{Q}}_n\end{array}\text{\hspace{1em}(VI-3-1-5)}$$

${\mathbf{P}}_{n|n-1}$ 是正定对称矩阵.

Fig.3 当前时刻的正交投影示意图

有了状态向量的预测 ${\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}$ 之后, 可以进一步根据测量方程 (I-3) 预测该状态向量条件下的测量向量, 即 ${\mathbf{H}}_n{\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}$.

根据 “Minimum Variance Linear Estimate of a Linear Function”, ${\mathbf{H}}_n{\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}$ 是测量数据 ${\mathbf{y}}_n$ 基于 1 时刻到 $t=n-1$ 时刻测量 ${\mathbf{y}}_1,{\mathbf{y}}_2,\dots ,{\mathbf{y}}_{n-1}$ 得到的最优估计 (预测), 即由 ${\mathbf{y}}_n$ 正交投影于 $\mathcal{A}$ 得到. 定义符号
$${\mathbf{y}}_{\mathcal{A}}\triangleq {\mathbf{H}}_n{\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}\text{\hspace{1em}(VI-3-1-6)}$$

将已测量到的 ${\mathbf{y}}_n$ 与预测得到 ${\mathbf{y}}_{\mathcal{A}}$ 之间的误差定义为新息 (Innovation variable)
$${\tilde{\mathbf{y}}}_n\triangleq {\mathbf{y}}_n-{\mathbf{y}}_{\mathcal{A}}\text{\hspace{1em}(VI-3-1-7)}$$

对应于 ${\mathbf{y}}_{\mathcal{A}}$ 的误差协方差矩阵
$$\begin{array}{rl}& \mathbf{E}[({\mathbf{y}}_n-{\mathbf{y}}_{\mathcal{A}})({\mathbf{y}}_n-{\mathbf{y}}_{\mathcal{A}}{)}^{\mathrm{T}}]\\ =& \mathbf{E}[({\mathbf{H}}_n{\mathbf{x}}_n+{\mathbf{v}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1})({\mathbf{H}}_n{\mathbf{x}}_n+{\mathbf{v}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1}{)}^{\mathrm{T}}]\\ =& \mathbf{E}[({\mathbf{H}}_n({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n-1})+{\mathbf{v}}_n)({\mathbf{H}}_n({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n-1})+{\mathbf{v}}_n{)}^{\mathrm{T}}]\\ =& {\mathbf{H}}_n{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}+{\mathbf{R}}_n\end{array}\text{\hspace{1em}(VI-3-1-8)}$$
这里定义符号
$${\mathbf{P}}_y\triangleq {\mathbf{H}}_n{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}+{\mathbf{R}}_n\text{\hspace{1em}(VI-3-1-9)}$$
${\mathbf{P}}_y$ 就是新息协方差矩阵, 也是正定对称矩阵 (保证逆矩阵存在).

3.2. 更新推导

我们分析一下新息, 由定理 “Orthogonal Decomposition” 可知
$${\tilde{\mathbf{y}}}_n={\mathbf{y}}_n-{\mathbf{y}}_{\mathcal{A}}\in {\mathcal{A}}^{\perp }\text{\hspace{1em}(VI-3-2-1)}$$
实时上 ${\mathbf{y}}_{\mathcal{A}}$ 是有 $\mathcal{A}$ 上原有信息构成的, 并不提供新的信息, 而 ${\tilde{\mathbf{y}}}_n$ 是无法由 $\mathcal{A}$ 上原有信息构成的新的信息, 故而命名. 类似于 Gram-Schmidt正交化, 新的子空间 $\mathcal{B}$ 是由原有子空间 $\mathcal{A}$ 扩展加上新的向量基 ${\tilde{\mathbf{y}}}_n$ 得到的. 换一种说法是 $\mathcal{B}=\mathcal{A}\oplus A^{\perp }$.

进一步, 对 $t=n$ 时刻的状态向量 ${\mathbf{x}}_n$ 在 $\mathcal{B}$ 和 ${\mathcal{B}}^{\perp }$ 两个正交子空间内进行直和分解, 得到在子空间 $\mathcal{B}$ 的投影 (最优估计) 为 ${\mathbf{x}}_{\mathcal{B}}$. 参考 [6] 构造新的变量
$${\mathbf{z}}_n={\mathbf{x}}_n-{\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}+{\overline{\mathbf{K}}}_n({\mathbf{y}}_n-{\mathbf{y}}_{\mathcal{A}})\text{\hspace{1em}(VI-3-2-2)}$$
下面试图选取合适的 ${\overline{\mathbf{K}}}_n$ 使得 ${\mathbf{z}}_n\perp \mathcal{B}$.

由式 (VI-3-1-4) 、式 (VI-3-2-1) 以及推论 V-2 可知, 任意的 ${\overline{\mathbf{K}}}_n$ 都使得
$${\mathbf{z}}_n\perp \mathcal{A}\text{\hspace{1em}(VI-3-2-3)}$$
故要使 ${\mathbf{z}}_n\perp \mathcal{B}$, 只要再让 ${\mathbf{z}}_n$ 垂直于新扩展的向量基 ${\tilde{\mathbf{y}}}_n$, 即
$$⟨{\mathbf{z}}_n,{\mathbf{y}}_n-{\mathbf{y}}_{\mathcal{A}}⟩=0\text{\hspace{1em}(VI-3-2-4)}$$
上式左边计算展开, 得到
⟨ z n , y n − y A ⟩ = ⟨ x n − x A ′ + K ‾ n ( y n − y A ) , y n − y A ⟩ = ⟨ x n − x A ′ , y n − y A ⟩ + ⟨ K ‾ n ( y n − y A ) , y n − y A ⟩ = T r { E [ ( x n − x ^ n ∣ n − 1 ) ( H n ( x n − x ^ n ∣ n − 1 ) + v t ) T ] } + T r { K ‾ n E [ ( y n − y A ) ( y n − y A ) T ] } = T r [ P n ∣ n − 1 H n T + K ‾ n ( H n P n ∣ n − 1 H n T + R n ) ] (VI-3-2-5) \begin{aligned} &\langle \mathbf{z}_n, \mathbf{y}_n - \mathbf{y}_{\mathcal{A}}\rangle \\ =& \langle \mathbf{x}_{\mathcal{n}} - \mathbf{x}_{\mathcal{A}}^{'} + \overline{\mathbf{K}}_n ({\mathbf{y}}_n - \mathbf{y}_{\mathcal{A}}) , \mathbf{y}_n - \mathbf{y}_{\mathcal{A}}\rangle\\ =& \langle \mathbf{x}_{\mathcal{n}} - \mathbf{x}_{\mathcal{A}}^{'} , \mathbf{y}_n - \mathbf{y}_{\mathcal{A}}\rangle + \langle \overline{\mathbf{K}}_n ({\mathbf{y}}_n - \mathbf{y}_{\mathcal{A}}) , \mathbf{y}_n - \mathbf{y}_{\mathcal{A}}\rangle \\ =&{\rm Tr} \{\mathbf{E}[( \mathbf{x}_{\mathcal{n}} -{\hat{\mathbf{x}}}_{n \vert n-1}) (\mathbf{H}_{n}(\mathbf{x}_{n} - \hat{\mathbf{x}}_{n\vert n-1}) + \mathbf{v}_t)^{\rm\scriptsize T} ] \}+{\rm Tr}\{ \overline{\mathbf{K}}_{n} \mathbf{E}[(\mathbf{y}_{n} - \mathbf{y}_{\mathcal{A}}) (\mathbf{y}_{n} - \mathbf{y}_{\mathcal{A}})^{\rm\scriptsize T}]\} \\ =& {\rm Tr}[ \mathbf{P}_{n \vert n-1} \mathbf{H}_{n}^{\rm \scriptsize T} +\overline{\mathbf{K}}_{n}( \mathbf{H}_{n} \mathbf{P}_{n \vert n-1} \mathbf{H}_{n}^{\rm\scriptsize T} + \mathbf{R}_n )] \end{aligned} \tag{VI-3-2-5} ====​⟨zn​,yn​−yA​⟩⟨xn​−xA′​+Kn​(yn​−yA​),yn​−yA​⟩⟨xn​−xA′​,yn​−yA​⟩+⟨Kn​(yn​−yA​),yn​−yA​⟩Tr{E[(xn​−x^n∣n−1​)(Hn​(xn​−x^n∣n−1​)+vt​)T]}+Tr{Kn​E[(yn​−yA​)(yn​−yA​)T]}Tr[Pn∣n−1​HnT​+Kn​(Hn​Pn∣n−1​HnT​+Rn​)]​(VI-3-2-5)
一个矩阵的迹为零的一个充分条件是该矩阵为零矩阵, 故式 (VI-3-2-4) 成立的充分条件是
$${\overline{\mathbf{K}}}_n=-{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}({\mathbf{H}}_n{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}+{\mathbf{R}}_n{)}^{-1}\text{\hspace{1em}(VI-3-2-6)}$$
与标准 Kalman Filter 中式 (II-5) 比较, 此处 ${\overline{\mathbf{K}}}_n$ 与式 (II-5) 中的 ${\mathbf{K}}_t$ 之间相差负号, 即
$$\begin{array}{rl}{\mathbf{K}}_n& =-{\overline{\mathbf{K}}}_n\\ & ={\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}({\mathbf{H}}_n{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}+{\mathbf{R}}_n{)}^{-1}\\ & ={\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{P}}_y^{-1}\end{array}\text{\hspace{1em}(VI-3-2-7)}$$
已知 ${\overline{\mathbf{K}}}_n$ 取式 (VI-3-2-6) 时, ${\mathbf{z}}_n\perp \mathcal{B}$. 变换等式 (VI-3-2-2) 得到
$$\begin{array}{r}{\mathbf{x}}_n={\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}+{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}({\mathbf{H}}_n{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}+{\mathbf{R}}_t{)}^{-1}({\mathbf{y}}_n-{\mathbf{y}}_{\mathcal{A}})+{\mathbf{z}}_n\end{array}\text{\hspace{1em}(VI-3-2-8)}$$
对式 (VI-3-2-8) 右手边每一项分析他们的所处几何状态:

- 第三项 ${\mathbf{z}}_n\in {\mathcal{B}}^{\perp }$

- 第一项 ${\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}\in \mathcal{A}\subset \mathcal{B}$

- 已知 ${\mathbf{y}}_n-{\mathbf{y}}_{\mathcal{A}}\in {\mathcal{A}}^{\perp }\subset \mathcal{B}$, 由推论 V-2 可知, 第二项 ${\mathbf{K}}_n({\mathbf{y}}_n-{\mathbf{y}}_{\mathcal{A}})\in {\mathcal{A}}^{\perp }\subset \mathcal{B}$.

由式 (VI-3-2-8) 各项几何状态, 我们定义 ${\mathbf{x}}_n$ 在子空间 $\mathcal{B}$ 上的正交投影所得向量 ${\mathbf{x}}_{\mathcal{B}}$. 本身属于 $\mathcal{B}$ 的向量在正交投影 ${\mathcal{P}}_{\mathcal{B}}(\cdot )$ 作用下不变, 而属于 ${\mathcal{B}}^{\perp }$ 的向量在正交投影 ${\mathcal{P}}_{\mathcal{B}}(\cdot )$ 作用下消失, 即
$$\begin{array}{rl}{\mathbf{x}}_{\mathcal{B}}& ={\mathcal{P}}_{\mathcal{B}}({\mathbf{x}}_n)\\ & ={\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}+{\mathbf{K}}_n({\mathbf{y}}_n-{\mathbf{y}}_{\mathcal{A}})\\ & ={\mathbf{x}}_{\mathcal{A}}^{{}^{\prime }}+{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}({\mathbf{H}}_n{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}+{\mathbf{R}}_t{)}^{-1}({\mathbf{y}}_n-{\mathbf{y}}_{\mathcal{A}})\end{array}\text{\hspace{1em}(VI-3-2-9)}$$
${\mathbf{x}}_{\mathcal{B}}$ 即为基于上一时刻的最优估计 ${\mathbf{x}}_{\mathcal{A}}$ 及当前时刻的测量 ${\mathbf{y}}_n$ 得到的当前时刻上的最优估计 (当前时刻 $t=n$). ${\mathbf{x}}_{\mathcal{B}}$ 也记作 ${\hat{\mathbf{x}}}_{n|n}$, 式 (VI-3-2-9) 可以写成
$${\hat{\mathbf{x}}}_{n|n}={\hat{\mathbf{x}}}_{n|n-1}+{\mathbf{K}}_n({\mathbf{y}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1})\text{\hspace{1em}(VI-3-2-10)}$$

几何推导得到的式 (VI-3-2-10) 与标准 Kalman Filter 更新方程 (II-5) 完全一致.

做到这里基本上已经实现基于正交投影的几何推导与标准 Kalman Filter 之间殊途同归了, 下面再验算一下更新阶段的误差协方差矩阵.

3.3. 误差协方差

$n=t$ 时刻, 估计值与真实值之间的误差为 ${\mathbf{ϵ}}_n={\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n}$. 误差协方差矩阵
$$\begin{array}{rl}{\mathbf{P}}_{n|n}& =\mathbf{E}[({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n})({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n}{)}^{\mathrm{T}}]\\ & =\mathbf{E}\left[\{({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n-1})-{\mathbf{K}}_n({\mathbf{y}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1})\}\{({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n-1})-{\mathbf{K}}_n({\mathbf{y}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1}){\}}^{\mathrm{T}}\right]\\ & =\mathbf{E}[({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n-1})({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n-1}{)}^{\mathrm{T}}]\\ & -\mathbf{E}[({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n-1})({\mathbf{y}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1}{)}^{\mathrm{T}}]{\mathbf{K}}_n^{\mathrm{T}}\\ & -{\mathbf{K}}_n\mathbf{E}[({\mathbf{y}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1})({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n-1}{)}^{\mathrm{T}}]\\ & +{\mathbf{K}}_n\mathbf{E}[({\mathbf{y}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1})({\mathbf{y}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1}{)}^{\mathrm{T}}]{\mathbf{K}}_n^{\mathrm{T}}\end{array}\text{\hspace{1em}(VI-3-3-1)}$$
我们分开计算,

- 根据式 (VI-3-1-5)可知, 式 (VI-3-2-10) 中的第一项为 ${\mathbf{P}}_{n|n-1}$.

- 式 (VI-3-2-10) 中的第二项为
$$\begin{array}{rl}& -\mathbf{E}[({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n-1})({\mathbf{y}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1}{)}^{\mathrm{T}}]{\mathbf{K}}_n^{\mathrm{T}}\\ =& -\mathbf{E}[({\mathbf{x}}_n-{\hat{\mathbf{x}}}_{n|n-1})({\mathbf{H}}_n{\mathbf{x}}_n+{\mathbf{v}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1}{)}^{\mathrm{T}}]{\mathbf{K}}_n^{\mathrm{T}}\\ =& -{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{K}}_n^{\mathrm{T}}\end{array}$$
- 式 (VI-3-2-10) 中的第三项为 $-{\mathbf{K}}_n{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}$.

- 式 (VI-3-2-10) 中的第四项为
$$\begin{array}{rl}& {\mathbf{K}}_n\mathbf{E}[({\mathbf{y}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1})({\mathbf{y}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1}{)}^{\mathrm{T}}]{\mathbf{K}}_n^{\mathrm{T}}\\ =& {\mathbf{K}}_n\mathbf{E}[({\mathbf{H}}_n{\mathbf{x}}_n+{\mathbf{v}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1})({\mathbf{H}}_n{\mathbf{x}}_n+{\mathbf{v}}_n-{\mathbf{H}}_n{\hat{\mathbf{x}}}_{n|n-1}{)}^{\mathrm{T}}]{\mathbf{K}}_n^{\mathrm{T}}\\ =& {\mathbf{K}}_n{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{K}}_n^{\mathrm{T}}+{\mathbf{K}}_n{\mathbf{R}}_n{\mathbf{K}}_n^{\mathrm{T}}\end{array}$$
这四项分别代入式 (VI-3-2-10) 得
$$\begin{array}{rl}{\mathbf{P}}_{n|n}=& {\mathbf{P}}_{n|n-1}-{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{K}}_n^{\mathrm{T}}-{\mathbf{K}}_n{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}+{\mathbf{K}}_n{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{K}}_n^{\mathrm{T}}+{\mathbf{K}}_n{\mathbf{R}}_n{\mathbf{K}}_n^{\mathrm{T}}\\ =& (\mathbf{I}-{\mathbf{K}}_n{\mathbf{H}}_n){\mathbf{P}}_{n|n-1}(\mathbf{I}-{\mathbf{K}}_n{\mathbf{H}}_n{)}^{\mathrm{T}}+{\mathbf{K}}_n{\mathbf{R}}_n{\mathbf{K}}_n^{\mathrm{T}}\end{array}\text{\hspace{1em}(VI-3-3-2)}$$
这是更新阶段误差协方差矩阵的一种形式.

由式 (VI-3-2-7) ${\mathbf{K}}_n={\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{P}}_y^{-1}$ . 又有相关协方差矩阵的对称性, 可知 ${\mathbf{K}}_n^{\mathrm{T}}={\mathbf{P}}_y^{-1}{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}$.

代入式 (VI-3-3-2) 得到
$$\begin{array}{rl}{\mathbf{P}}_{n|n}=& {\mathbf{P}}_{n|n-1}\\ & -{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}\underline{{\mathbf{P}}_y^{-1}{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}}\\ & -\underline{{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{P}}_y^{-1}}{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}\\ & +\underline{{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{P}}_y^{-1}}{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}\underline{{\mathbf{P}}_y^{-1}{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}}\\ & +\underline{{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{P}}_y^{-1}}{\mathbf{R}}_n\underline{{\mathbf{P}}_y^{-1}{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}}\\ =& {\mathbf{P}}_{n|n-1}\\ & -2{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{P}}_y^{-1}{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}\\ & +{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{P}}_y^{-1}\underline{\left({\mathbf{H}}_n{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}+{\mathbf{R}}_n\right)}{\mathbf{P}}_y^{-1}{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}\\ =& {\mathbf{P}}_{n|n-1}\\ & +2{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{P}}_y^{-1}{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}\\ & +{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{P}}_y^{-1}\underline{{\mathbf{P}}_y}{\mathbf{P}}_y^{-1}{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}\\ =& {\mathbf{P}}_{n|n-1}\\ & -2{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{P}}_y^{-1}{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}\\ & +{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{P}}_y^{-1}{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}\\ =& {\mathbf{P}}_{n|n-1}-\underline{{\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}{\mathbf{P}}_y^{-1}}{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}\\ =& {\mathbf{P}}_{n|n-1}-{\mathbf{K}}_n{\mathbf{H}}_n{\mathbf{P}}_{n|n-1}\end{array}\text{\hspace{1em}(VI-3-3-3)}$$
与标准 Kalman Filter 中更新阶段误差协方差矩阵 (II-4) 一致.

3.4. 卡尔曼增益

$t=n$ 时刻的代价函数为
$$\begin{array}{rl}{\mathbf{J}}_n& =\mathrm{Tr}({\mathbf{P}}_{n|n})\\ & =\mathrm{Tr}\left[(\mathbf{I}-{\mathbf{K}}_n{\mathbf{H}}_n){\mathbf{P}}_{n|n-1}(\mathbf{I}-{\mathbf{K}}_n{\mathbf{H}}_n{)}^{\mathrm{T}}+{\mathbf{K}}_n{\mathbf{R}}_n{\mathbf{K}}_n^{\mathrm{T}}\right]\end{array}\text{\hspace{1em}(VI-3-4-1)}$$
直接从代价函数也可以求出卡尔曼增益 ${\mathbf{K}}_n$,
$$\frac{\partial {\mathbf{J}}_n}{\partial {\mathbf{K}}_n}=-2(\mathbf{I}-{\mathbf{K}}_n{\mathbf{H}}_n){\mathbf{P}}_{n|n-1}{\mathbf{H}}_n^{\mathrm{T}}+2{\mathbf{K}}_n{\mathbf{R}}_n=0\text{\hspace{1em}(VI-3-4-2)}$$
一样可以计算得到式 (VI-3-2-7) 中的 ${\mathbf{K}}_n$.

这样我们就可以对卡尔曼滤波的关键项 —— 卡尔曼增益 Kalman gain 进行如下解读:

- 以信息融合的观点, 卡尔曼滤波增益 ${\mathbf{K}}_n$ 是对预测与测量融合过程中不确定性 (置信程度) 的反映.

- 以最优化的角度, 卡尔曼滤波增益 ${\mathbf{K}}_n$ 是最小二乘法意义下的最优解, 可以获得最小均方误差估计.

- 几何直观的解释, 卡尔曼滤波增益 ${\mathbf{K}}_n$ 是为了获得状态向量 ${\mathbf{x}}_n$ 在扩展子空间 $\mathcal{B}$ 上的正交投影 ${\mathbf{x}}_{\mathcal{B}}$.

到此完成归纳法第三步, 当 $t=n$ 时刻时, 利用几何正交投影获得的最优状态估计和标准 Kalman Filter 完全一致.
这就完成了对 Kalman Filter 的几何直观解读.

VII 总结

以上我们利用了 Hilbert 空间中正交投影的几何概念, 借助归纳法证明 (解读) 了卡尔曼滤波器最优估计的几何意义:

- 预测和更新均为正交投影意义下的最优估计;

- 新的测量是对原有子空间的扩展;

- 每一步状态估计都是在新扩展空间上的正交投影;

- 新息 (Innovation Variable) 在历史测量张成子空间的正交空间上, 可看做新的扩展维度上的向量基;

- 卡尔曼滤波增益 (Kalman Gain) 是为了获得状态向量在扩展子空间上的正交投影.

本文结束, 欢迎指正交流.

参考文献

[1] Ramsey Faragher, “Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation [Lecture Notes]”, IEEE Signal Processing Magazine, 2012

[2] Wolfram MathWorld, “Hilbert Space”, https://mathworld.wolfram.com/HilbertSpace.html

[3] Onno van Gaans, “Probability measures on metric spaces”, “https://www.math.leidenuniv.nl/~vangaans/jancol1.pdf”

[4] Yang Chen, Shaoshu Li, “A Brief Introduction to Hilbert Space”, https://sites.math.washington.edu/~farbod/teaching/cornell/math6210pdf/math6210Hilbert.pdf, 2016

[5] David G. Luenberger, “Optimization by Vector Space Methods”, Wiley-Interscience, 1969

[6] https://dornsife.usc.edu/assets/sites/1185/docs/507b/Linear.pdf