维纳滤波和卡尔曼滤波_3

由该方程解出：
rxx (1) a1,1 rxx (0)
12 (1 a12,1 )rxx (0)
然后增加一阶，即令p=2，得到
2 rxx (0) rxx (1) rxx (2) 1 2 rxx (1) rxx (0) rxx (1) a2,1 0 r (2) r (1) r (0) a 0 xx xx xx 2, 2
由上面方程解出：
2 2 2 a2, 2 [rxx (0)rxx (2) rxx (1)] /[rxx (0) rxx (1)]
[rxx (2) a1,1rxx (1)] / 12
2 2 a2,1 [rxx (0)rxx (1) rxx (1)rxx (2)] /[rxx (0) rxx (1)]
vk
xk
Ck xk-1
sk
yk
k-1
xk
z-1
Ak
图 3.5.1 卡尔曼滤波器的信号模型
为了分析方便, 假设状态变量的增益矩阵A不随时间发生变化，
wk,vk 都是零均值白噪声，方差分别是 Qk 和 Rk ，并且初始状态 x0 与wk,vk都不相关，且噪声向量wk,vk也互不相关，即
2 T wk : E[ wk ] 0, w Qk , E w w k j Qk kj T vk : E[vk ] 0, v2 Rk , E v v k j Rk kj Cov x0 , wk 0; Cov x0 , vk 0 T E[ wk vk ]0
ˆ k 1 仅依赖于xk-1,vk-1，而与vk不相关，即 x
T ˆk 1 )vk ˆk 1 ) T ] 0 E[(xk 1 x ] E[vk ( xk 1 x
ˆk 1 )kT1 ] E[k 1 ( xk 1 x ˆk 1 ) T ] 0 E[(xk 1 x
j 0 k 1
xk-1仅依赖于x0,ω0, ω1,…,ωk-2,与ωk-1不相关，即
T T E[ xk 1k ] E [ x 1 k 1 k 1 ] 0
又
' ˆk 1 Ak 1 x ˆk 2 H k 1 ( yk 1 y ˆk x 1 ) ˆk 2 H k 1 (Ck 1 xk 1 vk 1 Ck 1 Ak 1 x ˆk 2 ) Ak 1 x
卡尔曼滤波要求状态变量的估计误差的均方值Pk为最小， 因
此卡尔曼滤波的关键就是要得到 Pk 与Hk 的关系式，即通过选择
合适的Hk,使Pk取得最小值。
ˆk min Pk H k x
递推步骤：
ˆk ： 1. 计算状态变量的估计值 x
' ˆ ˆ ˆ xk Ak xk 1 H k ( yk yk ) ˆk 1 H k (Ck xk vk Ck Ak x ˆk 1 ) Ak x ˆk 1 H k Ck xk H k vk ( I H k Ck ) Ak x
计算得到 a1=-0.8， a2=0, σ
2 ω =0.36
（2）、如果取p=3，可计算出a1=-0.8, a2=a3=0, σ2ω=0.36，说明 AR模型的阶数只能是一阶的。 （ 3 ）、采用谱分解的方法，即对 Sxx(z) 进行谱分解，得到的模 型也是一阶的，其时间序列模型和差分方程为
1 B( z ) 1 0.8 z 1 x(n) (n) 0.8 x(n 1)
T ˆk 1 )vk E[ac T ] E ( I H k Ck ) Ak ( xk 1 x H kT ]
ˆk 1 )T AkT ( I H k Ck ) T ] E[ba T ] E ( I H k Ck )k 1 ( xk 1 x ˆk 1 )T AkT ( I H k Ck )T ] E[ca T ] E ( H k vk ( xk 1 x
T T c T vk Hk
3. 计算状态变量估计误差的均方值Pk ：
T k x k Pk E[ x ] E[(a b c)(a b c)T ]
E[aa bb cc ab ac ba bc ca cb ]
T T T T T T T T T
Levinson-Durbin算法
Levinson-Durbin 算法首先由一阶 AR 模型开始，一阶 AR 模型 (p=1)的Yule-Walker为
rxx (0) rxx (2) 1 12 rxx (1) rxx (0) a1,1 0
E[cc ] E[H v v Hk ] Hk Rk H
T T k k k
T k
T E[bc T ] E ( I H k Ck )k 1vk H kT ] 0
E[cb T ] E ( H k vk kT1 ( I H k Ck )T ] 0 ˆk 1 )kT1 ( I H k Ck )T ] E[ab T ] E[( I H k Ck ) Ak ( xk 1 x
k xk x ˆk x
T ˆ min E x x x k k k
2、卡尔曼滤波的状态方程和量测方程
假设某系统 k 时刻的状态变量为 xk ，状态方程和量测方程
（也称为输出方程）表示为
xk Ak xk 1 wk 1 yk Ck xk vk
Ak为状态转移矩阵，描述系统状态由时间 k-1 的状态到时间 k 的状态之间的转移； Ck为量测矩阵，描述状态经其作用，变成可量测或可观测的； xk为状态向量，是不可观测的；yk为观测向量； wk为过程噪声；vk为量测噪声。
p 1
k p a p, p a p ,k a p 1,k k p a p 1, p k k 1,2,3, , p 1
(1 k )
2 p 2 p
2 p 1
02 rxx (0) E[ x 2 (n)]
例2.4.2 已知
0.36 S xx ( z ) (1 0.8 z 1 )(1 0.8 z )
其中，
ˆk 1 )( xk 1 x ˆk 1 )T AkT ( I H k Ck )T E[aa T ] E[( I H k Ck ) Ak ( xk 1 x ( I H k Ck ) Ak Pk 1 AkT ( I H k Ck )T
E[bbT ] E[( I H k Ck )k 1kT1 ( I H k Ck )T ] ( I H k Ck )Qk 1 ( I H k Ck )T
a1,1 a2, 2 a1,1
2 2 2 2 (1 a2 ) ,2 1
然后令p=3, 4, …, 以此类推， 可以得到Levinson-Durbin 的一般递推公式如下：
kp
[rxx ( p) a p 1rxx ( p k )]
k 1 2 p 1

其中
1 kj 0
k j k j
3、 卡尔曼滤波的递推算法
基本思想：
先不考虑输入信号 ωk和观测噪声vk的影响，得到状态变量和 ˆ 'k 输出信号（即观测数据）的估计值 x ˆ 'k 和 y
再用输出信号的估计误差 y k 加权后校正状态变量的估计 值 x ˆk ，使状态变量估计误差 ~ xk 的均方值最小。
3.5 卡尔曼(Kalman)滤波
本节讨论的主要问题及方法 卡尔曼滤波的状态方程和量测方程 卡尔曼滤波的递推算法 发散问题及其抑制

1、本节讨论的主要问题及方法

讨论的主要问题：本节将主要讨论卡尔曼滤波
的状态方程和量测方程，及其递推算法。
解决方法：利用状态方程和递推方法寻找最小 ˆk ，即 均方误差下状态变量 xk 的估计值 x
输出信号的估计误差（新息）为：
' ~ ˆk yk yk y
为了提高状态估计的质量，用输出信号的估计误差 ~ yk 来校
正状态变量
' ˆ ˆ ˆ xk Ak xk 1 H k ( yk yk ) ˆk 1 H k ( yk Ck Ak x ˆk 1 ) Ak x
x(n)为AR模型，求AR模型参数（包括模型阶数和系数）。
解 首先对Sxx(z)做傅里叶反变换，得到x(n)的自相关函数rxx(m), rxx(m)=0.8|m|
（1）、采用试验的方法确定模型阶数 p。首先取p=2，各相 关函数值由上式计算
1 0.8 0.64 1 2 0.8 a1 0 0.8 1 a 0 0 . 64 0 . 8 1 2
根据假设的条件，状态变量的增益矩阵 A不随时间发生变化，
起始时刻为0，则
x1 Ax0 0 x2 Ax1 1 A2 x0 A0 1
k 2 j 0

xk 1 Ak 1 x0 Ak 2 j j xk Ak x0 Ak 1 j j
其中，Hk为增益矩阵，实质是一加权矩阵。 校正后状态变量的估计误差及其均方值分别为：
k xk x ˆk x
T T ˆ ˆ Pk E x x E [( x x )( x x ) ] k k k k k k
未经校正的状态变量估计误差的均方值为：
' ' ' T ˆ ˆ P E [( x x )( x x k k k k k) ]
' ˆ ˆ ˆ x 'k y 'k yk yk yk 校正 量测方程
ˆk x k xk x ˆk x
T E x x k k min
ˆk x
当不考虑观测噪声和输入信号时，状态方程和量测方程为：