【飞控理论】从零开始学习Kalman Filters之二：最优状态估计、最优估计算法和方程

---

  本文是博主学习笔记，点击这里观看原视频。

---

前言

  在上次的学习中我们学习了卡尔曼滤波器的常见用途和状态观测器的建立从而将现实问题抽象成数学问题，在接下来的文章中会介绍卡尔曼滤波器是如何在这些用途中发挥作用的。

---

学习目录

从零开始学习Kalman Filters之一：Kalman Filters的常见用途、什么是状态观测器？
从零开始学习Kalman Filters之二：最优状态估计、最优估计算法和方程
从零开始学习Kalman Filters之三：非线性状态估算器
从零开始学习Kalman Filters之四：卡尔曼滤波C语言代码实现

---

1、开始之前先需要知道的几个概念

  概率密度：零基础的同学推荐戳这里。在数学中，连续型随机变量的概率密度函数(在不至于混淆时可以简称为密度函数)是一个描述这个随机变量的输出值，在某个确定的取值点附近的可能性的函数。而随机变量的取值落在某个区域之内的概率则为概率密度函数在这个区域上的积分。当概率密度函数存在的时候,累积分布函数是概率密度函数的积分。概率密度函数一般以小写标记。

  协方差：零基础的同学推荐戳这里。协方差( Covariance )在概率论和统计学中用于衡量两个变量的总体误差。而方差是协方差的一种特殊情況,即当两个变量是相同的情況。协方差表示的是两个变量的总体的误差,这与只表示一个变量误差的方差不同。如果两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值，另外一个也大于自身的期望值,那么两个变量 之间的协方差就是正值。如果两个变量的变化趋势相反，即其中一个大于自身的期望值,另外一个却小于自身的期望值,那么两个变量之间的协方差就是负值。

注：$Var(X)$即$D(X)$
  
  高斯分布即正态分布记为:
$$X\sim N(\mu ,{\sigma }^2)$$
  其中$\mu$是均值$\sigma$是标准差，其概率密度函数为：
$$f(x|\mu ,\sigma )=\frac{1}{\sqrt{2\pi {\sigma }^2}}{e}^{-\frac{{(x-\mu )}^2}{2{\sigma }^2}}$$
  期望和方差分别为：
$$E(x)=\mu$$
$$D(x)={\sigma }^2$$

---

2、最优状态估计

  现在有一场自动驾驶汽车设计大赛。要求车里使用GPS传感器来测量它的位置。你的车将会在100种不同的地形上各行驶1公里。每次都必须尽可能停靠在终点线。比赛结束时，计算每支队伍的平均最终位置，具有最小的误差方差和平均距离最接近1公里的参赛者会获得大奖。
  如果你仅仅依赖于GPS的原始数据的话你可能输的很惨。现在我们需要一种算法来优化我们所得到的数据从而估算汽车的位置。
  汽车行驶过程中的数学模型是十分复杂的，这里我们假设一个简单的理想模型：

  其中：
  $u_k$是速度
  $y_k$是汽车的位置
  $x_k$是系统状态由于我们设定的系统十分简单于是这里将$x_k$也定义为汽车的位置，于是顺利成章的$C$也就是常数1了。
  但是上述模型是一个理想模型，实际上无论是GPS的数据还是环境干扰都会具有噪声，于是有以下的实际模型：

  $v_k$表示GPS数据中的测量噪声。
  $w_k$表示风的影响和速度变化产生的过程噪声。
  我们假设这些噪声即随机变量遵循高斯分布（正态分布）。
$$v\sim N(0,R)$$
$$w\sim N(0,Q)$$
  由于我们的单输出系统协方差$R$也就是该随机变量的方差同时也是一个标量，对于过程噪声的随机变量$w$也是一样。
  上面的模型是真实的模型，在汽车的理想模型中现在采用估计值来建立一个新的理想模型用以推算，这时采用估计值，从而使卡尔曼滤波器发挥作用。

  现在我们的任务是求出估计值$\hat{x}$，下图为$x$的概率密度函数，$\hat{x}$最可能在这个分布的平均值附近，如果直接按照理想模型中的公式求解的话，$\hat{x}$的值k值的逐渐增大时也变得更加不准确，因为在实际运动的过程中噪声也在叠加起干扰作用。

  接下来我们将使用GPS的数据也就是测量数据来矫正预估值偏差逐渐增大的问题。可以看到下图的橙色部分就是使用GPS得到的汽车位置数据的概率密度函数曲线，它受到测量噪声的影响从而显示为这个状态。我们将这两个概率密度函数相乘从而得到灰色的概率密度函数曲线，这时候取这个概率密度函数的均值为估计值，同时这个估计值的方差小于先前的估计值的方差。

  这便是卡尔曼滤波器背后的基本思想，接下来我们要做的便是实施该算法。

