必要的背景知识 II

在涉足多变量卡尔曼滤波之前，我们需要回顾一些必要的数学知识：

• 矩阵运算
• 期望的代数运算
• 多变量正态分布

如果对这些内容已经很熟悉了，可以直接跳到下一章。

本教程中所使用的符号注解：

• 粗体小写字母表示向量，例如 \( \boldsymbol{x} \)
• 粗体大写字母表示矩阵，例如 \( \boldsymbol{A} \)
• 普通小写字母表示标量或向量的分量
• 普通大写字母表示矩阵元素

获取本书

矩阵运算

你所需了解的是诸如下述的运算：

• 向量和矩阵的加法和乘法
• 矩阵转置
• 逆矩阵（不需要知道怎么求逆，只需要知道逆矩阵是什么即可）
• 对称矩阵
• 特征值和特征向量

很多线性代数书籍和在线教程都有上述知识点的讲解。

期望的代数运算

在推导卡尔曼滤波方程时会大量用到期望的代数运算。如果你感兴趣并想深入理解推导过程，就需要掌握期望代数。

我们已经知道什么是随机变量，以及什么是期望。如果还不清楚，请回顾必要的背景知识 I这一章。

基本期望代数运算法则

期望记为大写字母 \( E \).

随机变量的期望 \( E(X) \) 等于该随机变量的均值：

这里是一些基本的期望代数运算法则：

方差和协方差的期望的代数运算法则

下表总结了方差和协方差的期望的代数运算法则。

方差和协方差的期望的代数运算法则不是很直观。我在这里证明其中的一部分。

法则 8

\[ V(a) = 0 \]

一个常量是不会改变的，所以常量的方差是0.

法则 9

\[ V(a \pm X) = V(X) \]

给一个随机变量加上常量不会影响样本散布，故不会影响方差。

Rule 10

\[ V(X) = E(X^{2}) - \mu_{X}^{2} \]

证明：

法则 11

\[ COV(X,Y) = E(XY) - \mu_{X}\mu_{Y} \]

证明：

法则 13

\[ V(aX) = a^{2}V(X) \]

证明：

回忆前面章节中所提到的匀速运动模型：

法则 14

\[ V(X \pm Y) = V(X) + V(Y) \pm 2COV(X,Y) \]

证明：

多变量正态分布

简介

我们知道卡尔曼滤波的输出是一个随机变量，该随机变量的均值描述了状态的估计，方差描述了状态估计的不确定性。即卡尔曼滤波同时提供了状态估计和状态估计的可信程度。

一维卡尔曼滤波方程包含四个描述不确定性的变量：

• \( p_{n,n} \) 是当前估计值的方差
• \( p_{n+1,n} \) 是（对下一时刻）预测值的方差
• \( r_{n} \) 是测量值的方差
• \( q \) 是过程噪声

对多变量卡尔曼滤波而言，系统状态是通过向量描述的。比如平面上运动的物体需要两个变量来描述：x方向位置和y方向位置：

获取本书

\[ \boldsymbol{x} = \left[ \begin{matrix} x\\ y\\ \end{matrix} \right] \]

此时卡尔曼滤波的输出就是一个 多维随机变量。它的不确定性通过 协方差矩阵 描述。

多变量卡尔曼滤波中描述不确定性的变量对应为：

• \( \boldsymbol{P}_{n,n} \) 是描述估计值不确定性的协方差矩阵
• \( \boldsymbol{P}_{n+1,n} \) 是描述预测值不确定性的协方差矩阵
• \( \boldsymbol{R}_{n} \) 是描述测量值不确定性的协方差矩阵
• \( \boldsymbol{Q} \) 是描述过程噪声的协方差矩阵

本章讲述 多变量正态分布 和其协方差矩阵。

协方差

协方差是对两个或两个以上随机变量的相关程度的度量。

假设给定 \( x-y \) 平面上一个物体的一系列测量值。

由于存在随机误差，这些测量值存在方差。

来看看几种不同的测量值分布示意：

上两个图描述的是不存在相关的测量值分布。\( x \) 的取值不依赖于 \( y \). 蓝色的测量分布图中 \( x \) 和 \( y \) 具有相同的方差，所以整个样本分布的形状大致是个圆。对于红色的测量分布图，\( x \) 的分布方差比 \( y \) 更大，因此样本分布形状是个椭圆。

由于两个方向上的测量没有关联，因此 \( x \) 和 \( y \) 的协方差是0.

