Kalman Filter 卡尔曼滤波1

卡尔曼滤波介绍

卡尔曼滤波顾名思义是一种滤波方法，可以用于线性滤波和非线性滤波。卡尔曼滤波分为很多类，主要应用于线性滤波的是一般卡尔曼滤波，应用于非线性的有延申卡尔曼滤波（Extended Kalman Filter） 、无损卡尔曼滤波（Unscented Kalman Filter） 、以及**粒子滤波器(Particle Kalman Filter)**。

这篇主要说一说啥是一般卡尔曼滤波。

卡尔曼滤波的基本模型

基本假设

假设现在有线性的离散时间系统：

$$
\begin{equation}\label{1}
\begin{aligned}
x_ {k} &=F_ {k-1} x_ {k-1}+G_ {k-1} u_ {k-1}+w_ {k-1}\\
\\
y_ {k} &=H_ {k} x_ {k}+v_ {k}
\end{aligned}
\end{equation}
$$

系统固有的

对于这个系统描述方程，各个矩阵的描述如下：

$F_ {k-1}$： 系统转化矩阵，将系统状态从时刻间$k-1$转化到时刻$k$时的状态。

$G_ {k-1}$:系统控制矩阵，转化外部施加的控制量$u_k$ 到系统中。

$H_k$ :测量转化矩阵，将系统状态量转化成可测量量，比如电路中某个原件承载电压值这个状态量，可以通过流经此原件的电流和原件的电阻值乘积得到，$H_k$的作用就是将某些状态量转化到可直接测量的测量空间去。

$y_k$ :时刻$k$时系统的测量值。

$w_ {k-1}$:在$k-1$时刻系统的噪音，也是矩阵。

$v_ {k-1}$: 在k-1时刻对系统观测时产生的噪音干扰.

噪音

其中的$w_k$ 和 $v_k$ 都是零均值、不相关的白噪音（即为一般的高斯白噪音），而且二者都有已知的协方差矩阵$Q_k$ 和$R_k$ ,其满足的分布分别为：

$$
\begin{equation}\label{2}
\begin{aligned}
w_ {k} & \sim\left(0, Q_ {k}\right) \\
\\
v_ {k} & \sim\left(0, R_ {k}\right) \\
\\
E\left[w_ {k} w_ {j}^{T}\right] &=Q_ {k} \delta_ {k-\jmath} \\
\\
E\left[v_ {k} v_ {j}^{T}\right] &=R_ {k} \delta_ {k-j} \\
\\
E\left[v_ {k} w_ {\jmath}^{T}\right] &=0
\end{aligned}
\end{equation}
$$

上式中， $\sim(a,b)$ 符号表示的是“服从均值为$a$,方差为$b$的高斯分布” , $E\left[w_ {k} w_ {j}^{T}\right]$ 表示的是括号内二者的联合期望，$\delta_{k-j}$ 则是Kronecker delta function的函数，其简明意义是k=j的时候$\delta_ {k-j}= 1$，反之则为$\delta_ {k-j} = 0$。

注：知识复习 $Cov[X,Y] = E[XY] - E[X]E[Y]$,对于均值为 0的高斯分布后两项为0

上式表明，对于噪音的协方差矩阵，$w_k$和$v_k$ 都是对角矩阵，而二者也是独立的( $E[v_ {k} w_ {j}^{T}]=0$)。

$Q_ {k}$表示系统状态受到的噪音扰动的协方差：

$$
\begin{equation}\label{3}
\begin{aligned}
Q_k &=E\left(w_k w_k^{T}\right) \\
\\
&=\left[\begin{array}{ccc}
\sigma_ {1}^{2} & \cdots & 0 \\
\vdots & & \vdots \\
0 & \cdots & \sigma_ {k}^{2}
\end{array}\right]
\end{aligned}
\end{equation}
$$

$R_ {k}$ 代表测量误差的协方差：

$$
\begin{equation}\label{4}
\begin{aligned}
R_k &=E\left(v_k v_k^{T}\right) \\
\\
&=\left[\begin{array}{ccc}
\sigma_ {1}^{2} & \cdots & 0 \\
\vdots & & \vdots \\
0 & \cdots & \sigma_ {k}^{2}
\end{array}\right]
\end{aligned}
\end{equation}
$$

制定一个小目标 – get next time state $x_k$

