控制算法学习 一、卡尔曼滤波（1）以小车为例

前言

卡尔曼滤波（Kalman Filter, KF）是最经典的传感器滤波/去噪/估计/预测算法，适用于线性系统的最优状态估计，并且衍生出扩展卡尔曼滤波（EKF）、迭代卡尔曼滤波（IKF），无迹卡尔曼滤波（UKF）和容积卡尔曼滤波（CKF）等。

本文将非常简单的介绍和推导卡尔曼滤波，加深KF的理解记忆。

线性系统

什么是线性系统？控制领域中，系统输入输出满足叠加原理（叠加性和齐次性）就是线性系统。

一个简单的例子，一维路径上运行的匀速小车，时间（输入）和位置（输出）就是线性的。

状态方程和观测方程

如果只知道小车的初始速度，怎么获得小车在任一时刻的位置呢？

第一种方法是直接通过匀速计算：$p=v\ast t$，但是现实中路面可能会有崎岖，导致结果出现偏差；
第二种方法是在小车上加传感器（比如GPS），但GPS也会有测量误差；

因此以上两种方法就分别可以用状态方程和观测方程来表示：
$$\begin{array}{rl}& x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1},w_k\sim [0,Q]\\ & z_k=H{x}_k+v_k,v_k\sim [0,R]\end{array}$$
$A,B,H$分别表示状态转移矩阵，控制（输入）转换矩阵，观测转换矩阵。

$w_k,v_k$都是高斯白噪声，表示了系统误差（过程噪声）和测量误差。

$x_k,z_k$是向量，表明k时刻的系统状态。

举例

对于匀速运动的小车而言，可以这样建模：
状态方程
$${\left[\begin{array}{c}p\\ v\end{array}\right]}_t=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]{\left[\begin{array}{c}p\\ v\end{array}\right]}_{t-1}+w_{k-1}$$
观测方程（z是GPS读数）
$${\left[\begin{array}{c}z\end{array}\right]}_t=\left[\begin{array}{c}1\end{array}\right]{\left[\begin{array}{c}p\end{array}\right]}_t+v_k$$

卡尔曼滤波

KF有两个流程，先预测，再更新，其实就是把预测得到的状态与观测得到的状态，以某种方式进行加权更新，得到状态的最优估计。

状态预测

预测过程有两个公式：
$$\begin{array}{rlr}& {\tilde{x}}_k=A{x}_{k-1}+B{u}_{k-1}& (1)\\ & {\tilde{P}}_k=A{P}_{k-1}{A}^T+Q& (2)\\ & x_k=\left[\begin{array}{c}p\\ v\end{array}\right]P_k=\left[\begin{array}{cc}{\sigma }_{pp}& {\sigma }_{pv}\\ {\sigma }_{vp}& {\sigma }_{vv}\end{array}\right]& \end{array}$$
这里增加了一个状态协方差矩阵$P$（对称矩阵），用来描述状态量之间的关系。

这两个公式描述了：
(1) 将k-1时刻的状态$x_{k-1}$，通过状态转移矩阵和控制转换矩阵，预测k时刻的状态$x_k$;
(2) 将k-1时刻的状态协方差矩阵$P_{k-1}$，通过状态转移矩阵和过程噪声，预测k时刻的状态协方差矩阵$P_{k-1}$；

公式（1）容易理解，公式（2）需要从随机过程上解释：

$X=[x_1,x_2,\dots ,x_n{]}^T,Y=[y_1,y_2,\dots ,y_n{]}^T$，$X,Y$独立，则有$P(AX+Y)=A{P}_X{A}^T+P_Y$

还是用小车作为例子，公式（1）其实就是在用匀速运动模型，通过上一时刻小车的位置预测当前小车的位置。公式（2）则是通过上一时刻小车的速度和位置关系，预测当前速度和位置的关系，并且叠加过程噪声带来的不确定性$Q$

状态更新

更新过程有三个公式：
$$\begin{array}{rl}& K_k={\tilde{P}}_k{H}^T(H{P}_k{H}^T+R{)}^{-1}\\ & x_k={\tilde{x}}_k+K_k(z_k-H{\tilde{x}}_k)\\ & P_k=(E-K_{k}H){\tilde{P}}_k\end{array}$$
这里增加了卡尔曼增益$K_k$，实际上就是预测和观测权重。

这三个公式描述了：
（1）当前卡尔曼增益的计算；
（2）通过卡尔曼增益，进行预测\观测值的加权；
（3）通过卡尔曼增益，更新协方差矩阵；

本质上，以上三个公式是对预测状态和观测状态这两个高斯分布进行相乘，获得的高斯分布，就是最优状态估计。所谓的卡尔曼增益，就是预测状态的高斯分布与融合后的高斯分布的不确定性（协方差矩阵）的比值。

卡尔曼滤波流程图

最后用一张图来描述预测和更新过程：