（二）. 细说Kalman滤波：The Kalman Filter

转载请注明出处，http://www.cnblogs.com/ycwang16/p/5999034.html

前面介绍了Bayes滤波方法，我们接下来详细说说Kalman滤波器。

本节内容也主要参考《Probabilistic Robotics》的Kalman滤波一章，增加了很多讲解。

虽然Kalman滤波器已经被广泛使用，也有很多的教程，但我们在Bayes滤波器的框架上，来深入理解Kalman滤波器的设计，对理解采用Gaussian模型来近似状态分布的多高斯滤波器(Guassian Multi-Hyperthesis-Filter)等都有帮助。

一. 背景知识回顾

1.1 Bayes滤波

首先回顾一下Bayes滤波. Bayes滤波分为两步：1.状态预测；和 2.状态更新

1. 状态预测，基于状态转移模型：

$overline {bel} ({x_t}) = int {p({x_t}|{u_t},{x_{t - 1}})} ;bel({x_{t - 1}});d{x_{t - 1}}$

2. 状态更新，基于新的观测

$bel({x_t}) = ;eta \,p({z_t}|{x_t})\,overline {bel} ({x_t})$

我们可以看到，我们的目的是计算$x_t$的后验概率，如果$bel({x_t})$是任意分布，我们需要在$x_t$的所有可能取值点上，计算该取值的概率，这在计算上是难于实现的。这一计算问题可以有多种方法来近似，比如利用采样的方法，就是后面要讲的粒子滤波和无迹Kalman滤波。

这节要说的近似方法是，当假设$bel({x_t})$服从Gauss分布，那么我们只需要分布的均值和方差就可以完全描述$bel({x_t})$，而无需在$x_t$的每个可能取值点上进行概率计算。这也是用高斯分布来近似$bel({x_t})$的好处，因为我们在每一个时刻，只需要计算均值$mu_t$和方差$Sigma_t$这两个数值，就可以对$bel({x_t})$完全描述，所以我们就可以推导出这两个数值的递推公式，从而在每个时刻由这两个数值的递推公式完全获得状态估计，这就是The Kalman Filter的基本思想。

1.2 正态分布(Guassian Distribution)

然后我们再回顾一下正态分布的基础知识。正态分布是一种特殊的概率分布，分布的形态完全由二阶矩决定。一元高斯分布表述如下：

$$egin{array}{l}
p(x)sim N(mu ,{sigma ^2}):\
p(x) = frac{1}{{sqrt {2pi } sigma }}{e^{ - frac{1}{2}frac{{{{(x - mu )}^2}}}{{{sigma ^2}}}}}
end{array}$$

其中，一阶矩为均值$mu$表示期望值，二阶矩为方差$sigma$表示分布的不确定程度。

多元高斯分布的表达式为：

$$egin{array}{l}
p({f{x}})sim { m N}({f{mu }},{f{Sigma }}):\
p({f{x}}) = frac{1}{{{{(2pi )}^{d/2}}{{left| {f{Sigma }} ight|}^{1/2}}}}{e^{ - frac{1}{2}{{({f{x}} - {f{mu }})}^t}{{f{Sigma }}^{ - 1}}({f{x}} - {f{mu }})}}
end{array}$$

同样，一阶矩为$f{mu}$表示各元变量的期望值，二阶矩为方差矩阵$f{Sigma}$表示各元变量的不确定程度。

1.3 正态分布的特点

在线性变换下，一旦高斯，代代高斯。

首先，高斯变量线性变换后，仍为高斯分布，均值和方差如下：

$left. {egin{array}{*{20}{l}}
{Xsim N(mu ,Sigma )}\
{Y = AX + Bquad ;}
end{array}} ight}quad  Rightarrow quad Ysim N(Amu  + B,ASigma {A^T})$

然后，两个高斯变量线性组合，仍为高斯分布，均值和方差如下：

$left. {egin{array}{*{20}{c}}
{{X_1}sim N({mu _1},sigma _1^2)}\
{{X_2}sim N({mu _2},sigma _2^2)}
end{array}} ight} Rightarrow p({X_1} + {X_2})sim Nleft( {{mu _1} + {mu _2},sigma _1^2 + sigma _2^2 + 2 ho {sigma _1}{sigma _2}} ight)$

最后，两个相互独立的高斯变量的乘积，仍然为高斯分布，均值和方差如下：