---

3、最优估计算法和方程

  在上面的例子中，最后我们说你可以通过使用卡尔曼滤波器来赢得比赛。它能计算最优无偏差汽车位置预估值而且方差最小。计算最优值首先将预测概率密度函数和测量概率密度函数相乘，使得范围缩小然后计算概率密度函数的均值。

  (1)为状态观测器方程;(2)为卡尔曼滤波器方程：
$${\hat{x}}_{k+1}=A{\hat{x}}_k+B{u}_k+K(y_k-C{\hat{x}}_k)\text{\hspace{1em}(1)}$$
$${\hat{x}}_k=A{\hat{x}}_{k-1}+B{u}_k+K_k(y_k-C(A{\hat{x}}_{k-1}+B{u}_k))\text{\hspace{1em}(2)}$$
  实际上卡尔曼滤波器就是一种状态观测器，但它是为随机系统设计的。这就是卡尔曼方程和我们讨论的概率密度函数之间的关系。
  第一部分预测当前状态，它使用了前一个时间步的估算状态以及当前输入，我们用${\hat{x}}_k^{-}$来表示预测状态这也称为先验状态估计其中${}^{-}$表示先验，因为这是使用当前测量值之前计算的。

  我们将等式重写可以获得下面这个式子。

  等式的第二部分使用测量值，代入方程来更新预估值。我们将结果称为后验状态估计$\hat{x}$。

  由此可以定义先验估计误差$e_k^{-}$和后验估计误差$e_k$：
$$e_k^{-}\equiv x_k-{\hat{x}}_k^{-}$$
$$e_k\equiv x_k-{\hat{x}}_k$$
  卡尔曼滤波算法可以分为两部分，我们先从算法的第一步预测部分也就是先验状态估计${\hat{x}}_k^{-}$和先验估计误差的协方差$P_k^{-}$的计算开始。

$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_k\text{\hspace{1em}(3)}$$
$$P_k^{-}=A{P}_{k-1}{A}^T+Q\text{\hspace{1em}(4)}$$
  (3)和(4)组成的系统模型用于计算先验状态估计${\hat{x}}_k^{-}$和先验估计误差的协方差$P_k^{-}$，对于单系统来说$P_k^{-}$就是先验估计误差的方差。在算法的最开始，${\hat{x}}_k^{-}$和$P_k^{-}$来自初始估计值。
$$K_k=\frac{P_k^{-}{C}^T}{C{P}_k^{-}{C}^T+R}\text{\hspace{1em}(4)}$$
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(y_k-C{\hat{x}}_k^{-})\text{\hspace{1em}(5)}$$
$$P_k=(I-K_{k}C)P_k^{-}\text{\hspace{1em}(6)}$$
  算法的第二步由(4)、(5)、(6)组成，使用在预测步骤中计算得到的${\hat{x}}_k^{-}$，更新后验状态估计${\hat{x}}_k$和后验估计误差的协方差$P_k$，调整卡尔曼增益$K_k$，使得更新后的$P_k$最小。通过式子(5)可以看出通过调整修正项， 卡尔曼增益$K_k$对测量值$y_k$和${\hat{x}}_k^{-}$对计算${\hat{x}}_k$的影响程度。
  下面我们通过两个极端的例子来看卡尔曼增益$K_k$对${\hat{x}}_k$的影响。
  当测量噪声的协方差R接近于零时，卡尔曼增益接近C的倒数，在我们的系统中等于1。这个时候将$K_k$代入(5)式中运算可得${\hat{x}}_k=y_k$，这个时候${\hat{x}}_k$仅仅取决于测量值$y_k$。

  当预估误差的协方差$P_k^{-}$接近于零时，可以发现卡尔曼增益$K_k$也为0。这个时候将$K_k$代入(5)式中运算可得${\hat{x}}_k=x_k^{-}$，这个时候${\hat{x}}_k$仅仅取决于$x_k^{-}$。

  而后我们将更新后的值重新代入预测部分继而重复。这就是卡尔曼滤波器实现递归的原因。
  其实卡尔曼滤波器也被称为传感器融合算法，所以如果你想在这场比赛中胜出你可以购买更多的传感器来提升数据精度例如IMU。如果你有两个测量值，$y_k,C,K_k$的矩阵维度将发生变化这个时候式子(5)将会变为以下式子：
$${\hat{x}}_{k_{[1\times 1]}}={\hat{x}}_{{}_{k_{[1\times 1]}}}^{-}+K_{k_{[1\times 2]}}(y_{k_{[2\times 1]}}-C_{[2\times 1]}{\hat{x}}_{{}_{k_{[1\times 1]}}}^{-})$$

  从图上看我们将多出一个概率密度函数来表示IMU的测量，这次我们将三个概率密度函数相乘，来找到汽车位置的最优估算值。

---