对于式子$(1)$来说，卡尔曼滤波的总体目标是估计状态量$x_k$ , 那么如何估计呢？这就要用到我们已知的系统动力学性质和对系统的测量量$y_k$ 。这里变量的角标$k$代表的是各个离散的时刻。如果我们现在知道所有$k$时刻之前以及$k$时刻的的测量量$y_1,y_2,y_3 …y_k$，那么我们就可以估计一个对现在时刻的状态的估计状态，称之为后验估计（posteriori estimate）（顾名思义，相当于知道后再估计、检验）。

$$
\begin{equation}\label{5}
\hat{x}_ {k}^{+}=E\left[x_ {k} \mid y_ {1}, y_ {2}, \cdots, y_ {k}\right]=a\text {posteriori estimate}
\end{equation}
$$

注意，这里的上角标加号表示的是后验，意思是估计所用的依赖信息包含**时刻$k$的测量值的出来的估计量。

同理，我们可以得到先验估计（priori estimate）:

$$
\begin{equation}\label{6}
\hat{x}_ {k}^{-}=E\left[x_ {k} \mid y_ {1}, y_ {2}, \cdots, y_ {k-1}\right]=a \text { priori estimate }
\end{equation}
$$

以上两个估计都是在已经获得了测量结果的时刻$k$以及之前时刻的估计，那么如果我们有了$k$个时刻的样本，以及$k$时刻之后的$N$个样本，这时候我们对$k$时刻的估计值则称之为平滑估计 (smoothed estimate) ：
$$
\begin{equation}\label{7}
\hat{x}_ {k \mid k+N}=E\left[x_ {k} \mid y_ {1}, y_ {2}, \cdots, y_ {k}, \cdots, y_ {k+N}\right]=\text { smoothed estimate }
\end{equation}
$$
平滑估计之所以称之为smoothed，原因在于已经知道了想要估计时刻之后的$N$个样本的测量值。

最后我们想要预测$k$时刻的系统状态量，但是我们目前手上只有$k$时刻之前$M$步到初始时刻的测量量，那么这时候我们利用这些测量量做出的估计称之为预测估计（predicted estimate）：
$$
\begin{equation}\label{8}
\hat{x}_ {k \mid k-M}=E\left[x_ {k} \mid y_ {1}, y_ {2}, \cdots, y_ {k-M}\right]=\text { predicted estimate }
\end{equation}
$$
一张图帮你弄懂这些估计量代表的含义：

预测类型

上图表示，如果我们现在只有五个测量值拿在手里，那么对第一个时刻的估计值是smoothed estimate，对第五个时刻的估计值是posteriori estimate，对第六个时刻的估计值是 a priori estimate, 对第九个时刻的估计值是prediction。其实，a priori estimate 也是prediction，只不过由于和已知时刻的测量值的时刻挨得比较近。

卡尔曼滤波的基本原则从这里也可以略知一二，其大概总的原理，就是在$k$时刻即将到来之前，我们基于$k$时刻以前对系统的测量值，对系统的状态做出一个$k$时刻的预测值，也就是$\hat{x}{k}^{-}$ ，当$k$时刻到来的时候，我们反手一掏，拿到了$k$时刻的测量值，再结合之前时刻的测量值，这时候就得到了$k$时刻的后验估计值$\hat{x} {k}^{+}$，而实现循环预测的基本原则则是，我们利用前一时刻的后验估计值，“扔到”前一时刻的系统里，得出来后一时刻的先验。

简而言之，就是我们在下一时刻到来前，利用现在时刻我们所有拥有的信息，估计出来系统的后验值(不管先验后验，都是估计值哦，不是真实值)，放到现在时刻的系统里面，经过神奇的运作，我们就得到了未经下一时刻测量值证实的一个对系统状态的猜测和估计，这个估计就是下一时刻系统状态的先验（理论上讲，系统下一时刻的实际值，应该是这一时刻的实际值放在下一时刻的系统中，才能得到下一时刻的实际值，对于估计而言，我们利用这种方法对下一时刻系统进行估计）。

来个示意图:

时刻转化

用方程表示则为：
$$
\begin{equation}
\label{9}
\hat{x}{k}^{-} = F {k-1} \hat{x}{k-1}^{+} + G {k-1} u_{k-1}
\end{equation}
$$
上式称之为更新公式，非常形象，通过系统对预测状态更新，有那味儿了，但是初始状态是怎么样的呢？往前推：
$$
\begin{equation}\label{10}
\hat{x}_ {1}^{-}=F_{0} \hat{x}_ {0}^{+}+G_{0} u_ {0}
\end{equation}
$$
可以看出来，我们必须要拥有一个初始的后验值，这个后验值我们直接用初始时刻的测量值$\hat{x}_ {0}^{+} = E(x_0)$ 。

评价小目标的达成度 $P_k$

