卡尔曼滤波与组合导航课程报告

1、卡尔曼滤波与组合导航?课程实验报告实验捷联惯导/GPS组合导航系统静态导航实验实验序号3生名陈星宇系院专业17班级ZY11172学号ZY1117212日期2021-5-15指导教师宫晓琳成绩、实验目的掌握捷联惯导/GPS组合导航系统的构成和根本工作原理；掌握采用卡尔曼滤波方法进行捷联惯导/GPS组合的根本原理；掌握捷联惯导/GPS组合导航系统静态性能；了解捷联惯导/GPS组合导航静态时的系统状态可观测性；、实验原理系统方程X FX GWU Lh x y z x y z其中E、N、U为数学平台失准角X % N U VE VNVEv VN、VU分别为载体的东向、北向和天向速度误差;L、h分别为纬度

2、误差、经度误差和高度误差;x、 y、 z、 x、 y、z分别为陀螺随3 3Fs3 33 396VE VN天向速度、纬度经度机常值漂移和加速度计随机常值零偏.下E、N、U分别代表东、北、天标系统的噪声转移矩阵 G为：Cb" 3 3cn cb3 3勺39 315 6系统噪声矢量由陀螺仪和加速度计的随机误差组成,表达式为：系统的状态转移矩阵F组成内容为:FsF 069f ,其中FN中非零元素为可由惯导误差模型获得M2量测方程量测变量z VE VN VU L H ,和H分别为捷联解算与GPS的东向速度、北向速度 和月度之 差；量测矩阵 H Hv HP T, HP 03 6 d i ag RM

3、 H, (RN H ) cos L, 036 ,Hv O33 diag 1, 1, 1O39, v VVE Wn Wu VLV V H 为量测噪声.量测噪声差阵R根据GPS的位置、速度噪声水平选取.(3)卡尔曼滤波方程状态一步预测：XVk 1 k, k iX ?k 1状态估计：X k X?k/k 1 Kk (Zk Hk X ?k/k 1)滤波增益：Kk Pk/k IHk T (HkPk/k IHk T Rk),2一步预测均方误差：Pk/k 1 k, k三、实验内容及步骤1、实验内容捷联惯导/GPS组合导航系统静态导航实验；2、实验步骤2实验准备(由实验教师操作)将IMU安装在大理石实验台上,确

4、认IMU的安装基准面靠在大理石实 验 平台上的方位基准尺上；将IMU与导航计算机导航计算机与稳压电源'导航计算机与监控计 算 机GPS接收机与导航计算机'GPS天线与GPS接收机GPS接收机与GPS电池 之间的连接线正确连接；翻开GPS信号转发器； 翻开监控计算机中的监控软件； 翻开稳压电源开关,确认稳压电源输出为28V ;翻开捷联惯导/GPS组合实验系统电源,实验系统开始启动,注意 30s内严 禁动IMU; 翻开GPS接收机电源,确认通过信号转发器可以接收到4颗以上卫将监控软件设置为“准备状态,准备时间5分钟准备过程中由实验教师介绍捷联惯导/GPS组合实验系统的组成、工作原理

5、;iPk 1 kk 1 Qk 1 估计均方误差：Pk (I Kk H k) Pk/k 12捷联惯导/GPS组合导航系统静态导航实验实验系统准备5分钟之后,通过监控软件,将实验系统设置为“组 合导 航状态； 记录IMU的原始输出,即角增量和比力信息； 记录数据过程中,由实验教师介绍采用卡尔曼滤波方法进行捷联惯导/GPS组合导航的根本原理； 记录IMU数据5分钟后,停止记录数据,利用监控计算机中的捷联惯导/GPS组合导航软件进行静态导航解算,并显示静态导航结果；四、实验结果 与分析K SINS/GPS组合导航后得纬度曲线和 GPS导航纬度曲线比照方下列图34. 9534.9GPS纬度组合导航后纬度

