Kalman滤波算法解释与实现

最新推荐文章于 2024-04-19 10:57:44 发布

yuelcay

最新推荐文章于 2024-04-19 10:57:44 发布

阅读量2.3k

点赞数 2

分类专栏：数学文章标签：算法数学

数学专栏收录该内容

2 篇文章 0 订阅

订阅专栏

转载，原文地址，请支持：http://www.cnblogs.com/ycwang16/p/5999034.html

认知计算，还要从贝叶斯滤波的基本思想讲起。这一部分，我们先回顾贝叶斯公式的数学基础，然后再来介绍贝叶斯滤波器。

(一). 概率基础回顾

我们先来回顾一下概率论里的基本知识：

1. X : 表示一个随机变量，如果它有有限个可能的取值 {x1,x2,⋯,xn} .

2. p(X=xi) :表示变量 X 的值为 xi 的概率。

3. p(⋅) :称为概率质量函数(probability mass function).

例如：一个家里有3个房间，机器人在各个房间的概率为 p(room)={0.1,0.3,0.6} .

4. 如果 X 在连续空间取值， p(x) 称为概率密度函数(probability density function)，

p (x \in (a, b)) = \int a b p (x) d x

图1. 概率密度函数曲线示例

5. 联合概率： p(X=x and Y=y)=p(x,y) ，称为联合概率密度分布。如果 X 和 Y 是相互独立的随机变量， p(x,y)=p(x)p(y) 。

6. 条件概率： p(X=x|Y=y) 是在已知 Y=y 的条件下，计算 X=x 的概率。

p (x | y) = p (x, y) / p (y)

p (x, y) = p (x | y) p (y) = p (y | x) p (x)

如果 x 和 y 相互独立，则：

p (x | y) = p (x)

7. 全概率公式：

离散情况下:

p (x) = \sum y p (x, y) = \sum y p (x | y) p (y)

连续情况下：

p (x) = \int p (x, y) d y = \int p (x | y) p (y) d y

(二). 贝叶斯公式

2.1 贝叶斯公式

基于条件概率公式和全概率公式，我们可以导出贝叶斯公式：

P (x, y) = P (x | y) P (y) = P (y | x) P (x) \Rightarrow P (x | y) = P ( y | x ) P ( x ) P ( y ) = causal knowledge \cdot prior knowledge prior knowledge

这里面 x 一般是某种状态； y 一般是代表某种观测。
我们称 P(y|x) 为causal knowledge，意即由 x 的已知情况，就可以推算 y 发生的概率，例如在图2的例子中，已知如果门开着，则 z=0.5m 的概率为0.6；如果门关着，则 z=0.5m 的的概率为0.3。
我们称 P(x) 为prior knowledge，是对 x 的概率的先验知识。例如在图2的例子中，可设门开或关的概率各占 50% .
P(x|y) 是基于观测对状态的诊断或推断。贝叶斯公式的本质就是利用causal knowledge和prior knowledge来进行状态推断或推理。

例1: Dog face ：

在图2所示的例子中，机器人根据观测的到门的距离，估算门开或关的概率，若测量到门的距离为 z=0.5m ，则可用条件概率描述门开着的概率：

P (open | z = 0.6) = ?

图 2.机器人根据观测计算门开或关的概率

P (o p e n | z = 0.5) = P ( z | o p e n ) P ( o p e n ) P ( z ) < - - 贝 叶 斯 公 式 = P ( z | o p e n ) P ( o p e n ) P ( z | o p e n ) p ( o p e n ) + P ( z | \neg o p e n ) p ( \neg o p e n ) < - - 全 概 率 公 式 = 0.6 \cdot 0.5 0.6 \cdot 0.5 + 0.3 \cdot 0.5 = 2 / 3

2.2 贝叶斯公式的计算

可以看到贝叶斯公式的分母项 P(y) ，同 P(x|y) 无关，所以可以把它作为归一化系数看待：

P (x | y) = P ( y | x ) P ( x ) P ( y ) = η P (y | x) P (x) η = P (y) - 1 = 1 \sum x P ( y | x ) P ( x )

所以基于causal knowledge和prior knowledge进行条件概率计算的过程如下：

Algorithm:

2.3 贝叶斯公式中融合多种观测

在很多应用问题中，我们会用多种观测信息对一个状态进行猜测和推理，贝叶斯公式中是如何融合多种观测的呢？

我们简单推导一下：

P (x | y, z) = P ( x , y , z ) P ( y , z ) = P ( y | x , z ) p ( x , z ) P ( y , z ) = P ( y | x , z ) p ( x | z ) p ( z ) P ( y | z ) p ( z ) = P ( y | x , z ) p ( x | z ) P ( y | z )

所以有：

P (x | y, z) = P ( y | x , z ) P ( x | z ) P ( y | z )

