《卡尔曼滤波与组合导航课程实验报告.doc》由会员分享，可在线阅读，更多相关《卡尔曼滤波与组合导航课程实验报告.doc（18页珍藏版）》请在三一办公上搜索。
1、卡尔曼滤波与组合导航课程实验报告实验捷联惯导/GPS组合导航系统静态导航实验实验序号3姓 名陈星宇系院专业17班 级ZY11172学 号ZY1117212日期2012-5-15指导教师宫晓琳成 绩一、实验目的掌握捷联惯导/GPS组合导航系统的构成和基本工作原理；掌握采用卡尔曼滤波方法进行捷联惯导/GPS组合的基本原理；掌握捷联惯导/GPS组合导航系统静态性能；了解捷联惯导/GPS组合导航静态时的系统状态可观测性；二、实验原理（1）系统方程 其中，、为数学平台失准角；、分别为载体的东向、北向和天向速度误差；、分别为纬度误差、经度误差和高度误差；、分别为陀螺随机常值漂移和加速度计随机常值零偏。（下
2、标E、N、U分别代表东、北、天）系统的噪声转移矩阵为：系统噪声矢量由陀螺仪和加速度计的随机误差组成，表达式为：系统的状态转移矩阵组成内容为：，其中中非零元素为可由惯导误差模型获得。 （2）量测方程 量测变量，、和分别为捷联解算与GPS的东向速度、北向速度、天向速度、纬度、经度和高度之差；量测矩阵， ，为量测噪声。量测噪声方差阵根据GPS的位置、速度噪声水平选取。（3）卡尔曼滤波方程状态一步预测：状态估计：滤波增益：一步预测均方误差：估计均方误差：三、实验内容及步骤 1、实验内容 捷联惯导/GPS组合导航系统静态导航实验； 2、实验步骤1） 实验准备（由实验教师操作） 将IMU安装在大理石实验台
3、上，确认IMU的安装基准面靠在大理石实验平台上的方位基准尺上； 将IMU与导航计算机、导航计算机与稳压电源、导航计算机与监控计算机、GPS接收机与导航计算机、GPS天线与GPS接收机、GPS接收机与GPS电池之间的连接线正确连接； 打开GPS信号转发器； 打开监控计算机中的监控软件； 打开稳压电源开关，确认稳压电源输出为28V； 打开捷联惯导/GPS组合实验系统电源，实验系统开始启动，注意30s内严禁动IMU； 打开GPS接收机电源，确认通过信号转发器可以接收到4颗以上卫星； 将监控软件设置为“准备”状态，准备时间5分钟； 准备过程中由实验教师介绍捷联惯导/GPS组合实验系统的组成、工作原理；
4、2） 捷联惯导/GPS组合导航系统静态导航实验 实验系统准备5分钟之后，通过监控软件，将实验系统设置为“组合导航”状态； 记录IMU的原始输出，即角增量和比力信息； 记录数据过程中，由实验教师介绍采用卡尔曼滤波方法进行捷联惯导/GPS组合导航的基本原理； 记录IMU数据5分钟后，停止记录数据，利用监控计算机中的捷联惯导/GPS组合导航软件进行静态导航解算，并显示静态导航结果；四、实验结果与分析 1、SINS/GPS组合导航后得纬度曲线和GPS导航纬度曲线对比如下图2、SINS/GPS组合导航后得经度曲线和GPS导航经度曲线对比如下图3、SINS/GPS组合导航后得高度曲线和GPS导航高度曲线对
5、比如下图4、SINS/GPS组合导航后得东向速度曲线和GPS导航东向速度曲线对比如下图5、SINS/GPS组合导航后得北向速度曲线和GPS导航北向速度曲线对比如下图6、SINS/GPS组合导航后得天向速度曲线和GPS导航天向速度曲线对比如下图7、SINS/GPS组合导航后航向角曲线、俯仰角曲线和横滚角曲线如下图8、纯惯性导航轨迹、GPS导航轨迹和SINS/GPS组合导航轨迹对比如下图9、平台失准角的估计均方差曲线如下图10、速度和位置的估计均方差曲线如下图11、陀螺漂移的估计均方差曲线和加速度计偏置的估计均方差曲线如下图结果分析：从仿真结果可以看出，滤波之后载体的位置和速度比GPS输出的位置和
6、速度精度高，载体姿态在滤波校正后结果较好，INS/GPS组合导航有效地抑制了纯惯性导航的发散，也有效地去除了GPS量测输出的噪声。东北天方向的速度误差均能够估计出来，天向陀螺漂移估计不出来，在动基座情况下，东向和北向加计零偏、天向平台失准角以及东向和北向陀螺漂移均变得可观测，收敛性变好。可见，INS/GPS是一种较为理想的组合导航方式。五、源程序clear;clc;% 载入数据IMU=load(C:UsersAdministratorDesktop卡尔曼IMU.dat); GPS=load(C:UsersAdministratorDesktop卡尔曼GPS.dat); %定义常数 e=1/29
7、8.3; re=6378245; wie=7.292115147e-5; IMU_T=1/100;GPS_T=1/20;g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;LOOP=360000;%定义存储惯导解算的位置、速度、姿态和滤波后的位置、速度、姿态velocity=zeros(LOOP,3);position=zeros(LOOP,3);attitude=zeros(LOOP,3);velocity_kf=zeros(72000,3);position_kf=zeros(72000,3);attitude_kf=zero
8、s(72000,3);%用GPS导航的初始位置和初始速度作为导航结算的初始位置和初始速度vx=0;vy=0.0090 ;vz=0.0350;lat=34.6522*pi/180 ;long=109.2496*pi/180 ;h=362.2690;%定义四元数初值cita=0.25097*pi/180 ; %俯仰角gama=1.78357*pi/180 ; %横滚角kesai=305.34023*pi/180 ; %航向角q=cos(kesai/2)*cos(cita/2)*cos(gama/2)-sin(kesai/2)*sin(cita/2)*sin(gama/2); cos(kesai/2
9、)*sin(cita/2)*cos(gama/2)-sin(kesai/2)*cos(cita/2)*sin(gama/2); cos(kesai/2)*cos(cita/2)*sin(gama/2)+sin(kesai/2)*sin(cita/2)*cos(gama/2); cos(kesai/2)*sin(cita/2)*sin(gama/2)+sin(kesai/2)*cos(cita/2)*cos(gama/2);%滤波初始状态量和滤波初始所需矩阵k=1; X_f=zeros(15,1);Q=diag(0.01*pi/(180*3600)2,(0.01*pi/(180*3600)2,(
10、0.01*pi/(180*3600)2,(50e-6*g0)2,(50e-6*g0)2,(50e-6*g0)2);R=diag(0.012,0.012,0.012,0.12,0.12,0.152);H=zeros(6,15);p_kf=zeros(72000,15);x_kf=zeros(72000,15);P=diag(60*pi/180/3600)2,(60*pi/180/3600)2,(30*pi/180/60)2,0.052,0.052,0.052,(2/re)2,(2/re)2,22,(0.1*pi/180/3600)2, (0.1*pi/180/3600)2,(0.1*pi/180
11、/3600)2,(50e-6*g0)2,(50e-6*g0)2,(50e-6*g0)2);for i=1:LOOP Rx=re/(1-e*sin(lat)2)+h; Ry=re/(1+2*e-3*e*sin(lat)2)+h; g=g0*(1+gk1*sin(lat)2)*(1-2*h/re)/sqrt(1-gk2*sin(lat)2); %四元素姿态矩阵 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(1)2-q(2)2+q(3)2-q(4)
12、2 2*(q(3)*q(4)+q(1)*q(2); 2*(q(2)*q(4)+q(1)*q(3) 2*(q(3)*q(4)-q(1)*q(2) q(1)2-q(2)2-q(3)2+q(4)2; %捷联惯导结算 fibb=IMU(i,6:8)*g; fibn=(Cnb)*fibb; %求解加速度、速度和位置 ax=fibn(1)+(2*wie*sin(lat)+vx*tan(lat)/Rx)*vy-(2*wie*cos(lat)+vx/Rx)*vz; ay=fibn(2)-(2*wie*sin(lat)+vx*tan(lat)/Rx)*vx+vy*vz/Ry; az=fibn(3)+(2*wie
13、*cos(lat)+vx/Rx)*vx+vy2/Ry-g; vx=ax*IMU_T+vx; vy=ay*IMU_T+vy; vz=az*IMU_T+vz; lat=lat+vy/Ry*IMU_T; long=long+vx*sec(lat)/Rx*IMU_T; h=h+vz*IMU_T; %更新四元素姿态矩阵 wiet=0;wie*cos(lat);wie*sin(lat); wett=-vy/Ry;vx/Rx;vx*tan(lat)/Rx; wibb=(IMU(i,3:5)*pi/180/3600; wtbb=wibb-Cnb*(wiet+wett); angle=wtbb*IMU_T; M
14、 = 0, -angle(1), -angle(2), -angle(3); angle(1), 0, angle(3), -angle(2); angle(2), -angle(3), 0, angle(1); angle(3), angle(2), -angle(1), 0; q = (cos(norm(angle) / 2) * eye(4) + sin(norm(angle) / 2) / norm(angle) * M) * q; q =q / norm(q); 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(
15、4)-q(1)*q(3); 2*(q(2)*q(3)-q(1)*q(4) q(1)2-q(2)2+q(3)2-q(4)2 2*(q(3)*q(4)+q(1)*q(2); 2*(q(2)*q(4)+q(1)*q(3) 2*(q(3)*q(4)-q(1)*q(2) q(1)2-q(2)2-q(3)2+q(4)2; %根据更新过的四元素姿态矩阵求姿态角 cita=asin(Cnb(2,3); % 俯仰角 kesai_1=atan(Cnb(2,1)/Cnb(2,2); %航向角 gama_1=atan(-Cnb(1,3)/Cnb(3,3); % 横滚角 if Cnb(2,2)=0 if Cnb(2,1
16、)0 kesai=-pi/2; else kesai=pi/2; end elseif Cnb(2,2)0 kesai=kesai_1; else if Cnb(2,1)0 kesai=kesai_1+pi; else kesai=kesai_1-pi; end end if Cnb(3,3)=0 if Cnb(1,3)0 gama=pi/2; else gama=-pi/2; end elseif Cnb(3,3)0 gama=gama_1; else if Cnb(1,3)0 gama=gama_1-pi; else gama=gama_1+pi; end end %存储惯导解算求的的速度
17、、位置和姿态角 velocity(i,:) = vx,vy,vz; position(i,:) = lat/pi*180,long/pi*180,h; attitude(i,:) = kesai/pi*180,cita/pi*180,gama/pi*180; %开始滤波 if(mod(i,5)=0) Rx=re/(1-e*sin(lat)2)+h; Ry=re/(1+2*e-3*e*sin(lat)2)+h; %定义量测矩阵和系统噪声驱动阵 H1=zeros(3,3),eye(3,3),zeros(3,9); H2=zeros(3,6),diag(Ry,Rx*cos(lat),1),zeros
18、(3,6); H=H1;H2; G=Cnb,zeros(3,3);zeros(3,3),Cnb;zeros(9,3),zeros(9,3); %根据离散化求取系统的一步转移阵fai_x Fn=zeros(9,9); Fn(1,2)=wie*sin(lat)+vx*tan(lat)/Rx; Fn(1,3)=-wie*cos(lat)-vx/Rx; Fn(1,5)=-1/Ry; Fn(1,9)=vy/Ry/Ry; Fn(2,1)=-wie*sin(lat)-vx*tan(lat)/Rx; Fn(2,3)=-vy/Ry; Fn(2,4)=1/Rx; Fn(2,7)=-wie*sin(lat); Fn
19、(2,9)=-vx/Rx/Rx; Fn(3,1)=wie*cos(lat)+vx/Rx; Fn(3,2)=vy/Ry; Fn(3,4)=tan(lat)/Rx; Fn(3,7)=wie*cos(lat)+vx*sec(lat)2/Rx; Fn(3,9)=-vx*tan(lat)/Rx/Rx; Fn(4,2)=-fibn(3); Fn(4,3)=fibn(2); Fn(4,4)=(vy*tan(lat)-vz)/Rx; Fn(4,5)=2*wie*sin(lat)+vx*tan(lat)/Rx; Fn(4,6)=-2*wie*cos(lat)-vx/Rx; Fn(4,7)=2*wie*cos(l
20、at)*vy+2*wie*sin(lat)*vz+vx*vy*sec(lat)2/Rx; Fn(4,9)=(vx*vz-vx*vy*tan(lat)/Rx/Rx; Fn(5,1)=fibn(3); Fn(5,3)=-fibn(1); Fn(5,4)=-2*wie*sin(lat)-2*vx*tan(lat)/Rx; Fn(5,5)=-vz/Ry; Fn(5,6)=-vy/Ry; Fn(5,7)=-vx*(2*wie*cos(lat)+vx*sec(lat)2/Rx); Fn(5,9)=(vy*vz+vx*vx*tan(lat)/Rx/Rx; Fn(6,1)=-fibn(2); Fn(6,2)=
21、fibn(1); Fn(6,4)=-2*wie*cos(lat)-2*vx/Rx; Fn(6,5)=2*vy/Ry; Fn(6,7)=-2*wie*sin(lat)*vx; Fn(6,9)=-(vx*vx+vy*vy)/Rx/Rx; Fn(7,5)=1/Ry; Fn(7,9)=-vy/Ry/Ry; Fn(8,4)=sec(lat)/Rx; Fn(8,7)=vx*tan(lat)/Rx/cos(lat); Fn(8,9)=-vx/Rx/Rx/cos(lat); Fn(9,6)=1; Fs=Cnb,zeros(3,3);zeros(3,3),Cnb;zeros(3,6); F=Fn,Fs;zero
22、s(6,15); F=F*GPS_T; temp1=eye(15); fai_x=eye(15); for j=1:10 temp1=F*temp1/j; fai_x=fai_x+temp1; end GQG=G*Q*G; temp1=GQG*GPS_T; disB=temp1; for j = 2:11 temp2=F*temp1; temp1=(temp2+temp2)/j; disB=disB+temp1; end %滤波方程 Z=vx-GPS(k,6);vy-GPS(k,7);vz-GPS(k,8);lat-GPS(k,3);long-GPS(k,4);h-GPS(k,5); X_0=
23、fai_x*X_f; P_0=fai_x*P*fai_x+disB; K=P_0*H*(inv(H*P_0*H+R); X_f=X_0+K*(Z-H*X_0); P=(eye(15)-K*H)*P_0*(eye(15)-K*H)+K*R*K; %存储每次循环滤波得出的状态值 x_kf(k,1)=X_f(1)*180/pi; x_kf(k,2)=X_f(2)*180/pi; x_kf(k,3)=X_f(3)*180/pi; 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_
24、f(8)*Rx*cos(lat); x_kf(k,9)=X_f(9); x_kf(k,10)=X_f(10)*180*3600/pi; x_kf(k,11)=X_f(11)*180*3600/pi; x_kf(k,12)=X_f(12)*180*3600/pi; x_kf(k,13)=X_f(13)*106/g0; x_kf(k,14)=X_f(14)*106/g0; x_kf(k,15)=X_f(15)*106/g0; %计算状态的估计均方差 p_kf(k,1)=sqrt(abs(P(1,1)*180/pi; p_kf(k,2)=sqrt(abs(P(2,2)*180/pi; p_kf(k,
25、3)=sqrt(abs(P(3,3)*180/pi; 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(6,6); p_kf(k,7)=sqrt(abs(P(7,7)*Ry; p_kf(k,8)=sqrt(abs(P(8,8)*Rx*cos(lat); p_kf(k,9)=sqrt(abs(P(9,9); p_kf(k,10)=sqrt(abs(P(10,10)*180*3600/pi; p_kf(k,11)=sqrt(abs(P(11,11)*180*3600/pi; p_kf(k,12)=s
26、qrt(abs(P(12,12)*180*3600/pi; p_kf(k,13)=sqrt(abs(P(13,13)*106/g0; p_kf(k,14)=sqrt(abs(P(14,14)*106/g0; p_kf(k,15)=sqrt(abs(P(15,15)*106/g0; %由滤波值对速度和位置进行输出校正 vx_kf=vx-X_f(4); vy_kf=vy-X_f(5); vz_kf=vz-X_f(6); lat_kf=lat-X_f(7); long_kf=long-X_f(8); h_kf=h-X_f(9); %求取校正后的四元素姿态阵 Acita=X_f(1); Agama=X
27、_f(2); Akesai=X_f(3); Ctn_kf=cos(Agama)*cos(Akesai)-sin(Agama)*sin(Acita)*sin(Akesai),cos(Agama)*sin(Akesai)+sin(Agama)*sin(Acita)*cos(Akesai), -sin(Agama)*cos(Acita); -cos(Acita)*sin(Akesai), cos(Acita)*cos(Akesai), sin(Acita); sin(Agama)*cos(Akesai)+cos(Agama)*sin(Acita)*sin(Akesai), sin(Agama)*si
28、n(Akesai)-cos(Agama)*sin(Acita)*cos(Akesai), cos(Agama)*cos(Acita); Ctb_kf=Cnb*Ctn_kf; Cnb_kf=Ctb_kf; %根据校正后的四元素姿态矩阵求取姿态角 cita_kf=asin(Cnb_kf(2,3); % 俯仰角 kesai_2=atan(Cnb_kf(2,1)/Cnb_kf(2,2); %航向角 gama_2=atan(-Cnb_kf(1,3)/Cnb_kf(3,3); % 横滚角 if Cnb_kf(2,2)=0 if Cnb_kf(2,1)0 kesai_kf=-pi/2; else kesai
29、_kf=pi/2; end elseif Cnb_kf(2,2)0 kesai_kf=kesai_2; else if Cnb_kf(2,1)0 kesai_kf=kesai_2+pi; else kesai_kf=kesai_2-pi; end end if Cnb_kf(3,3)=0 if Cnb_kf(1,3)0 gama_kf=pi/2; else gama_kf=-pi/2; end elseif Cnb_kf(3,3)0 gama_kf=gama_2; else if Cnb_kf(1,3)0 gama_kf=gama_2-pi; else gama_kf=gama_2+pi; e
30、nd end %存储校正后的速度、位置和姿态值 velocity_kf(k,:)=vx_kf,vy_kf,vz_kf; position_kf(k,:)=lat_kf*180/pi,long_kf*180/pi,h_kf; attitude_kf(k,:)=kesai_kf*180/pi,cita_kf*180/pi,gama_kf*180/pi; k=k+1; X_f(10:15)=0; endendt=1:72000;figure(1)plot(1:72001,GPS(:,3);hold on plot(t,position_kf(:,1),r);hold on legend(GPS纬度,
31、组合导航后纬度);ylabel(纬度（度）);figure(2)plot(1:72001,GPS(:,4);hold on plot(t,position_kf(:,2),r);hold on legend(GPS经度,组合导航后经度);ylabel(经度（度）);figure(3)plot(1:72001,GPS(:,5);hold on plot(t,position_kf(:,3),r);hold on legend(GPS高度,组合导航后高度);ylabel(高度（m）);figure(4)plot(1:72001,GPS(:,6);hold on plot(t,velocity_kf
32、(:,1),r);hold on legend(GPS东向速度,组合导航后东向速度);ylabel(东向速度（m/s）);figure(5)plot(1:72001,GPS(:,7);hold on plot(t,velocity_kf(:,2),r);hold on legend(GPS北向速度,组合导航后北向速度);ylabel(北向速度（m/s）);figure(6)plot(1:72001,GPS(:,8);hold on plot(t,velocity_kf(:,3),r);hold on legend(GPS天向速度,组合导航后天向速度);ylabel(天向速度（m/s）)figu
33、re(7)plot(t,attitude_kf(:,1);hold on plot(t,attitude_kf(:,2),r);hold on plot(t,attitude_kf(:,3),g);hold on legend(组合导航后航向角,组合导航后俯仰角,组合导航后横滚角);ylabel(度);figure(8)plot(position(:,1),position(:,2);hold on plot(GPS(:,3),GPS(:,4),r);hold on plot(position_kf(:,1),position_kf(:,2),g);hold on legend(纯惯性导航轨迹
34、,GPS导航轨迹,组合导航导航轨迹);xlabel(纬度);ylabel(经度);figure(9)subplot(3,1,1);plot(t,p_kf(:,1);title(东向失准角方差);ylabel(度);subplot(3,1,2);plot(t,p_kf(:,2);title(北向失准角方差);ylabel(度);subplot(3,1,3);plot(t,p_kf(:,3);title(天向失准角方差);ylabel(度);figure(10)subplot(3,2,1);plot(t,p_kf(:,4);title(东向速度误差方差);ylabel(m/s);subplot(3
35、,2,2);plot(t,p_kf(:,5);title(北向速度误差方差);ylabel(m/s);subplot(3,2,3);plot(t,p_kf(:,6);title(天向速度误差方差);ylabel(m/s);subplot(3,2,4);plot(t,p_kf(:,7);title(纬度误差方差);ylabel(m);subplot(3,2,5);plot(t,p_kf(:,8);title(经度误差方差);ylabel(m);subplot(3,2,6);plot(t,p_kf(:,9);title(高度误差方差);ylabel(m);figure(11)subplot(3,2
36、,1);plot(t,p_kf(:,10);title(东向陀螺漂移方差);ylabel(度/小时);subplot(3,2,2);plot(t,p_kf(:,11);title(北向陀螺漂移方差);ylabel(度/小时);subplot(3,2,3);plot(t,p_kf(:,12);title(天向陀螺漂移方差);ylabel(度/小时);subplot(3,2,4);plot(t,p_kf(:,13);title(东向加计偏置方差);ylabel(g);subplot(3,2,5);plot(t,p_kf(:,14);title(北向加计偏置方差);ylabel(g);subplot(3,2,6);plot(t,p_kf(:,15);title(北向加计偏置方差);ylabel(g);