说了这么多，我们到此得到了我们的目标，即$k$时刻的系统状态估计值。但是到目前为止，我们并不知道我们对系统各个时刻的估计是否正确，或者说有多大的置信度 ，于是我们引入一个专门估计各个时刻估计置信度的变量$P$ ，即系统估计值错误的方差 。

$$
\begin{equation}\label{11}
\begin{array}{l}
P_{k}^{-}=E\left[\left(x_ {k}-\hat{x}_ {k}^{-}\right)\left(x_ {k}-\hat{x}_ {k}^{-}\right)^{T}\right] \\
\\
P_ {k}^{+}=E\left[\left(x_ {k}-\hat{x}_ {k}^{+}\right)\left(x_ {k}-\hat{x}_ {k}^{+}\right)^{T}\right]
\end{array}
\end{equation}
$$

对于零时刻：

$$
\begin{equation}\label{12}
\begin{aligned}
P_ {0}^{+} &=E\left[\left(x_ {0}-\bar{x}_ {0}\right)\left(x_ {0}-\bar{x}_ {0}\right)^{T}\right] \\
\\
&=E\left[\left(x_ {0}-\hat{x}_ {0}^{+}\right)\left(x_ {0}-\hat{x}_ {0}^{+}\right)^{T}\right]
\end{aligned}
\end{equation}
$$

其中$\bar{x}_ 0$ 表示的时零时刻系统状态的均值，表示为：

$$
\begin{equation}\label{13}
\begin{aligned}
\bar{x}_ {k} &=E\left(x_ {k}\right) \\
&=F_ {k-1} \bar{x}_ {k-1}+G_ {k-1} u_ {k-1}
\end{aligned}
\end{equation}
$$
然后结合公式$\eqref{10}$对状态值的递推估计，我们可以直接利用其思想得到状态值方差的递推式：

$$
\begin{equation}\label{14}
P_ {k}^{-}=F_ {k-1} P_ {k-1}^{+} F_ {k-1}^{T}+Q_ {k-1}
\end{equation}
$$
但其实$\eqref{14}$ 来源于状态值和状态方差的扩散，即：
$$
\begin{equation}\label{15}
P_k = F_{k-1} P_ {k-1} F_{k-1}^{T}+Q_ {k-1}
\end{equation}
$$
而公式$\eqref{15}$来源于状态递推：
$$
\begin{equation}\label{16}
\begin{aligned}
\bar{x}{k} &=E\left(x {k}\right) \\
\\
&=F_{k-1} \bar{x}_ {k-1}+G_{k-1} u_ {k-1}
\end{aligned}
\end{equation}
$$
然后我们结合公式$\eqref{1}$和公式$\eqref{14}$可以得到中间的一个公式：
$$
\begin{equation}\label{17}
\begin{aligned}
\left(x_{k}-\bar{x}_ {k}\right)(\cdots)^{T} &=\left(F_{k-1} x_ {k-1}+G_{k-1} u_ {k-1}+w_{k-1}-\bar{x}_ {k}\right)(\cdots)^{T} \\
\\
&=\left[F_ {k-1}\left(x_ {k-1}-\bar{x}_ {k-1}\right)+w_ {k-1}\right][\cdots]^{T} \\
\\
&=F_{k-1}\left(x_ {k-1}-\bar{x}{k-1}\right)\left(x {k-1}-\bar{x}{k-1}\right)^{T} F {k-1}^{T} \\
\\
&+ w_{k-1}w_ {k-1}^{T} + F_{k-1}(x_ {k-1}-\bar{x}{k-1})w {k-1}^{T} \\
\\
&+ w_{k-1}\left(x_ {k-1}-\bar{x}{k-1}\right)^{T} F {k-1}^{T}
\end{aligned}
\end{equation}
$$
最终我们可以得到公式$P_k$:
$$
\begin{equation}\label{18}
\begin{aligned}
P_{k} &=E\left[\left(x_ {k}-\bar{x}_ {k}\right)(\cdots)^{T}\right] \\
\\
&=F_ {k-1} P_{k-1} F_ {k-1}^{T}+Q_ {k-1}
\end{aligned}
\end{equation}
$$

经过上述一系列推导，我们可以知道的是，当 $P_{k}$ 越小的时候，不管是 $P_ {k}^{+}$ 还是$P_ {k}^{-}$ ，都表明估计值的不确定性越小，也就是说其估计错误越小，估计值越准确。

估计增益矩阵 $K_k$ – the Estimator Gain Matrix

接下来是卡尔曼滤波的重头戏了，估计增益矩阵 ，正是这个参数，将时刻间的估计值和测量值联系起来。