$left. {egin{array}{*{20}{c}}
{{X_1}sim N({mu _1},sigma _1^2)}\
{{X_2}sim N({mu _2},sigma _2^2)}
end{array}} ight} Rightarrow p({X_1}) cdot p({X_2})sim Nleft( {frac{{sigma _2^2}}{{sigma _1^2 + sigma _2^2}}{mu _1} + frac{{sigma _1^2}}{{sigma _1^2 + sigma _2^2}}{mu _2},quad frac{{sigma _1^2sigma _2^2}}{{sigma _1^2 + sigma _2^2}}} ight)$

正因为高斯分布有这些特点，所以，在Bayes滤波公式中的随机变量的加法、乘法，可以用解析的公式计算均值和方差，这使得Bayes滤波的整个计算过程非常简便，即Kalman滤波器的迭代过程。

二. Kalman滤波

2.1 Kalman滤波的模型假设

Kalman滤波所解决的问题，是对一个动态变化的系统的状态跟踪的问题，基本的模型假设包括：1）系统的状态方程是线性的；2）观测方程是线性的；3）过程噪声符合零均值高斯分布；4）观测噪声符合零均值高斯分布；从而，一直在线性变化的空间中操作高斯分布，状态的概率密度符合高斯分布。

1. 状态方程
   ${x_t} = {A_t}{x_{t - 1}} + {B_t}{u_t} + {varepsilon _t}$
2. 观测方程
   ${z_t} = {H_t}{x_t} + {delta _t}$

其中过程噪声${varepsilon _t}$假设符合零均值高斯分布；观测噪声${delta _t}$假设符合零均值高斯分布。对于上述模型，我们可以用如下参数描述整个问题：

2.2 Kalman滤波器的模型

1. $x_t$，$n$维向量，表示$t$时刻观测状态的均值。
2. $P_t$，$n*n$方差矩阵，表示$t$时刻被观测的$n$个状态的方差。
3. $u_t$，$l$维向量，表示$t$时刻的输入
4. $z_t$，$m$维向量，表示$t$时刻的观测
5. ${A_t}$，$n*n$矩阵，表示状态从$t-1$到$t$在没有输入影响时转移方式
6. ${B_t}$，$n*n$矩阵，表示$u_t$如何影响$x_t$
7. ${H_t}$，$m*n$矩阵，表示状态$x_t$如何被转换为观测$z_t$
8. ${R_t}$，$n*n$矩阵，表示过程噪声${varepsilon _t}$的方差矩阵
9. $Q_t$，$m*m$矩阵，表示观测噪声${delta _t}$的方差矩阵

图1. 在没有观测情况下，系统状态的从$t-1$到$t$的转移方式

图1给出了在没有观测，仅有输入$u_t$时，状态变量的均值和方差从$t-1$到$t$的转移方式，可见均值和方差的计算，完全是基于高斯分布的线性变化的方法来算的。

图2. Kalman 滤波解决在收到t时刻的输入$u_t$和观测$z_t$的情况下，更新状态$x_t$的问题

图2给出了Kalman滤波所解决的问题，即在获得t时刻的输入和观测的情况下，如何更新$x_t$的均值和方差的问题。当然$u_t$和$z_t$也并不是每一个时刻都需要同时获得，就像贝叶斯滤波一样，可以在获得$u_t$时就做一次状态预测，在获得$z_t$时做一次状态更新。

2.3 Kalman滤波算法

Kalman滤波整体算法如下：

Kalman Filter ($x_{t-1}, P_{t-1}, u_t, z_t$)

• Prediction
• ${overline x _t} = {A_t}{x_{t - 1}} + {B_t}{u_t}$
• ${overline P _t} = {A_t}{P_{t - 1}}A_t^T + {R_t}$
• Correction
• ${K_t} = {overline P _t}H_t^T{({H_t}{overline P _t}H_t^T + {Q_t})^{ - 1}}$
• ${x_t} = {overline x _t} + {K_t}({z_t} - {H_t}{overline x _t})$
• ${P_t} = (I - {K_t}{H_t}){overline P _t}$

• 第一行基于转移矩阵和控制输入，预测$t$时刻的状态
• 第二行是预测方差矩阵
• 第三行计算Kalman增益，K_t
• 第四行基于观测的新息进行状态更新
• 第五行计算更新状态的方差矩阵。

