一类线性系统卡尔曼滤波器自适应算法_图文

1、第29卷第4期2007年7月南京工业大学学报Vol . 29No . 4July 2007一类线性系统卡尔曼滤波器自适应算法焦成柱1, 刘国庆2, 章俊1(1. 南京工业大学信息科学与工程学院, 江苏南京2. 南京210009摘要:, 算法, 利用新息序列的方差, . 系统模拟显示, , 而且收敛速度比传.关键词:; ; 中图分类号:O2111文献标识码:A 文章编号:1671-7627(2007 04-0030-04Research on the self 2adapti ve a lgor ith m of the f i rst type Ka l m an f ilter i n li

2、 n ear systemJ I A O Cheng 2zhu 1, L I U Guo 2qing 2, ZHANG Jun1(11College of I nfor mati on Science and Engineering, Nanjing University of Technol ogy, Nanjing 210009, China;2. College of Sciences, Nanjing University of Technol ogy, Nanjing 210009, China Abstract:I n order t o s olve unknown syste

3、m noise variance and observati on noise variance in p ractical linear sys 2te m , a ne w self 2adap tive algorith m f or Kal m an filter was p resented, it made use of the variance of ne w sequences, which could esti m ate and correct the syste m noise variance and the observati on noise variance gr

4、adually in calcula 2ti on p r ocess. The computer si m ulati on shows that the esti m ated variance of the syste m noise and esti m ated variance of the observati on noise converge t o the real values more quickly than the traditi onal one . Key words:Kal m an filter; white noise; self 2adap tiveKal

5、 m an 滤波理论在机动目标(如飞机、船舶、汽车、机器人 的组合导航、控制、无线通信领域等都有着广泛的应用. 使用传统的卡尔曼滤波算法时, 需要确切地知道系统模型、观测模型、状态噪声和观测噪声. 但在实际系统中, 往往很难准确地知道状态噪声和观测噪声, 从而影响了传统的卡尔曼滤波算法的应用. 在已知系统转移矩阵A 和观测矩阵C 的前提下, 本文提出了一种自适应算法来实时估计状态噪声方差阵Q 和观测噪声方差阵R.系统的状态方程1-3B k +1=AB k +GW k (1式中:A 为状态转移矩阵; G 为过程噪声转移矩阵; Bk 为状态值序列; W k 是一个均值为0、方差为Q 的Gauss

6、白噪声序列.观测方程1-3Z k =CB k +V k(2式中:C 为观测矩阵; Z k 是观测序列; V k 是一个均值为0、方差为R 的Gauss 白噪声序列. 并且假设Vk 与Wk 相互独立, 其中A =(a ij n ×n , C =(c ij m ×n , G =(g ij n ×p .3收稿日期:2007203207基金项目:江苏省高校自然科学基金资助项目(03KJB110037作者简介:焦成柱(1981 , 男, 江苏徐州人, 硕士生, 主要研究方向为无线数据通讯技术;刘国庆(联系人 , 教授, E 2mail:guoqingnjut . edu .

7、 cn . 在选取状态转移矩阵A 、过程噪声转移矩阵G 和观测矩阵C 时, 必须满足可观性和可控性, 所以CA 为满秩矩阵2.1噪声估计模型设E =(e ij m ×n , 计算EZ k +1-Z k :由观测方程(1 和(2 可得EZ k +1-Z k =(ECAB k -CB k +(EV k +1-V k +ECGW k令(ECAB k -CB k =0, -=0,因为CA E C +,(CA +为CA . E Z k +1-Z k =(EV k +1-V k +ECGW k(3由Vk 与Wk 相互独立知Wk 与Vk +1独立, 且V k +1与V k 也独立, 则D (EZ

8、k +1-Z k =ECGQ G TC TE T+R +ER E T(4设F =(f ij m ×m , 计算F Z k +2-Z k :同样地, 由观测方程(1 和(2 可得F Z k +2-Z k =(FCA 2-C B k +FCAGW k +FCGW k +1+FV k +2-V k(5令(FCA 2-C B k =0, 即FCA 2-C =0, 可以求出F =C (CA 2+, 由于Wk 与Vk 独立, 且W k 与W k +1, V k +2与V k 也独立, 则D (F Z k +2-Z k =FCAGQG T A T C T F T+FCGQG T C T F T+F

9、RF T+R (6由于观测序列Z k 是已知的, 且通过E =C (CA +和F =C (CA 2 +可求出E 和F , 所以D (EZ k +1-Z k 和D (F Z k +2-Z k 都可以求出, 再根据方程(4和方程(6 求出Q 和R.2噪声估计模型的求解根据文献4中对矩阵的Kr onecker 积和矩阵的拉直的定义, :D (k + 1-Z k D 1D (F +2-Z k 的拉直为D 2, Q 的拉直为, R 的拉直为则式(4 可写成4(ECG ECG Q +(I I +(E E =D 1(7式(6 可写成4(FCAG FCAG +(FCG +(F F +(I I =2(8(7 和

10、式(8 可求出=(F F +I I -1(FCAG FCAG +FCG FCG -(E E +I I -1(ECG ECG -1(F F + I -1D 2-(E E +I -D 1(9这里(7 可求出=E +11(E E +I I -1ECG (10Q 和R, 故最后将和4的逆过程还原成Q 和R.3滤波在通过噪声估计获得状态噪声矩阵Q 和观测噪声矩阵R 之后, 再使用卡尔曼算法进行滤波. 在使用卡尔曼算法之前必须计算2个初始值:状态估计初始值B 0和估计误差方差初始值P 0. 计算B 0和P0的算法如下1:首先观测一组m 个值:Z 1, Z 2, Z 3, , Z m , 再通过公式(11

11、和(12 来计算B0和P 0, 此时观测矩阵C 必须满秩, C +为C 的广义逆矩阵.B 0=(mk =1C +Z k /m (11P 0=(mk =1(C +Z k -B 0 (C +Z k -B 0 T/m(12对观测值进行卡尔曼滤波, 估计状态值, 算法如下:(1 通过上一步的噪声估计可以计算出Q 和R, 故Q 和R 都为常数.(2 依次输入m 个观测值, 再根据式(12 来计算状态估计的初始值B 0, 根据式(12 来计算估计误差方差阵的初始值P0. (3 实时地对系统状态的观测值Z j 进行卡尔曼滤波, 步骤如下1, 3:B (k /k -1 =AB (k -1/k -1 (13P

12、(k /k -1 =AP (k -1/k -1 A T+GQGT(14 K (k =P (k /k -1 C T(CP (k /k -1 C T+R -1(15 B (k /k =B (k /k -1 +K (k (Z (k -CB (k /k -1 (1613第4期焦成柱等:一类线性系统卡尔曼滤波器自适应算法P (k /k =(I -K (k C P (k /k -1 ( 174仿真实例仿真环境:MAT LAB615.仿真参数:观测次数m =400, 采样周期h =0105.状态转移矩阵A =1hh 2/01h1观测矩阵C =10010过程噪声方差阵Q =01000.4. 1本算法仿真过程第

13、一步:噪声方差阵估计(估计Q 和R (1 随机生成状态值和观测值, 根据方程(1 和(2 来生成状态值系列B 1, B 2, , B m 和观测值系列Z 1, Z 2, , Z m .(2 噪声估计:先求E =C (CA +和F =C (CA 2+, 再根据观测值系列可求D (EZ k +1-Z k 和D (F Z k +2-Z k , 再根据式(10 和式(11 求出Q 和 R. Q 和R 的收敛图如图1、2所示.图1过程噪声方差矩阵Q 的收敛图Fig . 1Curves of the p r ocess noise variance matrix Q convergence图2观测噪声方差

14、矩阵R 的收敛图Fig . 2Curves of the observati on noise variance matrix R convergence第二步:对接收到的观测值进行卡尔曼滤波(1 根据方差矩阵Q 和R 随机生成状态值和观测值, 由方程(1 和(2 生成状态值系列B 1, B 2, , B n 和观测值系列Z 1, Z 2 , , Z n .(2 在经过噪声方差阵估计之后得到过程噪声方差阵Q 的估计值和观测噪声方差阵R 的估计值, 然后使用式(11 (17 进行卡尔曼滤波. 状态值和状态的估计值的对比图如图35所示.图3状态向量第一个变量的估计值与真实值比较Fig . 3Com

15、paris on bet w een si m ulated and real values of the first var 2iable of state vect or图4状态向量第二个变量的估计值与真实值比较Fig . 4Comparis on bet w een si m ulated and real values of the secondvariable of state vect or23南京工业大学学报第29卷 图5状态向量第三个变量的估计值与真实值比较Fig . 5Comparis on bet w een si m ulated and real values of t

16、he thirdvariable of state vect or4. 2传统卡尔曼算法滤波Q R 图6, 7为观测噪声方差矩阵R . 2和图6、7对比可以看出, 法, 提出的算法一般在50步之内可以收敛 .图6过程噪声方差矩阵Q 的收敛图Fig . 6Curves of the p r ocess noise variance matrix Q convergence图7观测噪声方差矩阵R 的收敛图Fig . 7Curves of the observati on noise variance matrix R convergence5结论提出了一种新的自适应滤波算法, 通过系统自身的计算来逐步估计并校正状态噪声方差矩阵Q 和观测噪声方差矩阵R ; 通过卡尔曼滤波算法来对接收到的观测值进行滤波, 对真实的状态值进行估计. 计算机仿真表明, 估计的系统噪声方差和观测噪声方差收敛于实际的系统噪声方差和观测噪声方差,