卡尔曼滤波器

1、第二章 维纳滤波和卡尔曼滤波 第二章 维纳滤波和卡尔曼滤波 2.1 引言引言 2.2 维纳滤波器的离散形式维纳滤波器的离散形式时域解时域解 2.3 离散维纳滤波器的离散维纳滤波器的z域解域解 2.4 维纳预测维纳预测 2.5 卡尔曼卡尔曼(Kalman)滤波滤波 第二章 维纳滤波和卡尔曼滤波 2.5 卡尔曼卡尔曼(Kalman)滤波滤波 卡尔曼滤波是用状态空间法描述系统的，由状态方程和量测方程所组成。卡尔曼滤波用前一个状态的估计值和最近一个观测数据来估计状态变量的当前值， 并以状态变量的估计值的形式给出。卡尔曼滤波具有以下的特点： (1) 算法是递推的，且状态空间法采用在时域内设计滤波器的方法

2、，因而适用于多维随机过程的估计；离散型卡尔曼算法适用于计算机处理。 (2) 用递推法计算，不需要知道全部过去的值，用状态方程描述状态变量的动态变化规律，因此信号可以是平稳的，也可以是非平稳的， 即卡尔曼滤波适用于非平稳过程。 (3) 卡尔曼滤波采取的误差准则仍为估计误差的均方值最小。 第二章 维纳滤波和卡尔曼滤波 2.5.1 2.5.1 卡尔曼滤波的状态方程和量测方程卡尔曼滤波的状态方程和量测方程 假设某系统k时刻的状态变量为xk，状态方程和量测方程（也称为输出方程）表示为 kkkkxAx1(2.5.1a) kkkkvxCy(2.5.1b) 其中，k表示时间，这里指第k步迭代时，相应信号的取值

3、；输入信号k是一白噪声，输出信号的观测噪声vk也是一个白噪声, 输入信号到状态变量的支路增益等于1，即B=1；A A表示状态变量之间的增益矩阵，可以随时间发生变化，用Ak表示第k步迭代时， 增益矩阵A的取值； 状态方程量测方程第二章 维纳滤波和卡尔曼滤波 C C表示状态变量与输出信号之间的增益矩阵，可以随时间变化， 第k步迭代时， 取值用Ck表示，其信号模型如图2.5.1所示。 将状态方程中时间变量k用k-1代替，得到的状态方程和量测方程如下所示： x xk=A Ak-1xk-1+k-1 yk=Ckxk+vk 其中，xk是状态变量;k-1表示输入信号是白噪声; vk是观测噪声; yk是观测数据

4、。 第二章 维纳滤波和卡尔曼滤波 图 2.5.1 卡尔曼滤波器的信号模型 z1Ak1Ckk1xk1xkvkyk第二章 维纳滤波和卡尔曼滤波 为了后面的推导简单起见，假设状态变量的增益矩阵A不随时间发生变化，k,vk都是均值为零的正态白噪声，方差分别是Qk和Rk，并且初始状态与k,vk都不相关，表示相关系数。即kjkvvkvkkkjkkkkRRvEvQQEjkjk,2,2, 0:, 0:其中 jkjkkj01第二章 维纳滤波和卡尔曼滤波 2.5.2 2.5.2 卡尔曼滤波的递推算法卡尔曼滤波的递推算法 卡尔曼滤波是采用递推的算法实现的，其基本思想是先不考虑输入信号k和观测噪声vk的影响，得到状态