6、 34. 85 34.8 34. 75 34.7 34. 65 34.6 34. 55 34.5 34. 45 012345678 4 x 102、SINS/GPS组合导航后得经度曲线和 GPS导航经度曲线对 比方下列图110. 1110GPS经度组合导航后经度109.93、SINS/GPS组合导航后得周度曲线和 比方下列图35006PS高度组合导航后300025002000:*A 1500 高1000500I11X 10GPS导航图度曲线对4、SINS/GPS组合导航后得东向速度 曲线和向速度组合导航GPS辨函后定 aGPS导航东向速度曲线比照方 下列图)向3卜:二东/0m(度速,601 2

7、 3 4 5 6 7 8 4 x 1005、SINS/GPS组合导航后得北向速度 线和60GPS导航北向速度曲线比照女 口下列图40GPS北向速度组合导航后北向速度20-20-40-600x 106、SINS/GPS组合导航后得天向速度 曲线和GPS导航天向速度曲线比照方 下列图2 0 2 4.S/ 度速向天567x 107 SINS/GPS组合导航后航向角曲线、俯仰角曲线和横滚角 曲线如下列图 200150组合导航后航向角组 合导 航后俯仰角组合导航后横 滚角10050-50-100-150-200012x 108、纯惯性导航轨GPS导航轨迹和SINS/GPS组合导航轨迹比照迹、如下列图11

8、0.1110纯惯性导航轨迹GPS导航轨迹组合导航导航109.9轨迹109.8 109.7滚109.6经109.5109.4109.3109.2109.134. 45 34. 5 34. 55 34. 6 34. 65 34. 7 34. 75 34. 8 34. 85 34.9 34. 95 纬度如下列图东向失准角方差0 023北向失准.角方差'276度0 01847 o9、平台失准角的估计均方差曲线0 00. 02 r度 0.01天向失准像方差01234567§8 AX 10010 1 2 3 4 5 6 710x速度和位置的估计均方差曲线如下列图东向速度误差方差0. 01

9、/Sm 0.0050 0 2 4 6 8 4 x 10天向速度误差方差 0.01/Sm 0.005002468io经度误差方差0. 1m o. 050024684 X 10北向速度误差方差0. 01/Sm 0.0050 2 4 6 8度误差方差0. 1m 0.0500 2 4 6 8度误差方差0.2m o. 1o0 2 4 6 84x 10纬4 x 10"、陀螺漂移的估计均方差曲线和加速度计偏置的估计均方差曲线如下列图东向陀螺漂移方差北向陀螺漂移方差or0.050016 10天向陀螺漂移方差/呵 0? 1 小 0.0568.10广 0.1 小 0. 05 <L东向加计偏昼方差5

10、04 X 10 ?北向加计偏查方差5050载体GPS输出的位置和速度精度高,结果分析：从仿真结果可以看出,滤波之后载体的位置和速度比姿态在滤波校正后结果较好,INS/GPS组合导航有效地抑制了纯惯性导航的发散,也有效地去除 了 GPS量测输出的噪声.东北天方向的速度误差均能够估计出来,天向陀螺漂移估 计不出来,在动基 座情况下,东向和北向加计零偏、天向平台失准角以及东向和北向陀螺漂移均变得可观测, 收敛性变 好.可见,INS/GPS是一种较为理想的组合导航方式.五、源程序 难入数据卡尔曼 IMU. dat1);卡尔曼 GPS. dat');%eh/298.3; re 二 6378245

11、;wie=7. 292115147e-5; IMU_T=1/100;GPS T=1/20; gO 二 9. 7803267714;gk1=0.00193185138639;gk2=0. 00669437999013;LOOP二360000; %啾存储惯导解算的位置、速度、姿态和滤波后的位 置、 速度、姿态 velocity 二 zeros (LOOP, 3);pos iti on=zeros(LOOP, 3);attitude 二 zeros(LOOP, 3);veIoc i ty k 仁 zeros(72000, 3);pos i ti on_kf 二 zeros (72000,3);att

