卡尔曼滤波原理

卡尔曼滤波简介

论文:A New Approach to Linear Filtering and Prediction Problems（线性滤波与预测问题的新方法），Rudolf Emil Kalman，1960。
论文地址:http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf 。
卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。比如我们可以使用第k个时刻的状态信息输入卡尔曼滤波器来预测第k+1个时刻的状态信息，同时，第k+1个时刻的实际状态信息还可以用来更新卡尔曼滤波器的参数，使其在之后时刻的预测中变得更加准确。

基础知识

正态分布

又叫高斯分布。
若随机变量X服从一个位置参数为μ、尺度参数为σ的概率分布，且其概率密度函数为
$$f(x)=\frac{1}{\sqrt{2\pi }\sigma }\exp \left(-\frac{(x-\mu {)}^2}{2{\sigma }^2}\right)$$
则这个随机变量X就称为正态随机变量，正态随机变量服从的分布就称为正态分布，记作
$$X\sim N\left(\mu ,{\sigma }^2\right)$$
称随机变量X服从正态分布。
当μ=0，σ=1时的正态分布是标准正态分布。一般正态分布可以通过下式转化成标准正态分布:
$$X\sim N\left(\mu ,{\sigma }^2\right),Y=\frac{X-\mu }{\sigma }\sim N(0,1)$$

协方差

协方差用来描述两个变量在变化时的线性相关关系，如果一个变量在变大时另一个变量也在变大，即两个变量是同向变化的，此时协方差是正的。如果一个变量在变大时另一个变量在变小，则两个变量是反向变化的，此时协方差是负的。
协方差公式:
$$\mathrm{Cov}(X,Y)=E\left[\left(X-{\mu }_x\right)\left(Y-{\mu }_y\right)\right]$$

卡尔曼滤波公式

卡尔曼滤波模型假设k时刻状态的真实向量xk可以用下面的公式表示：
$$x_k=A{x}_{k-1}+B{u}_k+w_{k-1}$$
其中xk是系统在k时刻的真实状态向量，大小为nx1。A是状态转移矩阵（与xk-1即系统在k-1时刻的真实状态向量相乘），大小为nxn，矩阵A的值都是不变的常数。uk为k时刻的系统输入，大小为kx1。B是将输入转换为状态的矩阵，大小为nxk，矩阵B的值都是不变的常数。在大多数情况下，上式没有Buk项。
随机变量wk-1为过程噪声，假定wk-1符合均值为零，协方差矩阵为Q的多元正态分布，注意协方差矩阵Q的值也都是不变的常数。即:
$$w_{k-1}\sim N\left(0,Q\right)$$
上式表达的含义是，每时刻的真实向量xk都可以通过一个线性随机方程估计出来。
在k时刻，对真实状态向量xk的观测值向量zk满足下式：
$$z_k=H{x}_k+v_k$$
其中zk是k时刻的测量值向量，大小为mx1（注意不是nx1），H是状态向量到测量向量的转换矩阵，大小为mxn，矩阵H的值都是不变的常数。xk是k时刻的真实状态向量，大小为nx1。
随机变量vk是观测噪声，假定vk符合均值为零，协方差矩阵为R的多元正态分布，注意协方差矩阵R的值也都是不变的常数。即:
$$v_k\sim N\left(0,R\right)$$
上式表达的含义是，任何测量值zk都是信号值与测量噪声的线性组合。
总结一下，A、B、H、Q、R都是值不变的矩阵或向量，一旦模型建立好后，它们在之后的迭代过程中是不会变化的。
假设初始状态以及每一时刻的噪声{x0, w1, …, wk, v1 … vk}都认为是互相独立的。

大多数真实世界中的动态系统并不是一个严格的线性模型，系统中存在一定的不确定性，这会导致我们估计的系统状态矩阵xk存在偏差，这个真实值和估计值之间的偏差值我们用过程噪声wk-1来表示，且近似wk-1为高斯分布。另外，观测值往往也包含噪声和干扰，或者观测矩阵H自身存在观测偏差，这个真实值和观测值之间的偏差值用测量噪声vk表示，且近似vk为高斯分布。

卡尔曼滤波五个方程：时间更新方程组（两个，用于预测）以及测量更新方程组（三个，用于修正）。这两个方程组在滤波器运行的每一个时刻k都会执行。在预测阶段，卡尔曼滤波器使用上一时刻k-1的状态估计值，做出对当前时刻k的状态估计值。在更新阶段，滤波器利用对时刻k的状态观测值优化在预测阶段获得k时刻的预测值，以获得一个更精确的k时刻新估计值。