2.4 贝叶斯递推公式

由此，我们来推导贝叶斯滤波的递推公式：

P(x|z1,…,zn)=?

我们把 zn 看做 y ，把 z1,…,zn−1 看做 z ，代入上面的公式：

P (x | z 1, \dots, z n) = P ( z n | x , z 1 , \dots , z n - 1 ) P ( x | z 1 , \dots , z n - 1 ) P ( z n | z 1 , \dots , z n - 1 )

再由Markov属性，在 x 已知的情况下， zn 同 {z1,…,zn–1} 无关，所以：

P (x | z 1, \dots, z n) = P ( z n | x , z 1 , \dots , z n - 1 ) P ( x | z 1 , \dots , z n - 1 ) P ( z n | z 1 , \dots , z n - 1 ) = P ( z n | x ) P ( x | z 1 , \dots , z n - 1 ) P ( z n | z 1 , \dots , z n - 1 )

从而我们得到贝叶斯的递推公式：

P (x | z 1, \dots, z n) = P ( z n | x ) P ( x | z 1 , \dots , z n - 1 ) P ( z n | z 1 , \dots , z n - 1 ) = η n P (z n | x) P (x | z 1, \dots, z n - 1) = η n P (z n | x) η n - 1 P (z n - 1 | x) P (x | z 1, \dots, z n - 2) = η 1 \dots η n \prod i = 1... n P (z i | x) P (x)

例2： Dog face 在例1的基础上，如果机器人第二次测量到门的距离仍然为0.5米，计算门开着的概率。

所以，第二次z=0.5m的观测增大了对门开着的概率的置信程度。

(三). 如何融入动作？

在实际问题中，对象总是处在一个动态变化的环境中，例如：

机器人自身的动作影响了环境状态
其它对象，比如人的动作影响了环境状态
或者就是简单的环境状态随着时间发生了变化。

如何在Bayes模型中来描述动作的影响呢?

首先，动作所带来的影响也总是具有不确定性的
其次，相比于观测，动作一般会使得对象的状态更为模糊（或更不确定）。

我们用 u 来描述动作，在 x′ 状态下，执行了动作 u 之后，对象状态改变为 x 的概率表述为：

P (x | u, x')

动作对状态的影响一般由状态转移模型来描述。如图3所示，表示了“关门”这个动作对状态影响的转移模型。这个状态转移模型表示：关门这个动作有0.1的失败概率，所以当门是open状态时，执行“关门”动作，门有0.9的概率转为closed状态，有0.1的概率保持在open状态。门是closed的状态下，执行“关门”动作，门仍然是关着的。

图3. “关门”动作的状态转移模型

执行某一动作后，计算动作后的状态概率，需要考虑动作之前的各种状态情况，把所有情况用全概率公式计算：

连续情况下：

P (x | u) = \int P (x | u, x') P (x') d x'

离散情况下：

P (x | u) = \sum P (x | u, x') P (x')

例3： Dog face 在例2的基础上，如果按照图3所示的状态转移关系，机器人执行了一次关门动作，计算动作后门开着的概率？

P (o p e n | u) = \sum P (o p e n | u, x') P (x') = P (o p e n | u, o p e n) P (o p e n) + P (o p e n | u, c l o s e d) P (c l o s e d) = 1 10 * 0.8 + 0 1 * 0.2 = 0.08

P (c l o s e d | u) = \sum P (c l o s e d | u, x') P (x') = P (c l o s e d | u, o p e n) P (o p e n) + P (c l o s e d | u, c l o s e d) P (c l o s e d) = 9 10 * 0.8 + 1 1 * 0.2 = 0.92

所以，执行一次关门动作后，门开着的概率变为了0.08.

(四). 贝叶斯滤波算法

4.1 算法设定

由上述推导和示例，我们可以给出贝叶斯滤波的算法，算法的输入输出设定如下。

系统输入
1. 1到 t 时刻的状态观测和动作： dt={u1,z1…,ut,zt}
2. 观测模型： P(z|x)
3. 动作的状态转移模型： P(x|u,x′)
4. 系统状态的先验概率分布 P(x) .
期望输出
1. 计算状态的后延概率，称为状态的置信概率： Bel(xt)=P(xt|u1,z1…,ut,zt)

4.2 算法基本假设

贝叶斯滤波的基本假设：

1. Markov性假设: t 时刻的状态由 t−1 时刻的状态和 t 时刻的动作决定。 t 时刻的观测仅同 t 时刻的状态相关，如图4所示：

图4. Markov模型

p(zt|x0:t,z1:t,u1:t)=p(zt|xt)
p(xt|x1:t−1,z1:t,u1:t)=p(xt|xt−1,ut)

2. 静态环境，即对象周边的环境假设是不变的

3. 观测噪声、模型噪声等是相互独立的

4.3 Bayes滤波算法

基于上述设定和假设，我们给出贝叶斯滤波算法的推导过程：

Bel(xt)=P(xt|u1,z1…,ut,zt)

=ηP(zt|xt,u1,z1,…,ut)P(xt|u1,z1,…,ut) <—Bayes

=ηP(zt|xt)P(xt|u1,z1,…,ut) <—Markov

=ηP(zt|xt)∫P(xt|u1,z1,…,ut,xt−1)P(xt−1|u1,z1,…,ut)dxt−1) <—TotalProb.