12、 itude k 仁 zeros (72000,3);%PS导航的初始位置和初始速度作为导航结算的初始位置和初始 速度vx0;vy 二 0. 0090 ;vz=0.lat=34. 6522*pi/180 ; long=109. 2496*pi/180 ;h 二 362. 2690; %四元数初值cita=0. 25097*pi/180 ; %W 彳卬角 gama=1.78357*pi/180 ; %1 滚角kesai 二 305. 34023*pi/180 ; %冗 向角 q 二cos (kesa i/2) *cos (c i ta/2) *cos (gama/2)- s i n(kesa i

13、/2)*s i n(c i ta/2)*s i n (gama/2);cos (kesa i/2)*s i n (c i ta/2)*cos (gama/2)-s i n (kesa i/2)*cos (c i ta/2)*s i n (gama/2);cos (kesa i/2)*cos (c i ta/2)*s i n(gama/2)+s i n(kesa i/2)*s i n(c i ta/2) *cos(gama/ 2);cos(kesa i/2) *s i n(c i ta/2) *s i n(gama/2)+s i n(kesa i/2)*cos (c i ta/2)*cos (g

14、ama/ 2);%前初始状态量和滤波初始所需矩阵kJ;X_f 二 zeros (15, 1);Q=diag( (0. 01*pi/(180*3600) 厂 2, (0. 01*pi/(180*3600)厂 2, (0. 01*pi/(180*3600) 2, (50e-6*g0 厂 2, (50e6g0 厂 2, (50e6g0 厂 2);R 二 diag(0? 0 2,0.0 2,0? 0 2,0. C2, 0.广 2, 0. 15八2);H 二 zeros (6, 15);p_kf=zeros(72000, 15);x kf 二 zeros (72000, 15);P 二 diag( (6

15、0*pi/180/3600 厂 2, (60*pi/180/3600 厂 2, (30*pi/180/60 厂 2,0.052,0.05 2, 0.05 人 2, (2/re 厂 2, (2/re 厂 2, 2 1 2, (0. 1*p i/180/3600) "2, (0.1*pi/180/3600 厂 2, (0.1*pi/180/3600 厂 2, (50e-6*g0 厂 2, (50e-6*g0 厂 2, (50e- 6*g0 厂 2);for i=1:LOOPRx=re/ (1-e*s i n(I at)"2)+h;Ry 二 re/ (1 +2*e-3*e*s i

16、 n (I at) "2) +h; g 二 gO* (1+gk1*s i n (I at) "2) * (1- 2*h/re)/sqrt (1-gk2*si n (I at)"2);%素姿态矢巨P车Cnb= q (1 厂 2+q (2 厂 2-q (3 厂 2-q (4 厂 2 2* (q (2) *q (3) +q (1) *q (4) 2* (q (2) *q (4)- q(1)*q (3) ; 2* (q (2) *q (3) -q (1) *q (4) q (l)/< 2-q (2 厂 2+q (3 厂 2-q (4) "2 2*(q(3)

17、*q(4)+q(1)*q(2) ; 2* (q (2) *q (4)+q (1) *q (3) 2* (q (3) *q (4)- q *q (2) q 八 2-q (2)八 2-q (3)八 2+q (4)八 2;%嘀算 f i bb 二 I MU (i, 6:8) ' *g;fibn=(Cnb l)*fibb; %军加速度、速度和位置ax=fibn + (2*w i e*s i n(I at)+vx*tan (I at)/Rx)*vy-(2*wi e*cos (I at)+vx/Rx) *vz;ay=fibn (2)- (2*w i e*s i n (I at)+vx*tan (I

18、 at)/Rx)*vx+vy*vz/Ry;az=fi bn (3) + (2*w i e*cos(I at)+vx/Rx)*vx+vy ' 2/Ry-g;vx 二 ax*lMU_T+vx;vy 二 ay*IMU_T+vy;vz 二 az*I MU T+vz:I at= I at+vy/Ry* I MU_T: I ong= I ong+vx*sec (I at) /Rx* I MU_T: h 二 h+vz* IMUT; %兀素姿态矩阵wi et 二0; wi e*cos (I at) ; wi e*s i n (I at);wett=-vy/Ry;vx/Rx;vx*tan (I at)/

19、Rx; wibb= (I MU (i,3:5)')*p i/180/3600;毗 bb 二 w i bb-Cnb* (wi et+wett); angIe 二 wtbb*IMU T:M _.0, -angle (1), -angle (2), -angle (3): angle(1), 0, angle(3), -angle(2);angle (2), -angle (3), 0, angled) ； angle (3), angle (2), -angle (1), 0；q 二 (cos (norm(angle) / 2) * eye(4) + sin (norm(angIe) /

20、2) / norm (angle)* M) * q; q =q / norm(q);Cnb=q 八2+q T-q -2-q (4) V 2* (q *q +q *q (4)2* (q *q (4) -q (1) *q (3);2* (q *q (3)-q (1) *q )q -q +q -2-q 2* (q (3) *q (4) +q (1) *q (2) ; 2*(q *q (4)+q (1)*q (3) 2* (q *q (4)-q (1) *q (2) q (1 /2-q .-q 八2+q (4)2;%根据更新过的四元素姿态矩阵求姿态角ci ta=as i n (Cnb (2, 3) ;

21、 % 俯仰角 kesa i_1 =atan (Cnb (2,1)/Cnb (2, 2) ; %航向 角 gama二atan (-Cnb (1,3)/Cnb(3, 3) ; % 横滚角 if Cnb(2, 2)=0if Cnb(2, 1)>0 kesai=-pi/2;e I sekesa i =p i/2;end eIse i f Cnb (2, 2)>0 kesai=kesai_1;e I sePUo e)quoOdde-Ue6 sPU"e-ue八He-ue6es-e +OAGL)PU八F_eEe八He-ue6EOEdduoOFdAeHCIDUOmsod+ -+ f3 ?

22、DU3抿& t s % u&& % =& NIPU&&&&&&&=JG%一&= M0105 a) L)01 GH2= zeros (3, 6), d i ag (Ry, Rx*cos (I at), 1 ), zeros (3, 6);H=H1;H2;G 二Cnb', zeros (3, 3) ; zeros (3, 3), Cnb' ; zeros (9, 3), zeros (9,3) ; %M取系统的一步转移阵Fn 二 zeros (9,9);Fn(1,2)=wie*s i n

23、 (I at)+vx*tan (I at)/Rx;Fn (1,3)二-wie*cos (Iat)-vx/Rx;Fn(1,5)二 T/Ry;Fn(1,9)二 vy/Ry/Ry;Fn (2,1)二-wi e*s i n (I at)-vx*tan(I at)/Rx;Fn (2,3)二-vy/Ry;Fn (2,4)h/Rx;Fn (2, 7) =-wi e*s i n (Iat);Fn (2,9)二-vx/Rx/Rx;Fn (3, 1)=wie*cos (I at) +vx/Rx;Fn (3, 2)=vy/Ry;Fn (3, 4) =tan (I at) /Rx;Fn(3, 7)=wi e*cos

24、(I at)+vx*sec (I at)"2/Rx;Fn (3,9)=-vx*tan (I at)/Rx/Rx;Fn (4, 2_f i bn (3);Fn (4, 3)二 f i bn ；Fn (4, 4)二(vy*tan (I at) -vz)/Rx;Fn (4, 5) =2八 i e*s i n (I at) +vx*tan (I at) /Rx;Fn (4, 6)=-2*w i e*cos(I at) -vx/Rx;Fn (4, 7)=2*wi e*cos (I at)*vy+2*wi e*s i n (I at)*vz+vx*vy*sec (I at)"2/Rx;

25、Fn (4, 9)二(vx*vzvx*vy*tan(I at)/Rx/Rx;Fn (5, 1)=f ibn (3);Fn (5, 3)二一 f ibn(1);Fn (5, 4)二-2*wi e*s i n(I at)-2*vx*tan(I at)/Rx;Fn (5,5)二-vz/Ry;Fn (5, 6)二-vy/Ry;Fn (5,7)二-vx* (2*w i e*cos(I at)+vx*sec(I at 厂 2/Rx);Fn (5,9)二(vy*vz+vx*vx*tan(I at)/Rx/Rx;Fn (6, 1) =-f i bn (2);Fn (6, 2)=f ibn(1);Fn (6,

26、4)二-2*w i e*cos(I at) -2*vx/Rx;Fn (6, 5)二 2*vy/Ry;Fn (6, 7)二-2*w i e*s i n (I at) *vx;Fn (6,9)二-(vx*vx+vy*vy)/Rx/Rx;Fn (7, 5)=1/Ry;Fn (7 二-vy/Ry/Ry;Fn(8, 4)二 sec (I at)/Rx;Fn (8,7)=vx*tan (I at)/Rx/cos(I at);Fn (8,9)二一 vx/Rx/Rx/cos(I at);Fn (9, 6)=1;Fs 二Cnb', zeros (3, 3) ; zeros (3, 3), Cnb'

27、; : zeros (3, 6);F 二Fn, Fs; zeros (6, 15);F 二 F*GPS_T;temp 仁 eye (15);fa i_x=eye (15);for j=:10temp1=F*temp1/j;fa i x 二 fa i x+tempi ;endGQG 二 G*Q*G 1;tempi 二 GQG*GPS_T;d i sB 二 temp1;for j 二 2:门temp2=F*temp1;tempi = (temp2+temp2 1) /j;d i sB 二 d i sB+templ;end%Z 二vx-GPS (k, 6) ; vy-GPS (k, 7) ; vz-

28、GPS (k, 8) : I at-GPS (k, 3) : I ong-GPS (k, 4) ; h- GPS(k,5) ; X_0 二 fa i _x*X_f;P_0 二 fai_x*P*fai_x'+disB;K 二 p_0*H 1 *(i nv(H*P_0*H'+R);X 仁 X 0+K* (Z-H*X 0);P 二(eye (15) -K*H) *P_0* (eye (15) -K*H)1 +K*R*K 1 ;%循环滤波得出的状态值x_kf (k, 1)=X_f (1)*180/pi ; x_kf (k, 2)二 X_f (2) *180/p i ;x_kf (k,

29、3)二 X_f (3)*180/p i :x_kf (k,4)=X_f (4);x_kf (k,5)二 X_f (5);x_kf (k, 6)二 X_f (6);x_kf (k, 7)二 X_f (7) *Ry;x_kf(k, 8)二 X_f (8)*Rx*cos (lat);x_kf (k,9)二 X_f (9);x_kf (k, 10)二 X_f (10) *180*3600/p i ;x_kf (k, 11)二 X_f (11)*180*3600/pi ;x_kf (k, 12)二 X_f (12) *180*3600/p i ;x_kf (k, 13)二 X_f (13) *1 (T

30、6/gO;x_kf (k, 14)二 X_f (14) *1 (T 6/gO;x_kf (k, 15)二 X_f (15) *1 (T6/gO;%嬲%勺估计均方差p_kf (k, 1) =sqrt (abs (P (1, 1) *180/p i ;p_kf (k, 2)=sqrt (abs (P (2, 2) *180/p i ;p_kf (k, 3) =sqrt (abs (P (3, 3) *180/p i ;p_kf (k, 4)=sqrt (abs (P (4, 4);p_kf (k, 5) =sqrt (abs (P (5, 5);p_kf (k, 6) =sqrt (abs (P

31、(6, 6);p kf (k, 7) =sqrt (abs (P (7, 7) *Ry:p_kf (k, 8)=sqrt (abs(P (8, 8)*Rx*cos(I at);p_kf (k, 9) =sqrt (abs (P (9, 9);p_kf (k, 10) =sqrt (abs (P (10,10) *180*3600/p i ;p_kf (k, 11) =sqrt (abs (P(11J1)*180*3600/pi ;p_kf (k, 12) =sqrt (abs (P(12, 12)*180*3600/pi ;p_kf (k, 13) =sqrt (abs (P(13, 13)*

32、1 (T6/gO; p kf (k, 14)=sqrt (abs (P(14, 14) *10"6/g0;p_kf (k, 15)=sqrt (abs (P (15, 15) *10'6/gO;%波值对速度和位置进行输出校正vx kf 二 vxX f(4);vy_k 仁 vy-X_f (5);vz_k 仁 vz-X_f(6);Iat_kf 二 Iat-X_f(7);I ong kf= I ong-X_f (8);h_k 仁 h-X_f (9);%后的四元素姿态阵Acita 二 X f (1)；Agama=X_f (2);AkesaiX_f (3);Ctn_k 仁cos(Aga

33、ma) *cos(Akesa i)-s i n (Agama)*s i n (Ac i ta)*s i n (Akesa i), cos(Agama)*s i n (Akesa i)+s i n (Agama)*s i n (Ac i ta)*cos (Akesa i),-s i n (Agama)*cos (Ac ita); -cos (Ac i ta)*s i n (Akesa i), cos (Ac i ta)*cos (Akesa i), s i n (Ac i ta); sin (Agama)*cos (Akesa i)+cos (Agama)*s i n(Ac i ta)*s i

34、n(Akesa i), s i n (Agama)*s i n (Akesa i)-cos (Agama)*s i n (Ac i ta)*cos (Akesa i), cos (Agama)*cos (Ac i ta);Ctb_kf=Cnb*Ctn_kf;Cnb_kf=Ctb_kf;%校正后的四元素姿态矩阵求取姿态角c i ta_kf=as i n (Cnb_kf (2, 3) : % 俯仰角kesa i _2=atan (Cnb_kf (2, 1) /Cnb_kf (2, 2) ;%冗向角 gama_2=atan (-Cnb kf (1,3)/Cnb kf (3, 3) : % 横滚角 i

35、f Cnb kf (2, 2) =0if Cnb kf(2, 1)>0kesai_kf=-pi/2;e I sekesa i kf=p i/2;endelseif Cnb_kf(2, 2)>0 kesai_kf=kesai_2;e I sei f Cnb_kf (2, 1)>0 kesai_kf 二 kesai_2+pi;e I sekesa i kf 二 kesa i 2-p i;endendif Cnb kf (3, 3)=0i f Cnb_kf (1,3)>0 gama_kf=pi/2;else gama_kf 二-pi/2;endelse i f Cnb_kf(

36、3, 3)>0 gama_kf 二 gama_2;e I se1i f Cnb_kf (1,3)>0 gama_kf=gama_2-p i;e I s?bama kf=gama 2+p i;endend%后的速度、位置和姿态值veIoc i ty_kf (k,:)二vx_kf, vy_kf, vz_kf:posit ion kf (k, :) = I at kf*180/p i, long kf*180/p i, h kf;att i tude_kf (k,:)二kesa i_kf*180/p i, c i ta_kf*180/p i, gama_kf*180/p 订;k 二 k+

37、1;X f (10:15)=0;end end t=1:72000; figure (1) pIot (1:72001,GPS (:,3):ho Id on p I ot (t, pos i t i on_kf (:, 1), 1 r') ; hold o n I egend ( 1 GPS 纬度', 组合导航后纬度 ,);yI abe I ('纬度(度)')；f i gure (2) p I ot (1 :72001, GPS (:, 4) ; ho I d onp Iot (t, pos it ion_kf (:, 2), 1 r') ; hold

38、on Iegend (* GPS 经度',' 组合导航后经度');y I abe I ('经度(度)')；f i gure (3) p I ot (1 :72001, GPS (:, 5) ; ho I d onp Iot (t, pos i t ion_kf (:, 3), 1 r') ;hold on Iegend (' GPS 图度','组合导航后图度')jylabel ('图度(m) ') ; figure (4) p I ot (1 :72001, GPS (:, 6) ; ho I d

39、onp Iot (t, velocity_kf (:, 1), 1 r') ; hold on Iegend (' GPS 东向速度,组合导航后东向速度')jylabel (* 东向速度(m/s) *) : figure (5)pIot(1 :72001, GPS(:, 7);ho Id on pIot(t, veloci ty_kf (:,2), 'r'):ho Id onI egend (' GPS北向速度','组合导航后北向速度');y I abe I C北向速度(m/s)') : f i gure (6)

40、pIot (1:72001,GPS (:,8);ho Id onplot (t, velocity_kf (:, 3), *r') ; hold on legend C GPS 天向速度','组合导航后天向速度')；ylabel ('天向速度(m/s) 1) figure (7)pIot (t,att i tude_kf (:, 1):ho Id on pIot (t, att i tude_kf (:, 2),1r');ho Id onp I ot (t, att i tude_kf (:, 3), * g') ; ho I d on legend C组合导航后航向角组合导航后俯仰角组合导航后横滚角)jylabel ('度')：figurepIot(pos it ion(:, 1), pos it i on(:, 2);ho Id on pIot(GPS (:, 3), GPS (:, 4),1r'):ho Id on plot (pos it i on_kf (:, 1), pos i t i on_kf (:, 2), &