下两个图描述的是存在相关的测量值分布，即\( x \) 和 \( y \) 之间存在相关性。绿色的分布图中 \( x \) 取较大值时对应的 \( y \) 取值也较大，反之亦然，因此具有正相关性，故协方差也为正。而青色的分布图中 \( x \) 取较大值时对应的 \( y \) 取值会较小，反之亦然，因此具有负相关性，故协方差也为负。

对 \( N \) 个给定的 \( X \) 和 \( Y \) 的总体（译注：总体 Population，指某个变量对应的全量样本，即所有可能取值的集合），其之间的协方差如下计算：

\[ COV(X,Y) = \frac{1}{N}\sum_{i=1}^{N}(x_{i} - \mu_{x})(y_{i} - \mu_{y}) \]

把总体协方差公式变换一下：

\( N \) 个样本（译注：样本 Sample，是总体的一个真子集）的协方差以 \( N-1 \) 来归一化：

\[ COV(X,Y) = \frac{1}{N-1}\sum_{i=1}^{N}(x_{i} - \mu_{x})(y_{i} - \mu_{y}) \]

把样本协方差公式变换一下：

示例：

给定样本：

\[ \boldsymbol{x} = \left[ \begin{matrix} 2 & 3 & -1 & 4 \end{matrix} \right] \]

\[ \boldsymbol{y} = \left[ \begin{matrix} 8 & 7 & 9 & 6 \end{matrix} \right] \]

\[ COV(X,Y) = \frac{1}{N-1}\sum_{i=1}^{N}(x_{i}y_{i}) - \frac{N}{N-1}\mu_{x}\mu_{y} = \]

\[ = \frac{1}{3} \left( 2 \times 8 + 3 \times 7 - 1 \times 9 + 4 \times 6 \right) - \frac{4}{3} \left( \frac{(2+3-1+4)}{4} \frac{(8+7+9+6)}{4} \right) = -2.67 \]

我们可以保留向量写法，此时协方差以向量形式写出：

\[ COV(X,Y) = \frac{1}{N-1}\boldsymbol{x}\boldsymbol{y}^{T} - \frac{N}{N-1}\mu_{x}\mu_{y} \]

对于零均值的随机变量，其协方差简化为：

\[ COV(X,Y) = \frac{1}{N-1}\boldsymbol{x}\boldsymbol{y}^{T} \]

协方差矩阵

协方差矩阵是一个方阵，描述一系列随机变量两两之间的协方差。

对于一个二维随机变量，协方差矩阵为：

\[ \boldsymbol{\Sigma} = \left[ \begin{matrix} \sigma_{xx} & \sigma_{xy} \\ \sigma_{yx} & \sigma_{yy} \\ \end{matrix} \right] = \left[ \begin{matrix} \sigma_{x}^{2} & \sigma_{xy} \\ \sigma_{yx} & \sigma_{y}^{2} \\ \end{matrix} \right] = \left[ \begin{matrix} VAR(\boldsymbol{x}) & COV(\boldsymbol{x, y}) \\ COV(\boldsymbol{y, x}) & VAR(\boldsymbol{y}) \\ \end{matrix} \right] \]

注意该矩阵中的非对角元素是相等的，因为 \( COV(\boldsymbol{x, y}) = COV(\boldsymbol{y, x}) \). 如果 \( x \) 和 \( y \) 相互独立，则非对角元素是0.

对 \( n \) 维随机变量，其协方差矩阵为：

\[ \boldsymbol{\Sigma} = \left[ \begin{matrix} \sigma_{1}^{2} & \sigma_{12} & \cdots & \sigma_{1n} \\ \sigma_{21} & \sigma_{2}^{2} & \cdots & \sigma_{2n} \\ \vdots & \vdots & \ddots & \vdots \\ \sigma_{n1} & \sigma_{n2} & \cdots & \sigma_{n}^{2}\\ \end{matrix} \right] \]

大多数数学计算工具都能计算协方差矩阵。

import numpy as np

x = np.array([2, 3, -1, 4])
y = np.array([8, 7, 9, 6])

C = np.cov(x,y)
print(C)

[[ 4.66666667 -2.66666667]
 [-2.66666667  1.66666667]]
												

x = [2 3 -1 4];
y = [8 7 9 6];

C = cov(x,y)

C =

    4.6667   -2.6667
   -2.6667    1.6667
												

协方差矩阵的性质

1. 协方差矩阵的对角元素即为每个多维随机变量分量的方差。

\[ \boldsymbol{\Sigma}_{ii} = \sigma^{2}_{i} \]

2. 由于对角元素非负，协方差矩阵的迹（对角元素之和）也非负：

\[ tr(\boldsymbol{\Sigma}) = \sum^{n}_{i=1}\boldsymbol{\Sigma}_{ii} > 0 \]