=ηP(zt|xt)∫P(xt|ut,xt−1)P(xt−1|u1,z1,…,ut)dxt−1)<—Markov

=ηP(zt|xt)∫P(xt|ut,xt−1)P(xt−1|u1,z1,…,zt−1)dxt−1)<—Markov

=ηP(zt|xt)∫P(xt|ut,xt−1)Bel(xt−1)dxt−1

其中第一步采用贝叶斯公式展开，第二步使用Markov性质( zt 仅由 xt 决定)；第三步使用全概率公式对 xt−1 进行展开；第四步继续使用Markov性质( xt 仅由 xt−1 和 ut 决定)；第五步继续使用Markov性质，因为 xt−1 同 ut 无关，最终得到 Bel(xt) 的递推公式。

可见递推公式中分为两个步骤， ∫P(xt|ut,xt−1)Bel(xt−1)dxt−1 部分是基于 xt−1,ut 预测 xt 的状态； ηP(zt|xt) 部分是基于观测 zt 更新状态 xt .

4.3 Bayes滤波算法流程

所以，Bayes滤波的算法流程图如图5所示。如果 d 是观测，则进行一次状态更新，如果 d 是动作，则进行一次状态预测。

图5. Bayes滤波的算法流程

我们看到，在进行状态预测时，需要对所有可能的 x′ 状态进行遍历，使得基本的Bayes模型在计算上成本是较高的。

4.3 Bayes滤波算法的应用

Bayes滤波方法是很多实用算法的基础，例如：

Kalman滤波
扩展Kalman滤波
信息滤波
粒子滤波

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

一. 背景知识回顾

1.1 Bayes滤波

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

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

bel¯¯¯¯¯¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1

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

bel(xt)=ηp(zt|xt)bel¯¯¯¯¯¯(xt)

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

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

1.2 正态分布(Guassian Distribution)

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

p (x) \sim N (μ, σ 2) : p (x) = 1 2 π \sqrt σ e - 1 2 ( x - μ ) 2 σ 2

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

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

p (x) \sim N (μ, Σ) : p (x) = 1 ( 2 π ) d / 2 | Σ | 1 / 2 e - 1 2 (x - μ) t Σ - 1 (x - μ)

同样，一阶矩为 μ 表示各元变量的期望值，二阶矩为方差矩阵 Σ 表示各元变量的不确定程度。

1.3 正态分布的特点

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

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

X∼N(μ,Σ)Y=AX+B}⇒Y∼N(Aμ+B,AΣAT)

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

X1∼N(μ1,σ21)X2∼N(μ2,σ22)}⇒p(X1+X2)∼N(μ1+μ2,σ21+σ22+2ρσ1σ2)

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

X1∼N(μ1,σ21)X2∼N(μ2,σ22)}⇒p(X1)⋅p(X2)∼N(σ22σ21+σ22μ1+σ21σ21+σ22μ2,σ21σ22σ21+σ22)

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

二. Kalman滤波

2.1 Kalman滤波的模型假设

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

状态方程
xt=Atxt−1+Btut+εt
观测方程
zt=Htxt+δt

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

2.2 Kalman滤波器的模型

xt ， n 维向量，表示 t 时刻观测状态的均值。
Pt ， n∗n 方差矩阵，表示 t 时刻被观测的 n 个状态的方差。
ut ， l 维向量，表示 t 时刻的输入
zt ， m 维向量，表示 t 时刻的观测
At ， n∗n 矩阵，表示状态从 t−1 到 t 在没有输入影响时转移方式
Bt ， n∗n 矩阵，表示 ut 如何影响 xt
Ht ， m∗n 矩阵，表示状态 xt 如何被转换为观测 zt
Rt ， n∗n 矩阵，表示过程噪声 εt 的方差矩阵
Qt ， m∗m 矩阵，表示观测噪声 δt 的方差矩阵

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

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

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

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

2.3 Kalman滤波算法

Kalman滤波整体算法如下：

Kalman Filter ( xt−1,Pt−1,ut,zt )