可以看到算法的所有的精妙之处都在于第三行和第四行。我们可以这样来理解：

1.    $({H_t}{overline P _t}H_t^T + {Q_t})$代表对状态进行观测时，观测的不确定程度，它与Kalman增益K_t成反比，表示观测的可能噪声越大的时候，Kalman增益K_t越小。
2. 再看第四行，${x_t}$的更新是在$overline x_t$上加一个 $K_t$ 乘以 $({z_t} - {H_t}{overline x _t})$。$({z_t} - {H_t}{overline x _t})$代表的是预测的值与观测之间的差异，这个差异当预测和观测都比较接近于真实值时比较小。当观测不准，或者预测不准时都会比较大。而前面的乘子K_t是在观测噪声大的时候比较小，所以整个${K_t}({z_t} - {H_t}{overline x _t})$这个修正量，表示利用观测对预测结果的修正量。
• 当观测噪声比较小，预测误差比较大时修正幅度比较大
• 当观测噪声比较小预测误差比较小的时候，或者观测噪声比较大的时候，修正误差的幅度也比较小，从而起到了一种平滑的作用。
3. 利用较准确的观测修正预测误差，不准确的观测修正量也较小，所以在误差较大的时候能快速修正，而在误差较小时能逐渐收敛。

2.4 Kalman滤波算法的推导

这里我们用Bayes公式，给出Kalman Filter是如何导出的。

1. 系统的初始状态是：

$bel({x_0}) = Nleft( {{mu _0},{P_0}} ight)$

2. 预测过程的推导

状态转移模型是线性函数

${x_t} = {A_t}{x_{t - 1}} + {B_t}{u_t} + {varepsilon _t}$

所以，由$x_{t-1}$到$x_{t}$状态转移的条件概率为:

$p({x_t}|{u_t},{x_{t - 1}}) = Nleft( {{A_t}{x_{t - 1}} + {B_t}{u_t},{R_t}} ight)$

回顾Bayes公式，计算预测状态的分布，需要考虑所有可能的$x_{t-1}$：

$overline {bel} ({x_t}) = int {p({x_t}|{u_t},{x_{t - 1}})} { m{ }}bel({x_{t - 1}});d{x_{t - 1}}$

这正是计算两个高斯分布的卷积的过程，参考文献[2]：

