1、1卡尔曼滤波与组合导航课程实验报告实验 捷联惯导 /GPS 组合导航系统静态导航实验 实验序号 3姓 名 陈星宇 系院专业 17 班 级 ZY11172 学 号 ZY1117212日期 2012-5-15 指导教师 宫晓琳 成 绩一、实验目的掌握捷联惯导/GPS 组合导航系统的构成和基本工作原理；掌握采用卡尔曼滤波方法进行捷联惯导/GPS 组合的基本原理；掌握捷联惯导/GPS 组合导航系统静态性能；了解捷联惯导/GPS 组合导航静态时的系统状态可观测性；二、实验原理（1）系统方程 XFGW TENUENU xyzxyzXvvLh 其中， 、 、 为数学平台失准角； 、 、 分别为载体的东向、北
2、向和天向速度误ENUv差； 、 、 分别为纬度误差、经度误差和高度误差； 、 、 、 、 、 分别为Lh xyzxyz陀螺随机常值漂移和加速度计随机常值零偏。（下标 E、N、U 分别代表东、北、天）系统的噪声转移矩阵 为：G391560nbC系统噪声矢量由陀螺仪和加速度计的随机误差组成，表达式为： Txyzxyzww 系统的状态转移矩阵 组成内容为：F，其中 中非零元素为可由惯导误差模型获得。 。690NSMFN nb396S0CF（2）量测方程量测变量 ， 、 、 、 、TENUzVLH EVNUL和 分别为捷联解算与 GPS 的东向速度、北向速度、天向速度、纬度、经度和高H度之差；量测矩阵
3、 ， ，TVPH36036diag,()cos,MR02， 为量测噪声。量测噪声方VH3039diag1,0ENUTLHVVvv差阵 根据 GPS 的位置、速度噪声水平选取。R（3）卡尔曼滤波方程状态一步预测： /1,1kkX状态估计： /1/1()kkkkKZHX滤波增益： /1TTkkkkPR一步预测均方误差： /1,1kkkkQ估计均方误差： /1()kkkIKH三、实验内容及步骤1、实验内容 捷联惯导 /GPS 组合导航系统静态导航实验；2、实验步骤1） 实验准备（由实验教师操作） 将 IMU 安装在大理石实验台上，确认 IMU 的安装基准面靠在大理石实验平台上的方位基准尺上； 将 I
4、MU 与导航计算机、导航计算机与稳压电源、导航计算机与监控计算机、GPS 接收机与导航计算机、 GPS 天线与 GPS 接收机、GPS 接收机与 GPS 电池之间的连接线正确连接； 打开 GPS 信号转发器； 打开监控计算机中的监控软件； 打开稳压电源开关，确认稳压电源输出为 28V； 打开捷联惯导/GPS 组合实验系统电源，实验系统开始启动，注意 30s 内严禁动IMU； 打开 GPS 接收机电源，确认通过信号转发器可以接收到 4 颗以上卫星； 将监控软件设置为“准备”状态，准备时间 5 分钟； 准备过程中由实验教师介绍捷联惯导/GPS 组合实验系统的组成、工作原理；32） 捷联惯导/GPS
5、 组合导航系统静态导航实验 实验系统准备 5 分钟之后，通过监控软件，将实验系统设置为“组合导航”状态； 记录 IMU 的原始输出，即角增量和比力信息； 记录数据过程中，由实验教师介绍采用卡尔曼滤波方法进行捷联惯导/GPS 组合导航的基本原理； 记录 IMU 数据 5 分钟后，停止记录数据，利用监控计算机中的捷联惯导/GPS组合导航软件进行静态导航解算，并显示静态导航结果；四、实验结果与分析1、SINS/GPS 组合导航后得纬度曲线和 GPS 导航纬度曲线对比如下图0 1 2 3 4 5 6 7 8x 10434.4534.534.5534.634.6534.734.7534.834.8534
6、.934.95东东东东东GPS东东东东东东东东东2、SINS/GPS 组合导航后得经度曲线和 GPS 导航经度曲线对比如下图190 1 2 3 4 5 6 7 8x 104109.1109.2109.3109.4109.5109.6109.7109.8109.9110110.1东东东东东GPS东东东东东东东东东3、SINS/GPS 组合导航后得高度曲线和 GPS 导航高度曲线对比如下图0 1 2 3 4 5 6 7 8x 1040500100015002000250030003500东东东m东GPS东东东东东东东东东4、SINS/GPS 组合导航后得东向速度曲线和 GPS 导航东向速度曲线对比
7、如下图0 1 2 3 4 5 6 7 8x 104-60-40-20020406080东东东东东m/s东GPS东东东东东东东东东东东东东5、SINS/GPS 组合导航后得北向速度曲线和 GPS 导航北向速度曲线对比如下图10 1 2 3 4 5 6 7 8x 104-60-40-200204060东东东东东m/s东GPS东东东东东东东东东东东东东6、SINS/GPS 组合导航后得天向速度曲线和 GPS 导航天向速度曲线对比如下图0 1 2 3 4 5 6 7 8x 104-4-202468东东东东东m/s东GPS东东东东东东东东东东东东东7、SINS/GPS 组合导航后航向角曲线、俯仰角曲线和
8、横滚角曲线如下图0 1 2 3 4 5 6 7 8x 104-200-150-100-50050100150200东东东东东东东东东东东东东东东东东东东东东东东东东8、纯惯性导航轨迹、GPS 导航轨迹和 SINS/GPS 组合导航轨迹对比如下图134.45 34.5 34.55 34.6 34.65 34.7 34.75 34.8 34.85 34.9 34.95109.1109.2109.3109.4109.5109.6109.7109.8109.9110110.1东东东东东东东东东东东GPS东东东东东东东东东东东东9、平台失准角的估计均方差曲线如下图0 1 2 3 4 5 6 7 8x 1
9、0400.010.02 东东东东东东东东0 1 2 3 4 5 6 7 8x 10400.010.02 东东东东东东东东0 1 2 3 4 5 6 7 8x 10400.5 东东东东东东东东10、速度和位置的估计均方差曲线如下图0 2 4 6 8x 10400.0050.01 东东东东东东东东m/s0 2 4 6 8x 10400.0050.01 东东东东东东东东m/s0 2 4 6 8x 10400.0050.01 东东东东东东东东m/s0 2 4 6 8x 10400.050.1 东东东东东东m0 2 4 6 8x 10400.050.1 东东东东东东m0 2 4 6 8x 10400.1
10、0.2 东东东东东东m11、陀螺漂移的估计均方差曲线和加速度计偏置的估计均方差曲线如下图10 2 4 6 8x 10400.050.1 东东东东东东东东东/东东0 2 4 6 8x 10400.050.1 东东东东东东东东东/东东0 2 4 6 8x 10400.050.1 东东东东东东东东东/东东0 2 4 6 8x 104050 东东东东东东东东g0 2 4 6 8x 104050 东东东东东东东东g0 2 4 6 8x 104050 东东东东东东东东g结果分析：从仿真结果可以看出，滤波之后载体的位置和速度比 GPS 输出的位置和速度精度高，载体姿态在滤波校正后结果较好，INS/GPS 组
11、合导航有效地抑制了纯惯性导航的发散，也有效地去除了 GPS 量测输出的噪声。东北天方向的速度误差均能够估计出来，天向陀螺漂移估计不出来，在动基座情况下，东向和北向加计零偏、天向平台失准角以及东向和北向陀螺漂移均变得可观测，收敛性变好。可见，INS/GPS 是一种较为理想的组合导航方式。五、源程序clear;clc;% 载入数据IMU=load(C:UsersAdministratorDesktop卡尔曼IMU.dat); GPS=load(C:UsersAdministratorDesktop卡尔曼GPS.dat);%定义常数 e=1/298.3; re=6378245; wie=7.2921
12、15147e-5; IMU_T=1/100;GPS_T=1/20;g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;LOOP=360000;%定义存储惯导解算的位置、速度、姿态和滤波后的位置、速度、姿态velocity=zeros(LOOP,3);1position=zeros(LOOP,3);attitude=zeros(LOOP,3);velocity_kf=zeros(72000,3);position_kf=zeros(72000,3);attitude_kf=zeros(72000,3);%用 GPS 导航的初始位置和
13、初始速度作为导航结算的初始位置和初始速度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)*sin(cita/2)*cos(gama/2)
14、-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,(0.01*pi/(180*3600)2,(50e-6*
15、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/3600)2,1(50e-6*g0)2,(50e-6
16、*g0)2,(50e-6*g0)2);for i=1:LOOPRx=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)2 2*(q(3)*q(4)+q(1)*q(2);2*(q(2
17、)*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*cos(lat)+vx/Rx)*vx+vy2/Ry-g;vx=ax*IM
18、U_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 = 0, -angle(1), -angle(2), -angle(3);angle(1), 0,
19、 angle(3), -angle(2);angle(2), -angle(3), 0, angle(1);1angle(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(4)-q(1)*q(3);2*(q(2)*q(3)-q(1)*q(4) q(1)2-q(2)2+q(3)2-
20、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)=0if Cnb(2,1)0kesai=-pi/2;elsekesai=pi/2;endelseif Cnb(2,2)0kesai=kesai_1;elseif Cnb(2,1)0kesai=kesai_1+pi;elsekesai=kesai_1-pi;endendif Cnb(3,3)=0if Cnb(1,3)0gama=pi/2;else gama=-pi/2;end
