经久不衰的卡尔曼滤波器-基础知识及公式推导

发表于 更新于 分类于 ， 阅读次数： Changyan：

一、卡尔曼滤波器的应用场景

卡尔曼滤波器之前被广泛用来做动态系统的状态估计、预测。主要的目的就是用来从带噪声的观测量，比如各种传感器的观测（IMU、GPS、里程计等）估计出最优的系统状态（state）。不过要明确强调的是，由于测量都带有噪声，也就是随机性，所以真正准确的状态是无法获知的。

最小二乘法可以从一长串的测量值回归出一个最为匹配的模型。卡尔曼滤波相比于最小二乘法，采用了一种递归的计算方式，也就是每一时刻只需要保存上一时刻的状态。因此可以被用来处理实时任务。

虽然卡尔曼滤波器从美国阿波罗登月计划到如今已有几十年，且目前有更先进的因子图（Factor Graph）算法，但是在很多领域依旧可以看到卡尔曼滤波器的身影，特别是在自动驾驶的各个模块，比如：从IMU的数据（三轴加速度，三轴角速度）计算出运动物体的当前位置。

二、卡尔曼滤波器的两个步骤——宏观认识

卡尔曼滤波包含两个步骤

  1. 预测（prediction）—— Dynamic model
  2. 更新（correction/measurment update）—— Observation model

所谓预测，就是用一个数学模型，根据当前的传感器输入，直接计算此时系统的状态。可以理解为一个方程的计算就行。

所谓的更新，就是在某些时刻或者每一时刻，获取一些系统的状态输入（可以同样是传感器的值），甚至是预测阶段中同样的传感器的值，将其当作真值，我们将这个值叫做测量值。比较此刻预测的系统状态和测量的系统状态，对预测出的系统状态进行修正，因此也叫测量更新（measurment update）。

framework

三、卡尔曼滤波的几大概念

1、状态向量State Vector

系统的状态向量包含了系统中我们关心的状态变量，比如速度\(\textbf{v}=[v_x, v_y,v_z]\)，距离\(\textbf{d}=[d_x, d_y,d_z]\)，加速度\(\textbf{a}=[a_x,a_y,a_z]\)等。我们用\(\textbf{x}\)表示系统向量 \[ \textbf{x} = \begin{bmatrix} \textbf{d}\\ \textbf{v}\\ \textbf{a} \end{bmatrix}\\\]

因为卡尔曼滤波有两个步骤，我们先以每个时刻都进行测量和修正这两个步骤作为讲解。那么，每一时刻的两个步骤的输出对应两个系统向量，一个是预测的系统向量\(\textbf{x}^-\)，一个是修正后的系统向量\(\textbf{x}^+\)。

ps:大写字母表示矩阵；小写加粗字母表示向量；小写字母表示变量

2、状态方程&&预测方程

状态方程

状态方程描述了将上一时刻的状态向量\(\textbf{x}_{t-1}\)映射到当前的系统状态\(\textbf{x}_t\) \[ \textbf{x}_t = A_t\textbf{x}_{t-1}+ B_t\textbf{u}_t+\textbf{w}_t \tag{1}\]

其中，矩阵\(A\)称为转换矩阵（Transition Matrix），\(\textbf{u}_t\)是当前时刻的系统输入，矩阵\(B\)称为控制矩阵（Control Matrix）反映了系统输入到系统状态的映射关系，\(\textbf{w}_t\)是过程噪声，我们假定其符合均值为0，协方差矩阵为\(Q_t\)的高斯噪声。这里要注意的有几点：

  1. 矩阵\(A、B\)都随着时间演进进行更新
  2. 从公式1的形式可以看出，卡尔曼滤波的数学建模形式是线性方程，这也是卡尔曼滤波的限制之一。扩展卡尔曼滤波器（Extended Kalman Filter）支持非线性模型。
  3. 这里的状态方程是一个“准确”的表达，因为不准确的部分已经放在噪声项中，要跟下面的预测方程区分开来。这个“准确”的表达由于有噪声（随机向量），所以我们没办法使用它作为输出，只能用它来分析、推导。

预测方程

预测方程跟状态方程基本一样，要强调的一点是，噪声\(\textbf{w}_t\)是均值为0的高斯噪声，因此最大概率对应的值为0，因此我们在预测状态向量的时候其实不用管最后一项 \[ \hat{\textbf{x}}^-_t = \textbf{A}_t\hat{\textbf{x}}^+_{t-1} + \textbf{B}_t\textbf{u}_t\tag{2}\]

