. 细说Kalman滤波：The Kalman Filter

Posted 2020-08-16 机器人感知、群智感知、图结构计算与优化算法

转载请注明出处，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)

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

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

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

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

$$\\begin{array}{l}
p({\\bf{x}})\\sim {\\rm N}({\\bf{\\mu }},{\\bf{\\Sigma }}):\\\\
p({\\bf{x}}) = \\frac{1}{{{{(2\\pi )}^{d/2}}{{\\left| {\\bf{\\Sigma }} \\right|}^{1/2}}}}{e^{ - \\frac{1}{2}{{({\\bf{x}} - {\\bf{\\mu }})}^t}{{\\bf{\\Sigma }}^{ - 1}}({\\bf{x}} - {\\bf{\\mu }})}}
\\end{array}$$

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

1.3 正态分布的特点

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

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

$\\left. {\\begin{array}{*{20}{l}}
{X\\sim N(\\mu ,\\Sigma )}\\\\
{Y = AX + B\\quad \\;}
\\end{array}} \\right\\}\\quad  \\Rightarrow \\quad Y\\sim N(A\\mu  + B,A\\Sigma {A^T})$

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

$\\left. {\\begin{array}{*{20}{c}}
{{X_1}\\sim N({\\mu _1},\\sigma _1^2)}\\\\
{{X_2}\\sim N({\\mu _2},\\sigma _2^2)}
\\end{array}} \\right\\} \\Rightarrow p({X_1} + {X_2})\\sim N\\left( {{\\mu _1} + {\\mu _2},\\sigma _1^2 + \\sigma _2^2 + 2\\rho {\\sigma _1}{\\sigma _2}} \\right)$

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

$\\left. {\\begin{array}{*{20}{c}}
{{X_1}\\sim N({\\mu _1},\\sigma _1^2)}\\\\
{{X_2}\\sim N({\\mu _2},\\sigma _2^2)}
\\end{array}} \\right\\} \\Rightarrow p({X_1}) \\cdot p({X_2})\\sim N\\left( {\\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^2\\sigma _2^2}}{{\\sigma _1^2 + \\sigma _2^2}}} \\right)$

正因为高斯分布有这些特点，所以，在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}) = N\\left( {{\\mu _0},{P_0}} \\right)$

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}}) = N\\left( {{A_t}{x_{t - 1}} + {B_t}{u_t},{R_t}} \\right)$

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

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

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

$\\begin{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 N\\left( {{A_t}{\\mu _{t - 1}} + {B_t}{u_t},{R_t}} \\right)\\quad \\quad \\sim N\\left( {{\\mu _{t - 1}},{P_{t - 1}}} \\right)\\\\
\\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})} \\right\\}} \\\\
\\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}})} \\right\\}\\;d{x_{t - 1}}\\\\
\\overline {bel} ({x_t}) = \\left\\{ {\\begin{array}{*{20}{c}}
{{{\\bar \\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}} \\right.\\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}) = N\\left( {{H_t}{x_t},{Q_t}} \\right)$

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

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

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

$\\begin{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 N\\left( {{z_t};{H_t}{x_t},{Q_t}} \\right)\\quad \\quad \\sim N\\left( {{x_t};{{\\overline \\mu  }_t},{{\\overline P }_t}} \\right)\\\\
\\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})} \\right\\}\\exp \\left\\{ { - \\frac{1}{2}{{({x_t} - {{\\bar \\mu }_t})}^T}\\bar P_t^{ - 1}({x_t} - {{\\bar \\mu }_t})} \\right\\}\\\\
\\end{array}$

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

$bel({x_t}) = \\left\\{ {\\begin{array}{*{20}{c}}
{{\\mu _t} = {{\\bar \\mu }_t} + {K_t}({z_t} - {H_t}{{\\bar \\mu }_t})}\\\\
{{P_t} = (I - {K_t}{H_t}){{\\overline P }_t}}
\\end{array}} \\right.\\quad \\quad {\\rm{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