预测（两个时间更新方程组）:
$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_k$$
上式左边xk是在时刻k的状态预估计值。xk-1表示k-1时刻的状态估计值。uk为k时刻的系统输入，大小为kx1。B是将输入转换为状态的矩阵，大小为nxk。在大多数情况下，上式没有Buk项。
注意上面xk-和xk-1的含义区别：一个是k时刻的状态预估计值，一个是k-1时刻的状态估计值。
$$P_k^{-}=A{P}_{k-1}{A}^T+Q$$
上式左边Pk是k时刻的预估计误差协方差（度量状态估计值和真实值之间的误差），Pk-1表示的是k-1时刻的估计误差协方差。wk符合均值为零，协方差矩阵为Q的多元正态分布。
注意上面Pk-和Pk-1的含义区别：一个是k时刻的预估计误差协方差，一个是k-1时刻的估计误差协方差。
更新（三个测量更新方程组）:
$$K_k=P_k^{-}{H}^T{\left(H{P}_k^{-}{H}^T+R\right)}^{-1}$$
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k\left(z_k-H{\hat{x}}_k^{-}\right)$$
$$P_k=\left(I-K_{k}H\right)P_k^{-}$$
上面三个方程计算的分别是k时刻的卡尔曼增益Kk，k时刻的状态估计值xk，k时刻的估计误差协方差Pk。zk即k时刻的测量值。H是状态向量到测量向量的转换矩阵。vk符合均值为零，协方差矩阵为R的多元正态分布。I为单位矩阵。
迭代过程:
在时刻k时我们先计算上面预测的两个方程的值（分别称为k时刻的预估计值和k时刻的预估计误差协方差），有了这两个值就能计算k时刻的卡尔曼增益Kk，再然后得到k时刻的估计值和k时刻的估计误差协方差。这两个值在下一个时刻k+1时计算k+1时刻的预估计值和k+1时刻的预估计误差协方差时会继续用到。

卡尔曼滤波公式的推导过程

假设k时刻的真实状态向量xk为:
$$x_k=A{x}_{k-1}+B{u}_k+w_{k-1}$$
其中xk是系统在k时刻的真实状态向量，大小为nx1。A是状态转移矩阵（与xk-1即系统在k-1时刻的真实状态向量相乘），大小为nxn，矩阵A的值都是不变的常数。uk为k时刻的系统输入，大小为kx1。B是将输入转换为状态的矩阵，大小为nxk，矩阵B的值都是不变的常数。在大多数情况下，上式没有Buk项。
随机变量wk-1为过程噪声，假定wk-1符合均值为零，协方差矩阵为Q的多元正态分布，注意协方差矩阵Q的值也都是不变的常数。即:
$$w_{k-1}\sim N\left(0,Q\right)$$
上式表达的含义是，每时刻的真实向量xk都可以通过一个线性随机方程估计出来。
在k时刻，对真实状态向量xk的观测值向量zk满足下式：
$$z_k=H{x}_k+v_k$$
其中zk是k时刻的测量值向量，大小为mx1（注意不是nx1），H是状态向量到测量向量的转换矩阵，大小为mxn，矩阵H的值都是不变的常数。xk是k时刻的真实状态向量，大小为nx1。
随机变量vk是观测噪声，假定vk符合均值为零，协方差矩阵为R的多元正态分布，注意协方差矩阵R的值也都是不变的常数。即:
$$v_k\sim N\left(0,R\right)$$
上式表达的含义是，任何测量值zk都是信号值与测量噪声的线性组合。
总结一下，A、B、H、Q、R都是值不变的矩阵或向量，一旦模型建立好后，它们在之后的迭代过程中是不会变化的。
假设初始状态以及每一时刻的噪声{x0, w1, …, wk, v1 … vk}都认为是互相独立的。
下面三个变量的含义如下:
$$x_k，{\hat{x}}_k^{-}，{\hat{x}}_k$$
上面三个变量分别表示k时刻的状态真实值，k时刻的状态预估计值（先验估计值），k时刻的状态估计值（预估计值再经过调整后的估计值，又叫后验估计值）。
又已知:
$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_k$$
上式即卡尔曼滤波五个公式中的预测方程组中的第一个，求k时刻的状态预估计值（先验估计值）。
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k\left(z_k-H{\hat{x}}_k^{-}\right)$$
上式即卡尔曼滤波五个公式中的更新方程组中的第二个，求得k时刻的状态估计值（预估计值再经过调整后的估计值，又叫后验估计值）。