$egin{array}{l}
overline {bel} ({x_t}) = int {p({x_t}|{u_t},{x_{t - 1}})} quad ;;quad quad quad quad bel({x_{t - 1}});d{x_{t - 1}}\
quad quad quad quad quad quad  Downarrow quad quad quad quad quad quad quad quad quad quad  Downarrow \
quad quad quad sim Nleft( {{A_t}{mu _{t - 1}} + {B_t}{u_t},{R_t}} ight)quad quad sim Nleft( {{mu _{t - 1}},{P_{t - 1}}} ight)\
quad quad quad quad quad quad  Downarrow quad quad quad quad quad quad quad \
overline {bel} ({x_t}) = eta ;int {exp left{ { - frac{1}{2}{{({x_t} - {A_t}{x_{t - 1}} - {B_t}{u_t})}^T}R_t^{ - 1}({x_t} - {A_t}{x_{t - 1}} - {B_t}{u_t})} ight}} \
quad quad quad quad ;quad exp left{ { - frac{1}{2}{{({x_{t - 1}} - {mu _{t - 1}})}^T}P_{t - 1}^{ - 1}({x_{t - 1}} - {mu _{t - 1}})} ight};d{x_{t - 1}}\
overline {bel} ({x_t}) = left{ {egin{array}{*{20}{c}}
{{{ar mu }_t} = {A_t}{mu _{t - 1}} + {B_t}{u_t}}\
{{{overline P }_t} = {A_t}{P_{t - 1}}A_t^T + {R_t}}
end{array}} ight.quad quad quad quad quad quad quad
end{array}$

所以Kalman滤波器的预测过程，正是基于两个高斯分布的卷积计算得到的解析表达式。

3. 观测更新过程的推导

观测方程也是线性方程，并且噪声是高斯噪声

${z_t} = {H_t}{x_t} + {delta _t}$

所以$p({z_t}|{x_t}) $的条件概率是高斯分布的线性变换计算：

$p({z_t}|{x_t}) = Nleft( {{H_t}{x_t},{Q_t}} ight)$

再考虑贝叶斯公式的状态更新步骤

$bel({x_t}) = \,quad eta quad \,p({z_t}|{x_t})overline {bel} ({x_t})$

这正是两个高斯分布的乘积的问题，参考文献[2]

$egin{array}{l}
bel({x_t}) = \,quad eta quad \,p({z_t}|{x_t})quad quad quad quad quad quad overline {bel} ({x_t})\
quad quad quad quad quad quad quad quad  Downarrow quad quad quad quad quad quad quad quad  Downarrow \
quad quad quad quad quad sim Nleft( {{z_t};{H_t}{x_t},{Q_t}} ight)quad quad sim Nleft( {{x_t};{{overline mu  }_t},{{overline P }_t}} ight)\
quad quad quad quad quad quad quad quad  Downarrow \
bel({x_t}) = eta ;exp left{ { - frac{1}{2}{{({z_t} - {H_t}{x_t})}^T}Q_t^{ - 1}({z_t} - {H_t}{x_t})} ight}exp left{ { - frac{1}{2}{{({x_t} - {{ar mu }_t})}^T}ar P_t^{ - 1}({x_t} - {{ar mu }_t})} ight}\
end{array}$

所以，基于求高斯变量乘积的分布的方法，可以导出结果仍然是高斯分布，用它的二阶矩表示：

$bel({x_t}) = left{ {egin{array}{*{20}{c}}
{{mu _t} = {{ar mu }_t} + {K_t}({z_t} - {H_t}{{ar mu }_t})}\
{{P_t} = (I - {K_t}{H_t}){{overline P }_t}}
end{array}} ight.quad quad { m{with    }}{K_t} = {overline P _t}H_t^T{({H_t}{overline P _t}H_t^T + {Q_t})^{ - 1}}$

所以状态更新中的Kalman增益，均值和方差的更新公式，都是这样导出的。

2.5 Kalman滤波算法的举例

图3和图4通过一维高斯分布的例子，给出在预测和更新过程中状态变量的概率密度分布是如何变化的。

图3. 预测过程的举例，蓝色曲线表示$x_{t-1}$的pdf，紫色曲线表示$overline x_t$的pdf.

图4. 更新过程举例，紫色为预测后的pdf, 黄色为更新后的pdf，青色为观测的结果

从这个例子中可以值得注意的是，在预测部分高斯分布的卷积一般会使状态估计的方差加大；在观测部分高斯分布的乘积一般会将估计的方差收窄。

 

2.6 Kalman滤波的代码实现

Kalman滤波算法可以非常方便的用矩阵计算方法实现，其迭代更新过程的Matlab实现的代码仅有如下几行：

 

2.7 Kalman滤波的效果示例

通过实现一个简单的Kalman滤波器，我们可以直观的看一下Kalman滤波器的提高跟踪准确性的效果。

图5. Kalman滤波器的实验效果示例，其中红色实线是真值；蓝色点是观测；绿色线是滑动平均的结果；紫色曲线是Kalman滤波的结果。

图6. 比较Kalman滤波的跟踪结果和滑动平均的跟踪结果

 

图6给出了直观的跟踪结果与真实值之间的最小二乘误差的比较，课件Kalman滤波算法相比滑动平均等，提供了更高的跟踪准确性。

2.8 Kalman滤波的算法特点

1. Kalman滤波计算快速，计算复杂度为$O(m^{2.376} + n^2)$，其中$m$是观测的维数；$n$是状态的个数。
2. 对于线性系统，零均值高斯噪声的系统，Kalman是理论上无偏的，最优滤波器。
3. Kalman滤波在实际使用中，要注意参数$R$和$Q$的调节，这两者实际上是相对的，表示更相信观测还是更相信预测。具体使用时，$R$可以根据过程噪声的幅度决定，然后$Q$可以相对$R$来给定。当更相信观测时，把$Q$调小，不相信观测时，把$Q$调大。
4. $Q$越大，表示越不相信观测，这是系统状态越容易收敛，对观测的变化响应越慢。$Q$越小，表示越相信观测，这时对观测的变化响应快，但是越不容易收敛。

参考文献

[1]. Sebastian Thrun, Wolfram Burgard, Dieter Fox, Probabilistic Robotics, 2002, The MIT Press.

[2]. P.A. Bromiley, Products and Convolutions of Gaussian Probability Density Functions, University of Manchester