3. 因为 \( \boldsymbol{\Sigma}_{ij} = \sigma_{ij} = \sigma_{ji} = \boldsymbol{\Sigma}_{ji} \), 协方差矩阵是对称阵：

\[ \boldsymbol{\Sigma} = \boldsymbol{\Sigma}^{T} \]

4. 协方差矩阵是半正定矩阵。
   如果对于任何向量 \( \boldsymbol{v} \neq 0 \)，都有 \( \boldsymbol{v}^{T}\boldsymbol{A}\boldsymbol{v} \geq 0 \)，则矩阵 \( \boldsymbol{A} \) 称为半正定矩阵
   \( \boldsymbol{A} \) 的特征值均为非负。

协方差矩阵和期望

给定一个有 \( k \) 个元素的向量 \( \boldsymbol{x} \)：

\[ \boldsymbol{x} = \left[ \begin{matrix} x_{1}\\ x_{2}\\ \vdots \\ x_{k}\\ \end{matrix} \right] \]

\( \boldsymbol{x} \) 的协方差矩阵为：

证明：

\[ COV(\boldsymbol{x}) = E \left( \left[ \begin{matrix} (x_{1} - \mu_{x_{1}})^{2} & (x_{1} - \mu_{x_{1}})(x_{2} - \mu_{x_{2}}) & \cdots & (x_{1} - \mu_{x_{1}})(x_{k} - \mu_{x_{k}}) \\ (x_{2} - \mu_{x_{2}})(x_{1} - \mu_{x_{1}}) & (x_{2} - \mu_{x_{2}})^{2} & \cdots & (x_{2} - \mu_{x_{2}})(x_{k} - \mu_{x_{k}}) \\ \vdots & \vdots & \ddots & \vdots \\ (x_{k} - \mu_{x_{k}})(x_{1} - \mu_{x_{1}}) & (x_{k} - \mu_{x_{k}})(x_{2} - \mu_{x_{2}}) & \cdots & (x_{k} - \mu_{x_{k}})^{2} \\ \end{matrix} \right] \right) = \]

\[ = E \left( \left[ \begin{matrix} (x_{1} - \mu_{x_{1}}) \\ (x_{2} - \mu_{x_{2}}) \\ \vdots \\ (x_{k} - \mu_{x_{k}}) \\ \end{matrix} \right] \left[ \begin{matrix} (x_{1} - \mu_{x_{1}}) & (x_{2} - \mu_{x_{2}}) & \cdots & (x_{k} - \mu_{x_{k}}) \end{matrix} \right] \right) = \]

\[ = E \left( \left( \boldsymbol{x} - \boldsymbol{\mu}_{x} \right) \left( \boldsymbol{x} - \boldsymbol{\mu}_{x} \right)^{T} \right) \]

多变量正态分布

我们已经熟悉了 单变量正态分布，它通过一个钟形的高斯曲线描述：

\[ p(x|\mu,\sigma) = \frac{1}{\sqrt{2\pi\sigma^{2}}} exp \left( -\frac{(x-\mu)^{2}}{2\sigma^{2}} \right)\]

正态分布记为：

\[ \mathcal{N}(\mu,\sigma^{2})\]

多变量正态分布是单变量正态分布在多维随机变量情况时的推广。

\( n \) 维多变量正态分布记为：

\[ p(\boldsymbol{x}|\boldsymbol{\mu},\boldsymbol{\Sigma}) = \frac{1}{\sqrt{(2\pi)^{n}|\boldsymbol{\Sigma}|}} exp \left( -\frac{1}{2}(\boldsymbol{x} - \boldsymbol{\mu})^{T}\boldsymbol{\Sigma}^{-1}(\boldsymbol{x} - \boldsymbol{\mu}) \right)\]

式中：

• \( \boldsymbol{x} \) 是 \( n \) 维随机向量
• \( \boldsymbol{\mu} \) 是 \( n \) 维均值向量
• \( \boldsymbol{\Sigma} \) 是 \( n \times n \) 协方差矩阵

双变量正态分布

双变量（二维）正态分布描述了两个具有正态分布的随机变量。我想围绕双变量正态分布进行后续讲述，因为二维是我们所能可视化的最高的维度了。

下图是二维高斯函数的图像：

置信区间

置信区间描述了一个样本落到单变量正态分布的均值附近的概率。

对单变量正态分布，高斯函数在 \( \mu \pm 1\sigma \) 区间内的面积是全部面积的 68.26%.

对单变量正态分布，有如下的性质：

• 68.26% 对应的置信区间是 \( 1\sigma \).
• 95.44% 对应的置信区间是 \( 2\sigma \).
• 99.74% 对应的置信区间是 \( 3\sigma \).