这里对系统状态向量上面加上一个帽子表示这是一个估计量，也就是我们实际输出的量。其中\(\hat{\textbf{x}}^+ _{t-1}\)表示上一时刻执行完步骤二更新后的状态向量，\(\hat{\textbf{x}}^- _{t}\)表示当前时候只执行了步骤1预测的状态向量。

理解预测方程

预测方程（公式2）从形式上看只是把状态方程（公式1）去掉了噪声项，但其中包含了很多含义。其中，要时刻记得的是我们把系统状态当作一个随机向量处理，严格的表述需要引入随机过程的相关知识，但其实没太大必要。由于公式（1）中状态向量是一个随机向量，且噪声项是均值为0的高斯噪声，那么状态向量\(\textbf{x}_t\)也满足高斯分布，其最大概率的地方就对应这噪声为0的地方，因此我们有理由将这个概率最大处对应的值当作我们的预测。

gaussian

3、测量方程&&测量估计方程

测量方程

测量方程反映了系统的测量值和系统状态向量之间的关系 \[ \textbf{z}_t=H_t\textbf{x}_t+v_t\tag{3}\]

其中，\(\textbf{z}_t\)是当前时刻的测量值，\(H_t\)称为测量矩阵（Measurement Matrix），描述了从系统状态到测量值的转换关系（举一个最简单的关系，系统状态是物体的直线距离，测量值是使用激光笔测出来的光从原点到物体的时间，那么\(H\)就是光速的倒数）, \(\textbf{v}_t\)是测量噪声，我们假定其符合均值为0，协方差为\(R_t\)的高斯噪声。注意这里无论是状态向量还是测量向量都没有加帽子（\(\hat{.}\)），表示这是个"准确"的表达式。

测量估计方程

同样，我们如果忽略噪声项，就变成了对此时测量量的估计（注意状态向量的上标和角标）：

\[ \hat{\textbf{z}}_t=H_t\hat{\textbf{x}}^-_t\tag{4}\]

4、测量更新方程

上面给出了对测量量的估计，但是现在的情况是，我们有了一个测量值（通过某些测量方式或者传感器数据），这是一个不需要计算得到的量，我们如何用这个测量得出的量来更新我们对状态向量的估计？这就涉及到测量更新方程，这也是卡尔曼滤波里最难的部分，这里先给出更新方程的形式。

\[ \hat{\textbf{x}}^+_t=\hat{\textbf{x}}^-_t+K_t(z_t-H_t\hat{\textbf{x}}^-_t)\tag{5}\] \[ K_t=\frac{P^-_tH_t^T}{H_tP^-_tH_t^T+R_t}\tag{6}\]

其中，\(K_t\)叫做卡尔曼增益；\(P^-_t\)是系统测量值\(\hat{\textbf{x}}^-_t\)的误差协方差矩阵。

四、误差和协方差矩阵

根据公式（1）减去公式（2），我们可以得出误差的协方差矩阵的表达形式

\[ e^-_t \triangleq \textbf{x}_t - \hat{\textbf{x}}^-_t\\\] \[ P^-_t = \mathbb{E}[\textbf{e}^-_t {\textbf{e}^{-}_{t}}^T]\\\]

把公式（1）和（2）带入误差表达式，可以推导出系统状态的协方差矩阵的递归表达形式

\[ \begin{split} P^-_t&=\mathbb{E}[\big((A_tx_{t-1}+w_t)-A_tx_{t-1}^+\big)\big((A_tx_{t-1}+w_t)-A_tx_{t-1}^+\big)^T]\\&=\mathbb{E}[\big(A_t(x_{t-1}-x_{t-1}^+)+w_t\big)\big(A_t(x_{t-1}-x_{t-1}^+)+w_t\big)^T]\\ &=\mathbb{E}[A_t(x_{t-1}-x_{t-1}^+)(x_{t-1}-x_{t-1}^+)^TA_t^T]+\mathbb{E}[w_tw_t^T]\\ &=A_tP_{t-1}^+A_t^T+Q_t \end{split}\\\]

其中，从第二个等号到第三个等号利用了状态向量和随机噪声 的无关性（协方差为0）。因此我们可以得到下面的公式（7）

\[P^-_t=A_tP^+_{t-1}A^T_t+Q_t\tag{7}\]