5、变量和输出信号（即观测数据）的估计值，再用输出信号的估计误差加权后校正状态变量的估计值，使状态变量估计误差的均方值最小。 因此, 卡尔曼滤波的关键是计算出加权矩阵的最佳值。 当不考虑观测噪声和输入信号时，状态方程和量测方程为 11 kkkkkkkkkxACxCyxAx(2.5.4) (2.5.5) 第二章 维纳滤波和卡尔曼滤波 显然，由于不考虑观测噪声的影响，输出信号的估计值与实际值是有误差的，用 表示 kykkkyyy(2.5.6) 为了提高状态估计的质量，用输出信号的估计误差 来校正状态变量 ,加上后面的校正项ky)()(111kkkkkkkkkkkkkxACyHxAyyHxAx(2.5.

6、7) 其中，H Hk为增益矩阵，实质是一加权矩阵。经过校正后的状态变量的估计误差及其均方值分别用 和Pk表示，把未经校正的状态变量的估计误差的均方值用 表示 kxkP第二章 维纳滤波和卡尔曼滤波 )()(TTkkkkkkkkkkkkkxxxxEPxxxxEPxxx(2.5.8) (2.5.9) (2.5.10) 卡尔曼滤波要求状态变量的估计误差的均方值Pk为最小， 因此卡尔曼滤波的关键就是要得到Pk与H Hk的关系式，即通过选择合适的H Hk,使Pk取得最小值。 首先推导状态变量的估计值 和状态变量的估计误差 ， 然后计算 的均方值Pk ，并通过化简Pk ，得到一组卡尔曼滤波的递推公式。 kx

7、 kx kx 第二章 维纳滤波和卡尔曼滤波 将（2.5.3）、 (2.5.5)式代入（2.5.7）式 kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkvHxACHxACHIvHxCHxACHIxACvxCHxAyyHxAx)()()()()(1111111(2.5.11) 同理，状态变量的估计误差 为 xkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkvHxxACHIvHCHIxxACHIvHACHxxACHxxAvHxACHxACHIxAxxx11111111111111111)()()()()()()()(2.5.1

8、2) 第二章 维纳滤波和卡尔曼滤波 由上式可以看出，状态变量的估计误差 由三部分组成， 可记为 xcbax其中 kkkkkkkkkkvHcCHIbxxACHIa111)()()(2.5.13b) (2.5.13c) (2.5.13d) 那么，状态变量的估计误差的均方值Pk就由9项组成:)(,TTTTTTTTTTTcbcabcbaacabccbbaaEcbacbaExxEPkkk(2.5.14a) 第二章 维纳滤波和卡尔曼滤波 其中 TkTkkkkkkkkkvHcCHIbCHIAxxaTTT1TTTT11T)()()(2.5.14b) (2.5.14d) (2.5.14c) 下面化简Pk的表达式