卡尔曼增益K表示过程误差与测量误差的比重，k的取值在[0, 1] 。即：
$$K=\frac{\text{预测误差}}{\text{预测误差+测量误差}}$$
当K=0时，即预测误差为0，系统的状态最优估计值完全取决与预测值；当K=1时，即测量误差为0，系统的状态值完全取决于测量值。K会随着迭代过程而逐步更新。
现在我们定义四个量:
$$e_k^{-}=x_k-{\hat{x}}_k^{-}$$
$$e_k=x_k-{\hat{x}}_k$$
$$P_k^{-}=E\left[e_k^{-}\ast e_k^{-T}\right]$$
$$P_k=E\left[e_k\ast e_k^T\right]$$
其中ek-称为先验状态误差（真实值与预估计值之间的误差），ek称为后验状态误差（真实值与估计值之间的误差）。Pk-为真实值与预估计值之间的协方差，Pk为真实值与估计值之间的协方差。
由下面两式:
$$z_k=H{x}_k+v_k$$
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k\left(z_k-H{\hat{x}}_k^{-}\right)$$
我们可推导出:
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k\left(H{x}_k+v_k-H{\hat{x}}_k^{-}\right)$$
继续化简，得:
$${\hat{x}}_k-x_k={\hat{x}}_k^{-}-x_k+K_{k}H\left(x_k-{\hat{x}}_k^{-}\right)+K_k{v}_k$$
将ek-和ek的表达式代入上式，得:
$$e_k=(I-K_{k}H)e_k^{-}-K_k{v}_k$$
Pk可化为:
$$P_k=E\left[e_k\ast e_k^T\right]=(I-K_{k}H)P_k^{-}(I-K_{k}H)-K_{k}R{K}_k^T$$
其中I代表单位矩阵，Rk即前面的测量噪声vk在k时刻的协方差矩阵。上面应用了转置矩阵的一个性质:
$$(A+B{)}^T=A^T+B^T$$
Pk可继续化为:
$$P_k=P_k^{-}-K_{k}H{P}_k^{-}-P_k^{-}{H}^T{K}^T+K_k\left(H{P}_k^{-}{H}^T+R\right)K_k^T$$
卡尔曼滤波的估计原则是使真实值与估计值之间的协方差Pk最小，使估计值越来越接近真实值。因此目标函数为:
$$J=\sum _{\min }{P}_k$$
上面的Pk表达式对卡尔曼增益矩阵K求偏导，并令偏导数为0:
$$\frac{\partial P_k}{\partial K_k}=-2{\left(H{P}_k^{-}\right)}^T+2K_k\left(H{P}_k^{-}{H}^T+R\right)=0$$
则当Pk取极小值时卡尔曼增益矩阵K的计算公式如下:
$$K_k=P_k^{-}{H}^T{\left(H{P}_k^{-}{H}^T+R\right)}^{-1}$$
又已知:
$${\hat{x}}_k-x_k={\hat{x}}_k^{-}-x_k+K_{k}H\left(x_k-{\hat{x}}_k^{-}\right)+K{v}_k$$
$$P_k=P_k^{-}-K_{k}H{P}_k^{-}-P_k^{-}{H}^T{K}_k^T+K_k\left(H{P}_k^{-}{H}^T+R\right)K_k^T$$
推导出真实值与估计值之间的协方差Pk的计算公式如下:
$$P_k=(I-K_{k}H)P_k^{-}$$
再由ek-的表达式:
$$e_k^{-}=x_k-{\hat{x}}_k^{-}$$
则ek+1-可化为:
$$e_{k+1}^{-}=x_{k+1}-{\hat{x}}_{k+1}^{-}=\left(A{x}_k+B{u}_k+w_k\right)-\left(A{\hat{x}}_k+B{u}_k\right)$$
$$e_{k+1}^{-}=A\left(x_k-{\hat{x}}_k\right)+w_k=A{e}_k+w_k$$
又已知:
$$P_k^{-}=E\left[e_k^{-}\ast e_k^{-T}\right]$$
我们来求k时刻的真实值与预测值之间的协方差PK-，上式可化为:
$$P_{k+1}^{-}=E\left[e_{k+1}^{-}\ast e_{k+1}^{-}T\right]=E\left[\left(A{e}_k+w_k\right){\left(A{e}_k+w_k\right)}^T\right]$$
$$P_{k+1}^{-}=E\left[\left(A{e}_k\right){\left(A{e}_k\right)}^T\right]+E\left[w_k{\left(w_k\right)}^T\right]$$
则k+1时刻的真实值与预测值之间的协方差Pk+1-为:
$$P_{k+1}^{-}=A{P}_k{A}^T+Q$$
推导完毕。