第1页 / 共6页
第2页 / 共6页
第3页 / 共6页
第4页 / 共6页
第5页 / 共6页
第6页 / 共6页
EKF UKF PF 总结.docx
一 现实中的问题
二 线性建模
三 连续系统离散化
四 非线性系统线性化(EKF)
五 无损卡尔曼滤波(UKF)
六 粒子滤波(PF)
七 总结
一 现实中的问题
问题一：物体在运动过程中有多种定位方式如卫星定位、惯性定位等，那么应该信任那种定
位方式呢？如果采用加权融合两种定位方式，如何确定各种定位方式的权值大小？
问题二：测量一个人的重量，使用了多种重量工具，得出的结果不一样，那么该信任哪个称
重工具呢？得到一个最优最可信赖的结果呢？
卡尔曼滤波就是这样一种最优滤波算法，他可以融合同一作用的多种值，得到一个最优最可
信赖的结果。同时告知你，每个值应该在融合时取的权重。
二 线性建模
2.1 模型架构
现实生活中一个系统的输出值，和系统已有的状态以及系统的输入状态有关，系统框图如下：
如上图所示建立起理想的线型模型，由系统模型建立数学方程如下。
(X
k
)1
)(
kAX
BU
)(
kWk
(2.1)
(2.2)
(Y
k
)1
C
X(k
1kV1)
W 和 V 相互独立，且满足：
)
(E
0
W
k
0
(
)
VE
k
)
0
(
VWE
k
k
)
cov(
)(
WW
iQ
ik
k
cov(
)
)(
iR
VV
k
2.2 模型求解
在当前 Y 和前一状态 X 已知的情况下求解方程。分为以下 5 个步骤。
1>预测：
(Xˆ
2>求出预测的可信赖度，即预测随机变量的方差。
（2.3）
)(X
kA
)1
BU
)(
k
k
ik
TAkAP
)(
k
)1
(Pˆ
3>计算增益系数
Q
（2.4）
K
1k
(ˆ
kP
T
(ˆ
kPCC
(
)1
)1
C
T
R
)
1
（2.5）
4>计算矫正值
(ˆ
(X
kX
)1
k
1
5>计算后验状态误差方差阵
k
)1
K
((
kY
)1
(ˆ
kXC
))1
（2.6）
(
KP
)1
(
ˆ)
PCKI
k
1
k
1
（2.7）
规律总结：向量 X 协方差阵位 P，则 AX 的协方差阵为 APAT
三 连续系统离散化
t
t
t
t
（3.1）
WGXF
t
t
HX
V
t
现实生活中的模型经常是非线性，非离散化的，因此需要建模者进行离散化、线性化等操
作。连续线型系统常用如下所示微分方程。
tX
Z
假设离散化的时间为 T，对以上线性方程进行离散化。即相当于对 X(t+T)在 X(t)处进行泰勒
展开(注意线性系统离散化泰勒展开和非线性系统不一样的地方)。利用一阶展开可得
t(X
T
(
TtZ
设 t=KT,t+T=(k+1)T 进行离散化，可得到模型的方程为，然后按照第二节的方式求解。
(X
k
(
kZ
1(
))
)1
)1
)(
kXkH
)(
)(
tX
tX
)
(
TtHX
)(F)(
tX
)
)(
TtWtGTtXt
T)(
)(
kV
T
(
TtV
kXkF
kWkG
)(
T)(
（3.2）
（3.3）
)(
)(
)(
)
)
(
注:也可以对 X(t+T)进行二阶或者三阶泰勒展开，得到更加精确的状态转移方程。
四 非线性系统线性化(EKF)
非线性系统表达的状态方程和观测方程如下式所示。
)(
tx
)(
tz
))
((
)(
((
txf
twtxg
))
((
)(
tv
txh
(4.1)
))
如果 t0 时刻 x(t0)的滤波估计已知为 Xt0,在 Xt0 附近对 f 进行线性化，即进行泰勒展开，如下
式所示。在式中忽略了噪声驱动矩阵 g(x)展开的余项。
)(
tx
(
xf
t
0
)
)(
tz
(
xh
t
0
)
f
x
h
x
)((
tx
x
t
0
)
((
)(
twtxg
))
(4.2)
)((
tx
x
t
0
)
)(
tv
经过整理可得到线性形式如下所示。
)(
tx
)(
tz
f
x
h
x
)(
tx
(
xf
t
0
)
)(
tx
(
xh
t
0
)
f
x
h
x
x
t
0
((
)(
twtxg
))
(4.3)
x
t
0
)(
tv
令 ：
F(t)
)(
tH
f
x
h
x
(
xf
t
)(U
t
)
0
)(
tI
(
xh
t
0
)
(4.4)
0
x
t
f
x
h
x
t
x
0
将 4.3 式转化为如下式：
)()(F)(
)(
tUtxt
tx
)()(
)(
)(
tz
txtH
tI
))
)(
((
twtxg
)(
tv
(4.5)
上式按照第三节离散化的步骤进行离散，上式 t 必须在 t0 附近进行展开，因此在后面离散
化的时候，t0 相当于上一时刻，t 就相当于当前时刻。最后得到表达式如下。
(X
k
(
kZ
)(UT)(
kWkGk
)(
)(
kV
kY
1(
))
)1
)1
)(
kXkH
kXkF
T)(
(4.6)
)(
)(
(
其中 U(k),Y(K)参照式 3.3 给出来。另外 F，H 两个重要的系数矩阵如下。
)(
kF
)(
kH
f
x
h
x
(|
x
x
k
)
(|
x
x
k
)
(4.7)
五 无损卡尔曼滤波(UKF)
非线性系统表达的状态方程和观测方程如下式所示。
)(
tx
)(
tz
((
))
)(
((
txf
twtxg
))
((
)(
tv
txh
(5.1)
))
在无损卡尔曼滤波里面，不进行确定的状态转移，而是采用样本集来描述随机变量的分布。
用样本集描述的优点是，样本集可以从前一刻的分布获得，进行转移后又通过统计的方法
获得后一时刻的状态分布，而不是通过方程推导的方法获得。这样就可以避免线性化过程
给系统带来的误差。
对 5.1 式进行离散化可得（此处是一阶泰勒公式离散化）：
(X
k
(
kZ
(
kXF
))
)(
kV
)(
kX
(
(
kXH
kWkXG
)1
)1
T)(
(5.2)
T))
(
))
(
(
n 3
是经验公式。
)(
i
)0(
选取样本集的公式，其中使
X
~1~,)
X
iP
i
~,)
n
X
iP
i
样本集对应的权值如下：
,
iX
X
X
)
)
(
(
0
(
(
n
n
)(
i
(5.3)
n
2~1
n
)0(
w
m
n
)0(
w
c
)(
i
w
m
n
)(
i
w
c
1(
2
a
)
(5.4)
1
n
(2
)
,
i
2~1
n
(2
)
na
n
，a 的选取控制了采样点的分布，为待选参数，其具体指没有界限，但
应确保
)
(
n
P
为半正定矩阵。待选参数是一个非负的权系数。此处权重系数不知为何
是负数，一直没明白？？？
整个 UKF 滤波步骤
1> 生成样本集，样本集的生成按照 5.3 式进行。
2> 对样本集做预测，样本集预测按照 5.2 式进行，不考虑噪声项。得到先验
ˆ )(
X i
(
K
)1
。
3> 计算系统的一步先验预测值和误差协方差阵。
(ˆ
KX
(ˆ
KP
ˆ
Xw
ˆ(
X
(ˆ
KX
)1
)1
)1
ˆ(
X
))1
)1
K
K
)(
i
(
i
i
(
w
(
kWkXG
))
)(
i
(
其中 Q 阵由
T)(
求得。
)(
i
(
K
)1
(ˆ
KX
T
))1
Q
4> 按照先验状态误差协方差生成先验状态样本集。样本集生成方法同步骤 1。得到新先验
样本
ˆ )(
X i
(
K
)1
。
5> 将新先验样本代入观测方程，得到观测样本。
Z
)(
i
)(
k
ˆ(
XH
)(
i
(
K
))1
6> 得到观测预测值以及系统预测的方差。
)1
(ˆ
KZ
(ˆ
KX
i
ˆ
(
i
i
(
)(
i
)(
i
w
)1
(ˆ
KZw
KZ
ˆ(
P
KZ
ZZ
ˆ(
X
P
XZ
7> 计算增益系数
K
K
w
1
P
XZ
P
ZZ
)(
i
(
/
k
)1
)1
))1
))1
(
)(
i
ˆ(
KZ
ˆ(
KZ
)(
i
(
)1
)1
(ˆ
KZ
(ˆ
KZ
T
))1
))1
R
T
8> 矫正更新状态值和协方差
(
kX
(
kP
)1
)1
)1
)1
(
K
KPK
(ˆ
kX
(ˆ
kP
1
k
k
1
ZZ
(
kZ
T
1
k
)1
(ˆ
kZ
))1
六 粒子滤波(PF)
非线性系统表达的状态方程和观测方程如下式所示。
)(
tx
)(
tz
(
))
((
tutxf
)(
((
tv
txh
((
)(
twtxg
),
))
(5.1)
))
其中
)(
(
tvtw
),
分别是系统噪声和观测噪声，此处噪声不一定要求是高斯噪声，因此粒子滤
波和卡尔曼滤波的区别是对随机过程的要求。卡尔曼滤波只适用于高斯过程，粒子滤波适
用于普通随机过程。对 5.1 式按照第三节过程进行离散化。
(X
k
(
kZ
1(
))
)1
)1
)(
kXkH
T)(
)(
kV
kXkF
kWkG
T)(
)(
)(
(
(5.2)
整个 PF 滤波步骤:
1>初始化：一开始对 x(1)一无所知，认为 x(1)在全状态空间内平均分布或高斯分布。于是初始的采样
就平均分布在整个状态空间中。然后将所有采样输入状态转移方程，得到预测粒子。初始采样粒子按
照如下公式取得。
X
part
)(
n
VecSize
*
Rand
VecSize 为各状态变量的取值范围，Rand 为[-1,1]区间均匀分布的随机变量。D 为 Xpart 求出的 Zpart
与 Z 之间的距离。权值矩阵可以根据自己想要的效果进行决定，原则是距离观测量越近，权值越重。
2>滤波的预估阶段：粒子滤波首先根据 x(t-1) 和他的概率分布生成大量的采样，这些采样就称之为
粒子。那么这些采样在状态空间中的分布实际上就是 x(t-1) 的概率分布了。好，接下来依据状态转移
方程加上控制量可以对每一粒子得到一个预测粒子。所有的预测粒子就代表了涉及哪些参数化的东
西。滤波估计按照 5.2 式进行。预测过程模拟系统噪声。
3>进入校正阶段来：有了预测粒子，当然不是所有的预测粒子都能得到我们的时间观测值 y 对不，越
是接近真实状态的粒子，当然获得越有可能获得观测值 y 对吧。于是我们对所有的粒子得有个评价了，
这个评价就是一个条件概率 P(y|xi),直白的说，这个条件概率代表了假设真实状态 x(t)取第 i 个粒子 xi
时获得观测 y 的概率。令这个条件概率为第 i 个粒子的权重。如此这般下来，对所有粒子都进行这么
一个评价，那么越有可能获得观测 y 的粒子，当然获得的权重越高。好了，预测信息融合在粒子的分
布中，观测信息又融合在了每一粒子的权重中。矫正过程模拟测量噪声。
X k
)(*)(
iwiX
注意：权重制定函数原理上是条件概率，实际上就是系统的模型，由自己确定。
4，最后采用重采样算法，去除低权值的粒子，复制高权值的粒子。所得到的当然就是我们说需要的
真实状态 x(t)了，而这些重采样后的粒子，就代表了真实状态的概率分布了。
总结:粒子滤波虽然适用于非线性非高斯过程，但是它依赖观测值计算权值，如果观测值误差存在短
时间内非零偏的随机变量，则粒子滤波效果不太好。
七 总结
系统建模以标准的线性化离散模型建立，而生活的实例往往是非线性非离散的模型，因此
我们需要列出数学方程以后，先进行线性化，再进行离散化。目标是求出状态转移矩阵和
观测矩阵。
应用到实际问题，即第一节中所说的多源融合最优化问题，将某源作为系统状态更新，将
另外一源作为观测，对数据进行融合。
如何确定系统噪声和观测声，应该通过统计系统输入的随机变量和观测的随机变量得到统
计结果和观测结果。
相关推荐
2023年江西萍乡中考道德与法治真题及答案.doc
2012年重庆南川中考生物真题及答案.doc
2013年江西师范大学地理学综合及文艺理论基础考研真题.doc
2020年四川甘孜小升初语文真题及答案I卷.doc
2020年注册岩土工程师专业基础考试真题及答案.doc
2023-2024学年福建省厦门市九年级上学期数学月考试题及答案.doc
2021-2022学年辽宁省沈阳市大东区九年级上学期语文期末试题及答案.doc
2022-2023学年北京东城区初三第一学期物理期末试卷及答案.doc
2018上半年江西教师资格初中地理学科知识与教学能力真题及答案.doc
2012年河北国家公务员申论考试真题及答案-省级.doc
2020-2021学年江苏省扬州市江都区邵樊片九年级上学期数学第一次质量检测试题及答案.doc
2022下半年黑龙江教师资格证中学综合素质真题及答案.doc