9、，根据假设的条件，状态变量的增益矩阵A A不随时间发生变化，起始时刻为k0，则（2.5.2）式经过迭代， 得到 00011)(kkllkkkklkAxAx令l=k-k0-j，得到 10010000)(kkjjkkkkkkjkAxAx第二章 维纳滤波和卡尔曼滤波 取k0=0，k=k-1，得到 jkjjkkkAxAx202011(2.5.15) 所以xk-1仅依赖于x0,0, 1,k-2,与k-1不相关，即 0T11T11kkkkxexE(2.5.16) 又据(2.5.7)式和(2.5.3)式， 得 )(2111111211kkkkkkkkkkxACvxCHxAx(2.5.17) 第二章 维纳滤波

10、和卡尔曼滤波 所以 仅依赖于xk-1,vk-1，而与vk不相关，即 1kx0)()(0)()(T111T111T11T11kkkkkkkkkkkkxxExxExxvEvxxE(2.5.18) (2.5.19) 把（2.5.15）（2.5.19）式代入(2.5.14)式, Pk中的9项可以分别化简为 TT1TTT1111T)()()()()(kkkkkkkkkkkkkkkkkCHIAPACHICHIAxxxxACHIEaaET1TT11T)()()()(kkkkkkkkkkkCHIQCHICHICHIEbbE(2.5.20a) (2.5.20b) 第二章 维纳滤波和卡尔曼滤波 0)(0)()(0

11、)(0)()()(0)()(0)()()(TT1TTTTTT11TTT1TTTT111TTT11TT11T111TTTkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkTkkkCHIvHEcbECHIAxxvHEcaEHvCHIEbcECHIAxxCHIEbaEHvxxACHIEacExxxxACHIEabEHRHHvvHEccE(2.5.20c) (2.5.20d) (2.5.20e) (2.5.20f) (2.5.20g) (2.5.20h) (2.5.20j) 第二章 维纳滤波和卡尔曼滤波 也就是说，Pk仅有其中的三项不为零， 化简成 Tkkkkkk

12、TkkkkkTkkkkkkkkkkTkkkkkkHRHCHIQAPACHIHRHCHIQCHICHIAPACHIccEbbEaaEPT11T1T1TTT)()()()()()(2.5.21) 第二章 维纳滤波和卡尔曼滤波 为了进一步化简Pk，推导未经误差校正的状态估计误差的均方值Pk，由下面推导结果可以看出，Pk是一对称矩阵，满足Pk=(Pk)T。 1T1T11TT1111T111111T111111T)()()()()(defkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkQAPAEAxxxxEAxxAxxAExAxAxAxAExxxxEP(2.5.22) 第二章 维纳

13、滤波和卡尔曼滤波 将(2.5.22)式代入(2.5.21)式，即把Pk代入Pk, TTTTTTTT)()()(kkkkkkkTkkkkkkkkkkkkkkTkkkkkkkkkkkkkkkHRCPCHHCPPCHPHRHHPCHHCPPCHPHRHCHIPCHIP(2.5.23) 其中, 是正定阵，记 kTkkkRCPCTSSRCPCkTkkk(2.5.24) 令 T TTT)(kkkkkkPCPCCPU(2.5.25) 第二章 维纳滤波和卡尔曼滤波 定义:设ACnn是Hermite矩阵,如果对任意0 xCn,都有 xHAx0,则A是Hermite正定阵; 若xHAx0,则A是Hermite半正

14、定阵.定理:设A Cnn 是Hermite矩阵,则下列条件等价(1)A是Hermite矩阵,AH=A(2)A的特征值全为正实数(3)存在矩阵P Cnn,使得A=PHP第二章 维纳滤波和卡尔曼滤波 TTTT)(kkTkkkkkHSSHUHUUUHPP将上式代入（2.5.23）式，得 (2.5.26) 将(2.5.26)式后三项配对 1TTT1T1TT1TT1T1T)()()()()()(kkkkkkkkkkkkkkkPCPCPCCPPSUSHSUSHUSSUPSUSHSUSHP(2.5.27) 第二项和第三项均与H Hk无关，第一项为一半正定阵，因此使Pk最小的Hk应满足 1T1T11Topt1

15、T)()()(0)(kkkkTkkkRCPCCPSSUSSUHSUSH(2.5.28) (2.5.29) 第二章 维纳滤波和卡尔曼滤波 将Hopt代入Pk，得到最小均方误差阵 1TT)()(kkkkkkkkkkkkkkkkkPCHIPCHPPCRCPCCPPP 将(2.5.7)、 (2.5.22)、 (2.5.29)式和(2.5.30)式联立， 得到一组卡尔曼递推公式 1T11TT11)()()(kkkkkkkkkkkkkkkkkkkkkkkkPCHIPQAPAPRCPCCPHxACyHxAx(2.5.30) (2.5.31a) (2.5.31b) (2.5.31c) (2.5.31d) 第二

16、章 维纳滤波和卡尔曼滤波 假设初始条件A Ak,C Ck,Q Qk,R Rk,y yk,xk-1, Pk-1已知，其中x0=Ex0, P0=varx0, 那么，递推流程见图2.5.2。 图 2.5.2 卡尔曼滤波递推流程 (2.5.31c)式(2.5.31b)式(2.5.31a)式(2.5.31d)式PkHkxkPkxk1 , Pk1第二章 维纳滤波和卡尔曼滤波 离散kalman滤波算法 状态方程 量测方程 统计特性Ewk=0,Evk=0,covwk,vj,covvk,vj covwk,vj=0 初始条件Ex0,varx0,covx0,wk=0,covx0,vk=0 递推公式 增益方程 均方误

17、差kkkkvxCyx xk=A Ak-1xk-1+k-1 1T11TT11)()()(kkkkkkkkkkkkkkkkkkkkkkkkPCHIPQAPAPRCPCCPHxACyHxAx第二章 维纳滤波和卡尔曼滤波 例例2.5.1 已知 0, 1)(,)8 . 01)(8 . 01 (36. 0)(1vvvxxmzSzzzS信号与噪声不相关，yk=xk+vk,求卡尔曼信号模型中的Ak和Ck。 解解 由yk=xk+vk知道，C Ck=1。 对Sxx(z)进行谱分解，确定x(n)的信号模型B(z)，从而确定Ak。 根据Sxx(z) =2B(z)B(z-1)，得出 )() 1(8 . 0)(8 . 0

18、11)(,36. 012nnxnxzzB第二章 维纳滤波和卡尔曼滤波 上式与卡尔曼状态方程相比，不同之处在于输入信号(n)的时间不同，因此将Sxx(z)改写为 zzzzzSxx8 . 018 . 0136. 0)(11再对Sxx(z)进行谱分解，得到 kkkxxnnxnxzzzB8 . 0) 1() 1(8 . 0)(8 . 01)(111(解毕) 第二章 维纳滤波和卡尔曼滤波 卡尔曼滤波和维纳滤波都是采用均方误差最小的准则来实现信号滤波的，但维纳滤波是在信号进入了稳态后的分析， 卡尔曼滤波是从初始状态采用递推的方法进行滤波。对于平稳随机信号，当过渡过程结束以后，卡尔曼滤波与维纳滤波的结果间存

19、在什么关系呢? 下面举一例说明。 第二章 维纳滤波和卡尔曼滤波 例例2.5.22.5.2 已知1var, 0, 0)(, 1)(,)8 . 01)(8 . 01 (36. 0)(0011xPxzSzSzzzSvxvvxx在k=0时开始观察yk, yk=xk+vk，用卡尔曼过滤的计算公式求xk， 并与维纳过滤的方法进行比较。 解解 (1) 由x(n)功率谱及量测方程，确定卡尔曼递推算法。 首先对Sxx(z)进行功率谱分解，由例2.5.1的结果，得到卡尔曼滤波的状态方程为 xk=0.8xk-1+k-1,确定Ak=0.8 第二章 维纳滤波和卡尔曼滤波 由量测方程yk=xk+vk, 确定Ck=1， 1

20、)0(2vvvvkSP将参数矩阵Ak,Ck,Rk代入卡尔曼递推公式（2.5.30），得到 1111)1 (36. 064. 0) 1()8 . 0(8 . 0kkkkkkkkkkkkkPHPPPPPHxyHxx(2.5.32a) (2.5.32b) (2.5.32c) (2.5.32d) 第二章 维纳滤波和卡尔曼滤波 (2) 求出卡尔曼滤波的输出。由卡尔曼递推公式，以及 ,P0=varx0=1, 可得到Pk,Hk,Pk及xk（k表示迭代次 数 ） ， 迭 代 流 程 为 ： 由具体迭代结果可以看出，原先的增益矩阵Ak,由于只选择了一个状态变量， 变成了加权系数。 见表2.5.1。 01xkkk

21、kkPdxaHbPcP32. 5 . 232. 5 . 232. 5 . 232. 5 . 21第二章 维纳滤波和卡尔曼滤波 表表2.5.1 Kalman滤波迭代结果滤波迭代结果 第二章 维纳滤波和卡尔曼滤波 (3) 求出卡尔曼滤波的稳态解。将(2.5.32b)式代入方程(2.5.32d), 得到第5个方程 kkkkkkkkkkkkkHPPPPPPPPPPHP) 1(1)() 1() 1(1 )1 (21(2.5.32e) 将方程(2.5.32c) 、(2.5.32e) 代入方程(2.5.32d), 消去Pk， 可以得到Pk的递推关系: Pk=(1-Pk)0.64 Pk-1+0.36=0.64

22、 Pk-1-0.64 Pk -1 Pk +0.36-0.36 Pk 第二章 维纳滤波和卡尔曼滤波 化简上式，得到 1.36Pk+0.64Pk-1Pk=0.64Pk-1+0.36 36. 164. 036. 064. 011kkkPPP要求的是稳态解，因此将Pk, Pk-1都用P代替, 得到 036. 072. 064. 0036. 064. 036. 164. 022PPPPP8364. 0236. 064. 0472. 072. 02P第二章 维纳滤波和卡尔曼滤波 根据P，可以确定达到稳态后的卡尔曼滤波的状态方程: kkkkkkkkkkkyxxyxxPPPHPP835 . 0)8 . 0(8

23、38 . 0836 . 16 . 016 . 036. 064. 01111(2.5.33) 第二章 维纳滤波和卡尔曼滤波 (4) 用维纳滤波的方法分析。采用功率谱分解的方法，得到x(n)的时间序列信号模型的传输函数H(z): 15 . 0183)()()(zzYzXzH第二章 维纳滤波和卡尔曼滤波 上式说明x是一阶AR模型，对H(z)做Z反变换得到 )21(83875. 085 . 183)2() 1 ()0()2( 218385 . 183) 1 ()0() 1 ( 83)0()0()0( )()()( )( )()5 . 0(83)(120120120101010*yyyyyyhyhyh

24、xyyyyyhyhxyyhxmnymhnxnsnunhmn第二章 维纳滤波和卡尔曼滤波 nmnnyymnymhnxyyyyyyyhyhyhyhx0123012301232183)()()( 21838375. 0875. 085 . 183) 3()2() 1 ()0() 3( (2.5.34) 比较（2.5.33）式和（2.5.34）式，可以看出卡尔曼滤波的稳态解与维纳解是相等的。 (解毕) 第二章 维纳滤波和卡尔曼滤波 通过上面的例题，可以看出维纳滤波是已知前p个观测数据及信号与噪声的相关函数，通过建立模型的方法分析的。卡尔曼滤波要求已知前一个时刻的状态估计值x(k-1)和当前的观测值yk

25、，由状态方程和量测方程递推得到结果。 维纳滤波的解以H(z)的形式给出，卡尔曼滤波是以状态变量的估计值给出解的形式。它们都采用均方误差最小的准则, 但卡尔曼滤波有一个过渡过程，其结果与维纳滤波不完全相同，但到达稳态后， 结果相同。 第二章 维纳滤波和卡尔曼滤波 2.5.3 2.5.3 应用举例应用举例 下面举一个雷达跟踪目标物的例子说明卡尔曼滤波的应用。 雷达跟踪目标的基本原理是通过发射脉冲，根据接收到的脉冲与发射脉冲的时间间隔，来确定目标物的距离和速度。 由于干扰的影响，接收到的脉冲波形变化很大，那么一次的测量结果可能存在很大的误差。为了减小误差，往往采取发射一串脉冲的方法进行测量。 第二章

26、 维纳滤波和卡尔曼滤波 我们知道，空间中的一点需要由径向距离和方位角来确定。 假设雷达跟踪的目标为飞行器，发射的脉冲时间间隔为T，在时间k，径向距离为R+(k)，在时间k+1，距离为R+(k+1)， 两者之间有秒的延时，因此T表示空间一次扫描的时间间隔。 平均距离用R表示，(k)和(k+1)表示对平均值的偏差。 假定偏差是统计随机的，均值为零, 那么可以写出距离方程 )()() 1(kTkk式中， 表示速度。用u表示加速度，则可以得到加速度方程 )(k)() 1() 1(kkkTu第二章 维纳滤波和卡尔曼滤波 假定加速度u(k)是零均值的平稳白噪声，即满足 22)(0)() 1(ukuEkuk

27、uE 为了后面叙述方便，定义x(k)表示第k个雷达回波脉冲获得的目标距离，z(k)表示在第k个雷达脉冲进行数据处理之后的目标距离估计，z(k)表示在第k个雷达脉冲进行数据处理之后的目标速度估计。设定状态变量为x(k)，选择的状态变量有4个， 分别表示径向距离、径向速度、方位角和方位角速度，即 .T4321)(),(),(),()(),(),(),()(kkkkkxkxkxkxkX（2.5.35） 第二章 维纳滤波和卡尔曼滤波 根据状态变量的物理含义，得到以下方程： )()() 1()()() 1()()() 1()()() 1(244433122211kukxkxkTxkxkxkukxkxkT

28、xkxkx式中，u1(k)和u2(k)表示在区间T径向速度和方向的变化。 将上式写成矩阵形式 )(0)(0)()()()(10001000010001) 1() 1() 1() 1(2143214321kukukxkxkxkxTTkxkxkxkx第二章 维纳滤波和卡尔曼滤波 由此得到卡尔曼滤波信号模型的状态方程 )()(), 1() 1(kkXkkAkX（2.5.36） 再来看量测方程， 距离和方向的估计值为 )()()()()()(232111kvkxkzkvkxkz这里v1(k), v2(k)为观测偏差，将上两式分别写成向量形式和矩阵形式: Z(k)=CX(k)+V(k) )()()()(

29、)()(01000001)()(21432121kvkvkxkxkxkxkzkz（2.5.37a） （2.5.37b） 第二章 维纳滤波和卡尔曼滤波 观测噪声V(k)假定为高斯噪声，均值为0，方差为2和2 。 状态方程激励信号的协方差阵为 2221TT00000000000000)()()()()()(kkEkQkQjkEkj(2.5.38) 其中， ,为径向加速度在T时刻的方差； , 为角加速度在T时刻的方差。 2121uE2222uE第二章 维纳滤波和卡尔曼滤波 量测方程的噪声协方差阵 )(00)()()()()()()()()()()()()(22222222122121TT2,21 ,

30、22, 11 , 1kkkvEkvkvEkvkvEkvEkvkvEkRkRjvkvEvvvvkj(2.5.39) 第二章 维纳滤波和卡尔曼滤波 为了简单起见，假定在各个方向，加速度服从均匀分布， 其概率密度函数 ，并将u的值限制在M之间， 那么， 加速度的方差 Muf21)(2231Mu根据误差信号协方差阵P(k)的定义 P(k)=Ee(k)eT(k) 可以计算出，单个信号的均方误差和两个信号的协方差矩阵分别为 )()()()()()()()()()()()()()(2, 21 , 22, 11 , 1221221212111kpkpkpkpkeEkekeEkekeEkeEEkPkeEkpkP

31、第二章 维纳滤波和卡尔曼滤波 根据接收到的相邻两个回波脉冲，可以测量出飞行器的距离z1(1)和z1(2)，方位角z2(1)和z2(2)。根据这四个数据，用(2.5.35)式计算状态变量 )1 ()2(1)2()()2()2()()1 ()2(1)2()()2()2( )(2242311111zzTkxzkxzzTkxzkxT222111) 1 ()2(),2(,) 1 ()2(),2()(TzzzTzzzkX因此，k时刻的状态向量 可以写为 )(kX第二章 维纳滤波和卡尔曼滤波 取k=2,将上式中的观测信号z1(1),z1(2),z2(1),z2(2)用(2.5.37)式代入，得到状态变量 T

32、vvxvxTvvxvxTvxvxvxTvxvxvxX) 1 ()2() 1 ()2()2() 1 ()2() 1 ()2()2()1 () 1 ()2()2()2()2()1 () 1 ()2()2()2()2()2(3343311211333333111111（2.5.40） 根据(2.5.35)式, k=2时的状态向量为 ) 1 () 1 ()2() 1 () 1 ()2()2(243121uxxuxxX（2.5.41） 第二章 维纳滤波和卡尔曼滤波 计算k=2时刻的协方差阵 )2()2()2()2()2(TXXXXEP式中 TvvuvTvvuvXX) 1 ()2() 1 ()2(0) 1 ()2() 1 ()2(0)2()2(11221111第二章 维纳滤波和卡尔曼滤波 因此，误差协方差阵是44阶矩阵，假设噪声源u和v是独立的， 则协方差阵为 444334332221121100000000)2(ppppppppP其中 22224424334233212222221122112,2,TpTpppTpTpppppp第二章 维纳滤波和卡尔曼滤波 2.5.4 2.5.4 发散问题及其抑制发散问题及其抑制 从理论上讲