卡尔曼滤波原理

卡尔曼滤波简介

论文: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)
$$

协方差

协方差用来描述两个变量在变化时的线性相关关系，如果一个变量在变大时另一个变量也在变大，即两个变量是同向变化的，此时协方差是正的。如果一个变量在变大时另一个变量在变小，则两个变量是反向变化的，此时协方差是负的。
协方差公式:
$$
\operatorname{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}+2 K_{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
$$
推导完毕。