卡尔曼滤波§

Abstract

这更偏向于是一篇笔记, 而不是教程. 直接看会对其中的一些说明感到困惑.

1. 数学基础§

在学习这一经典的控制算法前, 需要掌握一些基础的数学概念, 包括矩阵和线性变换, 方差, 协方差, 高斯分布与贝叶斯思想.

1.1 矩阵与线性变换§

在线性代数的学习中我们知道, 如果一个向量可以表示系统的状态, 那么矩阵就能够通过乘法运算将这个状态从一个空间坐标变换到另一个空间坐标, 即: \(\overrightarrow{X_t} = {A} \overrightarrow{X_0}\). 这一过程用于描述系统状态的演化或对测量信息的处理. 在卡尔曼滤波中, 我们规定两个矩阵:

状态转移矩阵 \(F\): 用于预测的矩阵, 通过上一时刻的状态线性地预测下一时刻的状态.
观测矩阵 \(H\): 用于将状态向量映射到测量空间.

1.2 方差与协方差§

方差表示的是一个数据的离散程度, 而协方差表示的是两个数据之间的关联程度. 二者的计算公式为:

方差 \(\text{Var}(X) = \frac{1}{n-1}\sum_{i=1}^n(x_i-\overline x)^2\)
协方差 \(\text{Cov}(X,Y)=\frac{1}{n-1}\sum_{i=1}^n(x_i-\overline x)(y_i-\overline y)\)

Note

关于方差和协方差的理解, 有一种"看面积"的有趣方式. 我们假设把坐标系的原点移动到均值点 \((\overline x, \overline y)\), 对于每一个样本点 \((x_i,y_i)\), 作垂线到 \(x\) 轴与 \(y\) 轴可以得到一个矩形, 该矩形:

"长"为 \((x_i - \overline x)\), "宽"为 \((y_i - \overline y)\), 面积为 \((x_i - \overline x)(y_i - \overline y)\). 根据所在点的位置, 矩形的"面积"带符号, 且在第I, III 象限时面积为正, 在 II, IV 象限时面积为负.

而协方差的本质就是将这些矩形的"面积"加起来求和. 如果 \(X\) 与 \(Y\) 强相关, 那么最终结果的绝对值将是一个极大的正数; 若几乎不相关, 即均匀分布在四个象限, 那么最终结果将趋近于 \(0\).

方差是协方差的一个特例, 即将 \(Y\) 也变成 \(X\), 此时只可能分布在 I, III 象限, 可以通过计算面积的大小确定 \(X\)(以 \(\overline x\) 为基准)的离散程度.

1.3 高斯分布与贝叶斯思想§

高斯分布存在一个特殊之处, 即经过线性变换后方差与均值的计算结果就是原本的方差与均值的线性变换.换言之, 已知值 \(x\sim N(\mu, \sigma^2)\), 对其进行线性变换 \(y=ax+b\), 有:

\[ E(y)=a\mu+b\ \ \ \ \ \text{Var}(y)=a^2\sigma^2 \]

对于均值, 这一结论是显然的; 对于方差, 同样可以利用面积的方式进行直观的理解.

根据贝叶斯定理, 我们知道新证据的诞生只能在旧证据的基础上作用得出新概率, 或者说先验概率与似然函数的乘积与后验概率成正比, 即:

\[ P(x|z)=\frac{P(z|x)P(x)}{P(z)} \]

卡尔曼滤波的更新过程恰好满足这一条件. 在卡尔曼滤波中, \(P(x)\) 表示的是先验概率, 即在时间层面上以前积累的经验推断出的当前应当的值的可信度, 而 \(P(z|x)\) 表示的是似然函数, 即根据测量结果对于这个先验概率的可靠程度进行修正(即测量结果是 \(x\) 的概率有多大), 二者相乘就是后验概率, 即结合了经验与实际测量结果推断出的结果的可靠程度.

Note

关于贝叶斯定理,这里推荐观看 3Blue1Brown 对于贝叶斯定理的直觉化说明, 讲解的远比这里描述的清晰.

2. 卡尔曼滤波的数学推导§

首先状态空间方程有:

\[ \begin{align} &X_k=AX_{k-1}+BU_{k-1}+W_{k-1}\\ &Z_k=HX_k+V_{k} \end{align} \]

其中, \(X_k\) 指在 k 时刻的空间状态(实际值), \(X_{k-1}\) 指在 \(k-1\) 时刻的空间状态, \(A\) 指在时域上从 \(k-1\) 时刻过度到 \(k\) 时刻空间状态之间的关系矩阵, \(U_{k-1}\) 指在 \(k-1\) 时刻额外输入给空间状态的影响量, \(B\) 指这一影响量对空间状态 \(X_k\) 的作用关系转换矩阵, \(W_{k-1}\) 指因为模型本身考虑不全面等问题造成的数学模型与现实值之间的偏差矩阵, \(Z_k\) 指在 \(k\) 时刻直接测得的值, 其中 \(X_k\) 指在 \(k\) 时刻的空间状态, \(H\) 指传感器与观测系统之间的关系转换矩阵, \(V_k\) 指在 \(k\) 时刻因为测量仪器精度等问题产生的偏差矩阵.