首先对于线性系统言，$K_ k$ 可以表示对测量值和估计值的信任系数。

让我们去掉上下脚标来看状态系统更新。
$$
\begin{equation}\label{19}
\begin{aligned}
y_{k} &=H_ {k} x+v_ {k} \\
\end{aligned}
\end{equation}
$$

$$\begin{equation}\label{20}
\hat{x}_ {k} =\hat{x}_ {k-1}+K_ {k}\left(y_ {k}-H_ {k} \hat{x}_ {k-1}\right)
\end{equation}$$

公式$\eqref{20}$ 表明$k$ 时刻的状态估计值由$k-1$时刻的状态估计值加上与实际状态测量值的差值然后乘以一个系数得到的，这个系数就是时刻$k$的估计增益矩阵，也称为卡尔曼增益。

对于带角标的系统更新方程：
$$
\begin{equation}\label{21}
\hat{x}{k}^{+}=\hat{x} {k}^{-}+K_{k}\left(y_ {k}-H_{k} \hat{x}_ {k}^{-}\right)
\end{equation}
$$

再来一个图表示：

卡尔曼增益(图中z即为y)

卡尔曼增益的推导比较繁琐，这里不再仔细推导，但是其最终形式有很多种体现，其中比较经常使用的有：
$$
\begin{equation}\label{22}
K_{k}=P_ {k}^{-} H_{k}^{T}\left(H_ {k} P_{k}^{-} H_ {k}^{T}+R_ {k}\right)^{-1} \\
\end{equation}
$$

$$
\begin{equation}\label{23}
K_ {k}=P_ {k}^{+} H_ {k}^{T} R_ {k}^{-1}
\end{equation}
$$

公式$\eqref{22}$ 相比于公式$\eqref{23}$ 而言，只需要先验估计，但是却需要求加和矩阵的逆，而矩阵的逆并不一定存在。

离散卡尔曼滤波算法

最后让我们总结一下卡尔曼滤波算法

第一步，系统模型为：
$$
\begin{equation}\label{24}
\begin{aligned}
x_{k} &=F_ {k-1} x_{k-1}+G_ {k-1} u_{k-1}+w_ {k-1} \\
\\
y_{k} &=H_ {k} x_{k}+v_ {k} \\
\\
E\left(w_{k} w_ {j}^{T}\right) &=Q_{k} \delta_ {k-\jmath} \\
\\
E\left(v_{k} v_ {j}^{T}\right) &=R_{k} \delta_ {k-\jmath} \\
\\
E\left(w_{k} v_ {\jmath}^{T}\right) &=0
\end{aligned}
\end{equation}
$$
第二步，初始化滤波模型初始状态：
$$
\begin{equation}\label{25}
\begin{aligned}
\hat{x}{0}^{+} &=E\left(x {0}\right) \\
\\
P_{0}^{+} &=E\left[\left(x_ {0}-\hat{x}_ {0}^{+}\right)\left(x_ {0}-\hat{x}_ {0}^{+}\right)^{T}\right]
\end{aligned}
\end{equation}
$$
第三步，递推：
$$
\begin{equation}\label{26}
\begin{aligned}
P_ {k}^{-} &=F_{k-1} P_ {k-1}^{+} F_{k-1}^{T}+Q_ {k-1} \\
\\
K_{k} &=P_ {k}^{-} H_{k}^{T}\left(H_ {k} P_{k}^{-} H_ {k}^{T}+R_{k}\right)^{-1}\\
&=P_ {k}^{+} H_{k}^{T} R_ {k}^{-1} \\
\\
\hat{x}{k}^{-} &=F {k-1} \hat{x}{k-1}^{+}+G {k-1} u_{k-1}=\text { a priori state estimate } \\
\\
\hat{x}_ {k}^{+} &=\hat{x}{k}^{-}+K {k}\left(y_{k}-H_ {k} \hat{x}{k}^{-}\right)=\text {a posteriori state estimate } \\
\\
P {k}^{+} &=\left(I-K_{k} H_ {k}\right) P_{k}^{-}\left(I-K_ {k} H_{k}\right)^{T}+K_ {k} R_{k} K_ {k}^{T} \\
&=\left[\left(P_ {k}^{-}\right)^{-1}+H_ {k}^{T} R_ {k}^{-1} H_ {k}\right]^{-1} \\
&=\left(I-K_{k} H_ {k}\right) P_ {k}^{-}
\end{aligned}
\end{equation}
$$
打完收工，下次说卡尔曼滤波的性质。