五、更新方程的推导

现在我们进行到了这样的情况，我们使用预测方程公式（2）得到了\(t\)时刻的系统状态预测量\(\textbf{x}_t^-\)；同时，我们在这个时刻得到了一个测量值，且根据测量方程，我们认为它与此时的状态向量满足公式（3）：\(\textbf{z}_t=H_t\textbf{x}_t+v_t\) 。那么其实我们可以从公式（3）也得到此时系统状态的另一个估计，由公式（3）将 移动到等式左侧，可以得到：

\[\textbf{x}^2_t = \textbf{H}_t^{-1}\textbf{z}_t\\\]

这里之所以没有把噪声\(\textbf{v}_t\)表示出来，是因为这里将 当作一个随机向量处理，其均值为\(\bar{z}_t\)，也就是当前时刻测量得出的量；其方差为\(R_t\)（在公式3中给出）

我们的状态方程也给出了状态向量的一个等式 \[ \textbf{x}^1_t = \textbf{A}_t\textbf{x}_{t-1}+ \textbf{B}_t\textbf{u}_t+w_t\]

这里用上标1和2表示状态向量的两个来源。要强调的是，这两个等式里面都包含了一个随机噪声，因此$^1_t 、^2_t $都是随机向量。同时，他们的分布都符合高斯分布。它们的均值和方差分别是：

\[ \begin{split} &\mu_1=x_t^-\\ &\sigma_1^2=P_t^-\\ &\mu_2=H_t^{-1}\bar{z}_t\\ &\sigma_2^2=H_t^{-1}R_t(H_t^{-1})^T\\ \end{split}\\\]

现在有了两个关于状态向量的概率分布，那接下来的事情就简单了。因为这两个状态向量的来源我们可以认为是独立的，因此他们的联合概率分布是各自概率分布的乘积。重点是，高斯分布的乘积依旧是高斯分布！！！！新的高斯分布的均值和方差有如下表达形式：

joint gaussian

用上面的式子代入，可以得到联合概率分布的均值和方差： 均值 \[ \begin{split} \mu_{fused}&=\frac{\mu_1\sigma_2^2+\mu_2\sigma_1^2}{\sigma_1^2+\sigma_2^2}\\ &=\mu_1+\frac{(\mu_2-\mu_1)\sigma_1^2}{\sigma_1^2+\sigma_2^2}\\ &=x_t^-+\frac{(H_t^{-1}\bar{z}_t-x_t^-)P_t^-}{P_t^-+H_t^{-1}R_t(H_t^{-1})^T}\\ &=x_t^-+\frac{P_t^-H_t^T}{H_tP_t^-H_t^T+R_t}(\bar{z}_t-H_tx_t^-)\\ &=x_t^-+K_t(\bar{z}_t-H_tx_t^-) \end{split}\\\]

方差

\[ \begin{split} \sigma_{fused}^2 &=\frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2}\\ &=\sigma_1^2-\frac{\sigma_1^4}{\sigma_1^2+\sigma_2^2}\\ &=P_t^--\frac{P_t^-H_t^TH_tP_t^-}{H_tP_t^-H_t^T+R_t}\\ &=P_t^--K_tH_tP_t^- \end{split}\\\]

这个\(\sigma_{fused}^2\)就是更新后的系统状态误差的协方差矩阵，也就是得到下面的式子

\[ P^+_{t}=P^-_t-K_tH_tP^-_t\tag{8}\]

六、总结

预测方程 \[ \hat{\textbf{x}}^-_t = \textbf{A}_t\hat{\textbf{x}}^+_{t-1} + \textbf{B}_t\textbf{u}(t)\tag{2}\] \[P^-_t=A_tP^+_{t-1}A^T_t+Q_t\tag{7}\]

更新方程 \[ \hat{\textbf{x}}^+_t=\hat{\textbf{x}}^-_t+K_t(z_t-H_t\hat{\textbf{x}}^-_t)\tag{5}\] \[ K_t=\frac{P^-_tH_t^T}{H_tP^-_tH_t^T+R_t}\tag{6} \] \[ P^+_{t}=P^-_t-K_tH_tP^-_t\tag{8} \]

转载须知

  1. 请于文章标题注明转载
  2. 请于文章起始位置注明原始链接
  3. 请在推文文末的【阅读原文】链接上原始链接
  4. 感谢~