我们还能推导出单变量正态分布时任意概率对应的置信区间大小，具体推导参见这里。

双变量正态分布的概率密度函数以一个二维高斯函数的围成体积来描述。

例如，二维高斯函数在 \( 1\sigma \) 对应的水平切片内部围成的体积是围成总体积的39.35%.

二维高斯函数的水平切片向下下投影形状为一个椭圆。

协方差椭圆

首先，我们看看协方差椭圆的性质。协方差椭圆是高斯分布的一条特殊等高线，使我们能以二维形式展示 \( 1\sigma \) 置信区间，从而从几何角度直观解释协方差矩阵。

任何椭圆可以由四个参数描述：

• 椭圆心 \( \mu_{x}, \mu_{y} \)
• 半长轴 \( a \)
• 半短轴 \( b \)
• 朝向角 \( \theta \)

椭圆心是随机变量的均值：

\[ \mu_{x} = \frac{1}{N}\sum^{N}_{i=1}x_{i} \]

\[ \mu_{y} = \frac{1}{N}\sum^{N}_{i=1}y_{i} \]

椭圆半长轴和半短轴长度是对应随机变量协方差矩阵的特征值的平方根：

• 半长轴长度 \( a \) 是最大的特征值平方根
• 半短轴长度 \( b \) 是第二大的特征值的平方根

椭圆的朝向由随机变量协方差矩阵的特征向量给出：

\[ \theta = arctan \left( \frac{v_{x}}{v_{y}} \right)\]

式中：

• \( v_{x} \) 是协方差矩阵对应最大特征值的特征向量的 \( x \) 分量
• \( v_{y} \) 是协方差矩阵对应最大特征值的特征向量的 \( y \) 分量

使用计算工具可以计算出协方差椭圆的各项参数：

import numpy as np

C = np.array([[5, -2],[-2, 1]]) # define covariance matrix

eigVal, eigVec = np.linalg.eig(C) # find eigenvalues and eigenvectors

a = np.sqrt(eigVal[0]) # half-major axis length
b = np.sqrt(eigVal[1]) # half-minor axis length

# ellipse  orientation  angle
theta = np.arctan(eigVec[1, 0] / eigVec[0, 0])  
												

C = [5 -2; -2 1]; % define covariance matrix

[eigVec, eigVal] = eig(C); % find eigenvalues and eigenvectors


if eigVal(1,1) > eigVal(2,2) % get the highest eigenvalue index

    a = sqrt(eigVal(1,1)); % half-major axis length
    b = sqrt(eigVal(2,2)); % half-minor axis length

    theta = atan(eigVec(2,1) / eigVec(1,1));  % ellipse angle (radians)
else

    a = sqrt(eigVal(2,2)); % half-major axis length
    b = sqrt(eigVal(1,1)); % half-minor axis length

    theta = atan(eigVec(2,2) / eigVec(2,1));  % ellipse angle (radians)
end
												

置信椭圆

在许多应用场景里，人们希望找到某个特定概率在概率密度函数上对应的边界。例如对95%的概率，我们希望找到一条边界，使其围成的体积为整个二维高斯函数围成体积的95%。

这个边界投影到 \( x-y \) 平面上就是置信椭圆（译注：从这里看出置信椭圆一定是一条二维高斯函数的等高线）。

我们希望找到一个 椭圆放缩系数 \( k \)，它将标准的协方差椭圆（\( 1\sigma \)）放缩到我们想要的95%概率所对应的置信椭圆。

由于 \( \sigma_{x} \) 和 \( \sigma_{y} \) 对应相互独立的随机变量的标准差，那么可以引入卡方分布(chi-square)这样一个新的定理来说明一个置信椭圆和其内部对应的概率之间的关系：

\[ p = 1 - exp \left( -\frac{1}{2}k^{2} \right) \]

对于一个协方差椭圆 \( k = 1 \)，与之对应的概率为：

\[ p = 1 - exp \left( -\frac{1}{2} \right) = 39.35\% \]

则对于任意概率，我们可以找到一个椭圆放缩系数：

\[ k = \sqrt{-2ln(1-p)} \]

特别地，对95%的概率：

\[ k = \sqrt{-2ln(1-0.95)} = 2.45\]

95%置信椭圆的性质有：

• 椭圆心 \( (\mu_{x}, \mu_{y}) \) 和协方差椭圆一样
• 朝向角 \( \theta \) 和协方差椭圆一样
• 半长轴长度是 \( 2.45a \) - 经过缩放的协方差椭圆的半长轴长度
• 半短轴长度是 \( 2.45b \) - 经过缩放的协方差椭圆的半短轴长度