Prediction
- x¯¯¯t=Atxt−1+Btut
- P¯¯¯¯t=AtPt−1ATt+Rt
Correction
- Kt=P¯¯¯¯tHTt(HtP¯¯¯¯tHTt+Qt)−1
- xt=x¯¯¯t+Kt(zt−Htx¯¯¯t)
- Pt=(I−KtHt)P¯¯¯¯t

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

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

(HtP¯¯¯¯tHTt+Qt) 代表对状态进行观测时，观测的不确定程度，它与Kalman增益K_t成反比，表示观测的可能噪声越大的时候，Kalman增益K_t越小。
再看第四行， xt 的更新是在 x¯¯¯t 上加一个 Kt 乘以 (zt−Htx¯¯¯t) 。 (zt−Htx¯¯¯t) 代表的是预测的值与观测之间的差异，这个差异当预测和观测都比较接近于真实值时比较小。当观测不准，或者预测不准时都会比较大。而前面的乘子K_t是在观测噪声大的时候比较小，所以整个 Kt(zt−Htx¯¯¯t) 这个修正量，表示利用观测对预测结果的修正量。
- 当观测噪声比较小，预测误差比较大时修正幅度比较大
- 当观测噪声比较小预测误差比较小的时候，或者观测噪声比较大的时候，修正误差的幅度也比较小，从而起到了一种平滑的作用。
利用较准确的观测修正预测误差，不准确的观测修正量也较小，所以在误差较大的时候能快速修正，而在误差较小时能逐渐收敛。

2.4 Kalman滤波算法的推导

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

1. 系统的初始状态是：

bel(x0)=N(μ0,P0)

2. 预测过程的推导

状态转移模型是线性函数

xt=Atxt−1+Btut+εt

所以，由 xt−1 到 xt 状态转移的条件概率为:

p(xt|ut,xt−1)=N(Atxt−1+Btut,Rt)

回顾Bayes公式，计算预测状态的分布，需要考虑所有可能的 xt−1 ：

bel¯¯¯¯¯¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1

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

bel¯¯¯¯¯¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1⇓⇓∼N(Atμt−1+Btut,Rt)∼N(μt−1,Pt−1)⇓bel¯¯¯¯¯¯(xt)=η∫exp{−12(xt−Atxt−1−Btut)TR−1t(xt−Atxt−1−Btut)}exp{−12(xt−1−μt−1)TP−1t−1(xt−1−μt−1)}dxt−1bel¯¯¯¯¯¯(xt)={μ¯t=Atμt−1+BtutP¯¯¯¯t=AtPt−1ATt+Rt

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

3. 观测更新过程的推导

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

zt=Htxt+δt

所以 p(zt|xt) 的条件概率是高斯分布的线性变换计算：

p(zt|xt)=N(Htxt,Qt)

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

bel(xt)=ηp(zt|xt)bel¯¯¯¯¯¯(xt)

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

bel(xt)=ηp(zt|xt)bel¯¯¯¯¯¯(xt)⇓⇓∼N(zt;Htxt,Qt)∼N(xt;μ¯¯¯t,P¯¯¯¯t)⇓bel(xt)=ηexp{−12(zt−Htxt)TQ−1t(zt−Htxt)}exp{−12(xt−μ¯t)TP¯−1t(xt−μ¯t)}

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

bel(xt)={μt=μ¯t+Kt(zt−Htμ¯t)Pt=(I−KtHt)P¯¯¯¯twithKt=P¯¯¯¯tHTt(HtP¯¯¯¯tHTt+Qt)−1

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

2.5 Kalman滤波算法的举例

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

图3. 预测过程的举例，蓝色曲线表示 xt−1 的pdf，紫色曲线表示 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滤波的算法特点

Kalman滤波计算快速，计算复杂度为 O(m2.376+n2) ，其中 m 是观测的维数； n 是状态的个数。
对于线性系统，零均值高斯噪声的系统，Kalman是理论上无偏的，最优滤波器。
Kalman滤波在实际使用中，要注意参数 R 和 Q 的调节，这两者实际上是相对的，表示更相信观测还是更相信预测。具体使用时， R 可以根据过程噪声的幅度决定，然后 Q 可以相对 R 来给定。当更相信观测时，把 Q 调小，不相信观测时，把 Q 调大。
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

yuelcay

关注

2
点赞
踩
7

收藏

觉得还不错? 一键收藏
0
评论
Kalman滤波算法解释与实现

转载，原文地址，请支持：http://www.cnblogs.com/ycwang16/p/5999034.html认知计算，还要从贝叶斯滤波的基本思想讲起。这一部分，我们先回顾贝叶斯公式的数学基础，然后再来介绍贝叶斯滤波器。(一). 概率基础回顾我们先来回顾一下概率论里的基本知识：1. XX: 表示一个随机变量，如果它有有限个可能的取值{x1,x2,
复制链接

扫一扫