2.5状态估计-卡尔曼滤波

卡尔曼滤波器的局限性：
卡尔曼滤波器解决运动目标或实体的状态估计问题时，动态方程和测量方程均为线性。
4
自动化学院
NUST
智能信息处理技术
一、数字滤波器作估值器
1、非递归估值器
2、递归估值器
5
自动化学院
NUST
智能信息处理技术
1、非递归估值器
采样平均估值器：
z1 h1 z2 h2 z3 h3 ∑
ˆ X
18
自动化学院
NUST
智能信息处理技术
z k 1
bk 1
最优递归估计器 ˆ X k 1
1（ / 1 bk )
z
1
递 归 公 式
bk 1 ^ X k 1 Xk zk 1 1 bk 1 bk
^
19
自动化学院
NUST
智能信息处理技术
z k 1


最优递归估计器
设要估计的随机信号为由均值为0，方差为σ2w的白噪声激励的一个一阶递归过 程，即信号对时间变化满足动态方程：
x(k)=ax(k-1)+w(k-1)
式中，a——系统参数 w(k-1)——白噪 声采样。
如果令x(0)=0，E［w(k)］=0， 则
0 j 0 Pw ( j ) E[ w(k ) w(k j )] 2 w j 0
17
bk 1 k b 1 bk k b 1 1 1 /(k b)
bk bk 1 1 bk
自动化学院
NUST
智能信息处理技术
ˆ k 1 bk 1 zi X
i 1
k 1
ˆ k 1 X
1 ˆ bk Xk zk 1 1 bk 1 bk
z－ 1
z－ 1
…
z－ 1 hm
采用时域分析方法在掺杂有噪声的测量信号中估计信号x。
6
自动化学院
NUST
智能信息处理技术
根据数字信号处理技术，所谓非递归数字滤波器是一种只有前馈而没有反馈 的滤波器。
假定用zk表示观测值
zk=x+nk
假定，E(x)=x0 E(nk)=0，E(n2k)=σ2n。 ，D(x)=σ2
2 2
9
自动化学院
NUST
智能信息处理技术
结论
①估计值 X 是用m个采样值的平均值作为被 估参量x的近似值； ②估值器的均方误差随着m的增加而减少； ③该估值器是一个无偏估值器
^
m 1 ˆ E ( X ) E ( x ni ) E ( x ) x0 m i 1
估计值， 则
ˆ k (1 a) yk X
12
ˆ k (1 a k ) x (1 a ) a k i ni X
i 1
k
自动化学院
NUST
智能信息处理技术
当k值较大时， 估值的均方误差
1 a 2 2 ˆ x) 2 P E [( X ] ? n k k 1 a
24
自动化学院
NUST
智能信息处理技术
2） 观测模型
观测模型由下式给出：
z(k)=cx(k)+v(k)
式中：c——测量因子；
v(k)——E(·)=0，
D(·)=σ2n的白噪声。
25
自动化学院
NUST
智能信息处理技术
2、标量卡尔曼滤波器
由前将递归估计的形式写成：
ˆ ( k ) a( k ) X ˆ (k 1) b(k ) z(k ) X
1. 最优非递归估计
非递归滤波器的估计值及其估计误差可分别表示为
ˆ hi zi X
i 1
m
在b<<m时
ˆ x )2 ] E[( h z x )2 ] P E[( X i i
i 1
m
最优非递归估计近似于采样平均
在噪声方差σ2n较大时
均值为零的白噪声
性能明显优于非最佳情况
均方误差
ˆ (k ) x (k ))2 ] P (k ) E[( X ˆ (k 1) b(k ) z (k ) x (k ))2 ] E[(a (k ) X
分别对a(k)和b(k)求导，并令其等于0，求其最佳估计，得出a(k)与b(k)的关系?
a(k)=a［1-cb(k)］
均方误差
1 2 P ( k ) n b( k ) c
对于给定的信号模型和观测模型，上述一组方程便称为一维标量卡尔曼滤波器， 其结构如图所示。
27
自动化学院
NUST
智能信息处理技术
z(k) ＋
∑ －
b(k)
＋
∑ ＋
ˆ (k ) X
c
a
z－ 1
标量卡尔曼滤波器结构
ˆ (k ) aX ˆ (k 1) b(k )[z(k ) acX ˆ (k 1)] X
ˆ 0 E ( x) X
若E(x)=0
ˆ 0 X 0 ˆ 0 X 20 x 1 b0 b0 2 ? n b
从零开始递推
21
自动化学院
NUST
智能信息处理技术
三、标量卡尔曼滤波器
主要作用：
对掺杂有噪声的随机信号进行线性估计。
22
自动化学院
NUST
智能信息处理技术
1、模型 1） 信号模型
智能信息处理技术
第5讲 状态估计—卡尔曼滤波
智能信息处理技术
状态估计的主要内容
状态估计主要内容： 位置估计： 速度估计：
位置与速度估计
距离、方位和高度或仰角的估计 速度、加速度估计
应用：
通过数学方法寻求与观测数据最佳拟合的状态向量。
1、确定运动目标的当前位置与速度； 2、确定运动目标的未来位置与速度； 3、确定运动目标的固有特征或特征参数。
这种最小均方误 差准则下的线性 滤波，通常称作
m个参数逐一求导，令等于零
1 h ˆ i X ? zi m b i 1
15
m
b=σ2n/σ2x
标量维纳滤波。
ˆ x )2 ] P E [( X
1 2 n mb
h1 h2 hm
1 mb
自动化学院
而一次取样的均方误差
2 2 2 P E [( x n x ) ] E ( n ) 1 k k n
故这一结果的均方误差约为一次采样的（1-a）/（1+a）倍。
13
自动化学院
NUST
智能信息处理技术
二、线性均方估计
1、最优非递归估计
2、递归估计
14
自动化学院
NUST
智能信息处理技术
28
自动化学院
NUST
智能信息处理技术
3、标量卡尔曼预测器
标量卡尔曼滤波是对掺杂有噪声的随机信号进行线性估计。但经 常要对信号的未来值进行预测，特别是在控制系统中。根据预测 提前时间的多少，把预测分成1步、2步、…、 m步预测， 通常
ˆ (k 1 / k ) 。预测的步数越多， 误差越大。 把1步预测记作 X
3
2、扩展Kalman滤波应用于时间非线性的动态系统。 自动化学院
NUST
智能信息处理技术
卡尔曼滤波器
卡尔曼滤波器的应用:
通信、雷达、导航、自动控制等领域 航天器的轨道计算、雷达目标跟踪、生产过程的自动控制等
对机动目标跟踪中具有良好的性能
：卡尔曼滤波器的应用特点
为最佳估计并能够进行递归计算 只需当前的一个测量值和前一个采样周期的预测值就能进行状态估计
最后有递归估值器：
26
ˆ (k ) aX ˆ (k 1) b(k )[z(k ) acX ˆ (k 1)] X
自动化学院
NUST
智能信息处理技术
滤波器增益b(k) ？
2 2 1 b(k ) cP ( k )[ c P ( k ) 1 1 n]
2 2 P ( k ) a P ( k 1 ) 1 w
23
Biblioteka Baidu
自动化学院
NUST
智能信息处理技术
x(k)的均值和方差分别为：
E[ [x E x( (k k )] )] 0 0
2 D[ x(k )] E[ x (k )] Px ?x x (0)
2 2
2 w
1 a2
自相关函数
j E[ x(k ) x(k j)] Px ( j ) ? a Px (0) x
x，
式中： x —恒定信号或称被估参量 nk —观测噪声采样
7
自动化学院
NUST
智能信息处理技术
h1, h2, …, hm是滤波器的脉冲响应hj的采样，或称滤波器的加权系数。滤 波器的输出
ˆ hi zi X
i 1
m
h1=h2=…=hm=1/m
m 1 ˆ z X i m i 1
2
自动化学院
NUST
智能信息处理技术
状态估计的主要方法：
这些方法针对匀速或 匀加速目标提出，如 目标真实运动与采用 的目标 模型不致， 滤波器发散。
1、α-β滤波 2、α-β-γ滤波
3、卡尔曼滤波
：算法的改进及适应性
状态估计难点：
机动目标的跟踪
1、自适应α-β滤波和自适应Kalman滤波均改善对机动目标的跟踪能力。
OR
分成二项
ˆ b z b z X k 1 k 1 i k 1 k 1
i 1
k
ˆ X ˆ b (z X ˆ ) X k 1 k k 1 k 1 k
第一项同时乘、除一个bk
k b bk 1 ˆ k 1 ˆ X b z b z X k bk 1 zk 1 k 1 k i k 1 k 1 bk i 1 bk
10
自动化学院
NUST
智能信息处理技术
2、递归估值器
一阶递归估值器：
yk zk ＋ ∑ z－ 1 1－a ˆ X k
＋
yk ayk 1 zk zk x nk
11
a
a为滤波器的加权系数，a<1。
式中，zk与非递归情况相同；a是一个小于1 的滤波器加权系数， 如果它大于或等于1， 该滤波器就不稳定了。
其中，α(k)和β(k)可以通过使预测均方误差最小来确定。预测的均方误差可表示为
2 ˆ (k 1/ k )]2 P ( k 1 / k ) E [ e ( k 1 / k )] E [ x ( k 1 / k ) X
将预测方程代入该式，并求导，就会得到一组正交方程：
ˆ ( k / k 1)] 0 E [ e( k 1 / k ) X E[e( k 1 / k ) z ( k )] 0
这里讨论1步预测问题。
信号模型和观测模型同前：
x(k ) ax(k 1) w(k 1) z (k ) cx(k ) v(k )
29
自动化学院
NUST
智能信息处理技术
根据前一节， 有一步线性预测递推公式：
ˆ (k 1/ k ) (k ) X ˆ (k / k 1) (k ) z(k ) X
30
自动化学院
NUST
智能信息处理技术
解之，得 将其代入预测方程，有
a(k)=aa(k)=？cβ(k)
NUST
智能信息处理技术
2、由最优非递归估计导出递归估计
由前可知， 非递归估值器可以表示为
ˆ k hi zi hi (k ) zi X
i 1 i 1
k
k
1 2 P ( k ) n k b
k+1次取样
ˆ k 1 hi zi hi (k 1) zi X
bk 1


ˆ X k 1
z
递 归 公 式
1
20
X k 1 X k bk 1 ( zk 1 X k )
^
^
^
自动化学院
NUST
智能信息处理技术
应用时要注意初始条件，即递推开始时的初始值
ˆ 0最佳，递 为使 X 推初始条件
ˆ 0 )2 ] E[( x X 0 ˆ X 0
ˆ 该式表明，估计 X
是用m个采样值的 平均值作为被估参 量x的近似值的，故 称其为采样平均估
值器。
8
自动化学院
NUST
智能信息处理技术
均方误差估计
2 m m 2 1 ˆ ( n P E E( ( ) ) E E (X X x x) )2 ? ˆ P E n n 2 j i m j 1 i 1 m
i 1 i 1
k 1
k 1
1 2 P (k 1) n (k 1) b
16
自动化学院
NUST
智能信息处理技术
b=σ2n/σ2x及hi(k)=1/（k+b）
hi (k ) hi (k 1)
P (k )

2 n
bk bk 1
P (k 1)

2 n
自动化学院
NUST
智能信息处理技术
k时刻的输出：
yk=ak-1z1+ak-2z2+…+azk-1+zk
将zk中的信号和噪声分开，并代入，有输出
k 1 ak yk x a k i ni 1 a i 1
由于│a│＜1，故随着k值的增加，yk趋近于x/(1-a)。这样，如果以(1-a)yk作为x的