惯性导航技术综合实验实验五惯性基组合导航及应用技术实验惯性/卫星组合导航系统车载实验一、实验目的①掌握捷联惯导/GPS组合导航系统的构成和基本工作原理；②掌握采用卡尔曼滤波方法进行捷联惯导/GPS组合的基本原理；③掌握捷联惯导 /GPS组合导航系统静态性能；④掌握动态情况下捷联惯导 /GPS组合导航系统的性能。

二、实验内容①复习卡尔曼滤波的基本原理（参考《卡尔曼滤波与组合导航原理》第二、五章）；②复习捷联惯导/GPS组合导航系统的基本工作原理（参考以光衢编著的《惯性导航原理》第七章）；三、实验系统组成①捷联惯导/GPS组合导航实验系统一套；②监控计算机一台。

③差分GPS接收机一套；④实验车一辆；⑤车载大理石平台；⑥车载电源系统。

四、实验内容1）实验准备①将IMU紧固在车载大理石减振平台上，确认IMU的安装基准面紧靠实验平台；②将IMU与导航计算机、导航计算机与车载电源、导航计算机与监控计算机、GPS 接收机与导航计算机、GPS 天线与GPS 接收机、GPS 接收机与GPS 电池之间的连接线正确连接；③ 打开GPS 接收机电源，确认可以接收到4颗以上卫星； ④ 打开电源，启动实验系统。

2） 捷联惯导/GPS 组合导航实验① 进入捷联惯导初始对准状态，记录IMU 的原始输出，注意5分钟内严禁移动实验车和IMU ；② 实验系统经过5分钟初始对准之后，进入导航状态； ③ 移动实验车，按设计实验路线行驶；④ 利用监控计算机中的导航软件进行导航解算，并显示导航结果。

五、 实验结果及分析(一)理论推导捷联惯导短时段（1分钟）位置误差，并用1分钟惯导实验数据验证。

1、一分钟惯导位置误差理论推导：短时段内（t<5min ），忽略地球自转0ie ω=，运动轨迹近似为平面1/0R =，此时的位置误差分析可简化为：（1） 加速度计零偏∇引起的位置误差：210.88022t x δ∇==m （2） 失准角0φ引起的误差：202 0.92182g t x φδ==m （3） 陀螺漂移ε引起的误差：330.01376g t x εδ==m 可得1min 后的位置误差值123 1.8157m x x x x δδδδ=++= 2、一分钟惯导实验数据验证结果：（1）纯惯导解算1min 的位置及位置误差图：lat0.01s 度lon0.01s度北向位移误差0.01sm 东向位移误差0.01sm（2）纯惯导解算1min 的速度及速度误差图：-100-50050Vx0.01s m /s020406080Vy0.01sm /s100020003000400050006000-0.4-0.3-0.2-0.10Vx 误差0.01s m /s100020003000400050006000-0.1-0.0500.050.1Vy 误差0.01sm /s实验结果分析：纯惯导解算短时间内精度很高，1min 的惯导解算的北向最大位移误差，东向最大位移误差，可见实验数据所得位置误差与理论推导的位置误差在同一数量级，结果不完全相同是因为理论推导时做了大量简化，而且实验时视GPS 为真实值也会带来误差；另外，可见1min 内纯惯导解算的东向速度最大误差s ，北向速度最大误差s 。

(二)选取IMU 前5分钟数据进行对准实验。

将初始对准结果作为初值完成1小时捷联惯导和组合导航解算，对比1小时捷联惯导和组合导航结果。

1、5minIMU 数据的解析粗对准结果：-1.1960 -0.9998 -0.0084 0.9979 -1.1990 -0.0163 0.0062 0.1198 0.9991Cnb ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦attitude =[219.7700 -0.9318 0.4828]2、5minIMU 数据的Kalman 滤波精对准结果：00.511.522.53x 104航向角0.01秒度0.511.522.53x 104俯仰角0.01秒度x 104横滚角0.01秒度fai = 219.9744642380873*pi/180;theta = -0.895865732956914*pi/180;gama = 0.640089448357591*pi/180;x 104-4北向速度误差0.01秒米/秒x 104-3北向速度误差方差0.01秒米/秒x 104-4东向速度误差0.01秒米/秒x 104-3东向速度误差方差0.01秒米/秒x 104-3北向失准角0.01秒度x 104-3北向失准角方差0.01秒度x 104-3东向失准角0.01秒度x 104-3东向失准角方差0.01秒度x 104-5天向失准角0.01秒度x 104-3天向失准角方差0.01秒度3、一小时IMU/GPS 数据的组合导航结果图及估计方差P 阵图：0.511.52 2.53 3.54x 105纬度0.01s °x 105经度0.01s °x 105高度0.01smx 105东向速度0.01s m /sx 105北向速度0.01s m /sx 105天向速度0.01sm /sx 105航向角0.01s °x 105俯仰角0.01s °x 105横滚角0.01s°x 105-4P 航向角误差0.01s度x 105-4P 俯仰角误差0.01s度x 105-3P 横滚角误差0.01s度x 105-3P 东向速度误差0.05sm /sx 105-3P 北向速度误差0.05sm /sx 105-3P 天向速度误差0.01sm /s00.51 1.52 2.53 3.54x 105-8P 纬度误差0.01s 度x 105-8P 经度误差0.01s 度x 105P 高度误差0.01sm4、一小时IMU 数据的捷联惯导解算结果与组合滤波、GPS 输出对比图：0.511.52 2.53 3.54x 105lat0.01s 度x 105lon0.01s 度54height0.01sm5、结果分析：由滤波结果图可以看出：（1） 由组合后的速度、位置的P 阵可以看出滤波之后载体的速度和位置比GPS 输出的精度高。

（2） 短时间内SINS 的精度较高，初始阶段的导航结果基本和GPS 、组合导航结果重合，1小时后的捷联惯导解算结果很差，纬度、经度、高度均发散。

（3） INS/GPS 组合滤波的结果和GPS 的输出结果十分近似，因为1小时的导航GPS 的精度比SINS 导航的精度高很多，Kalman 滤波器中GPS 信号的权重更大。

（4） 总体看来，SINS/GPS 组合滤波的结果优于单独用SINS 或GPS 导航的结果，起到了协调、超越、冗余的作用，使导航系统更可靠。

六、 SINS/GPS 组合导航程序%%%%%%%%%%%%%%%%%%%%%%%INS/GPS 组合导航跑车1h 实验%%%%%%%%%%%%%%%%%%%%%%%%%% %该程序为15维状态量，6维观测量的kalman 滤波程序，惯性/卫星组合松耦合的数学模型 clear clc close all%%初始量定义wie = ;Re = ;g = ;e = / ;T = ; %IMU频率100hz,此程序中GPS频率100hz datanumber = 360000; %数据时间3600sa = load('');w = a(:,3:5)'*pi/180/3600; %陀螺仪输出的角速率信息单位由°/h化为rad/s f = a(:,6:8)'; %三轴比力输出，单位ga = load('');gps_pos = a(:,3:5); %GPS输出的纬度、经度、高度信息gps_pos(:,1:2) = gps_pos(:,1:2)*pi/180; %纬经单位化为弧度gps_v = a(:,6:8); %GPS输出的东北天速度信息%捷联解算及卡尔曼相关v=zeros(datanumber,3); %组合后的速度信息atti = zeros(datanumber,3); %组合后的姿态信息pos = zeros(datanumber,3); %组合后的位置信息gyro=zeros(3,1);acc=zeros(3,1);x_kf = zeros(datanumber,15);p_kf = zeros(datanumber,15);lat = *pi/180; %组合导航的初始位置、姿态、速度lon =*pi/180;height =;fai = *pi/180;theta = *pi/180;gama = *pi/180;Vx=gps_v(1,1);Vy=gps_v(1,2);Vz=gps_v(1,3);X_o=zeros(15,1); %X的初值选为0X=zeros(15,1);%Q=diag([(50e-6*g)^2,(50e-6*g)^2,(50e-6*g)^2,*pi/180/3600)^2,*pi/180/3600)^2,*p i/180/3600)^2,0,0,0,0,0,0,0,0,0]); %随机Q=diag([*pi/180/3600)^2,*pi/180/3600)^2,*pi/180/3600)^2,(50e-6*g)^2,(50e-6*g)^2 ,(50e-6*g)^2,0,0,0,0,0,0,0,0,0]);R=diag([^2,^2,^2,^2,^2,^2]);P=zeros(15);P_k=diag([*pi/180)^2,*pi/180)^2,*pi/180)^2,^2,^2,^2,2^2,2^2,2^2,*pi/180/3600)^2 ,*pi/180/3600)^2,*pi/180/3600)^2,(50e-6*g)^2,(50e-6*g)^2,(50e-6*g)^2]); %K=zeros(15,6);Z=zeros(6,1);I=eye(15);Cnb = [ cos(gama)*cos(fai)-sin(gama)*sin(theta)*sin(fai), cos(gama)*sin(fai)+sin(gama)*sin(theta)*cos(fai), -sin(gama)*cos(theta);-cos(theta)*sin(fai), cos(theta)*cos(fai), sin(theta);sin(gama)*cos(fai)+cos(gama)*sin(theta)*sin(fai),sin(gama)*sin(fai)-cos(gama)*sin(theta)*cos(fai), cos(gama) * cos(theta)];q = [ cos(fai/2)*cos(theta/2)*cos(gama/2) - sin(fai/2)*sin(theta/2)*sin(gama/2); cos(fai/2)*sin(theta/2)*cos(gama/2) - sin(fai/2)*cos(theta/2)*sin(gama/2); cos(fai/2)*cos(theta/2)*sin(gama/2) + sin(fai/2)*sin(theta/2)*cos(gama/2); cos(fai/2)*sin(theta/2)*sin(gama/2) + sin(fai/2)*cos(theta/2)*cos(gama/2)];Cnb_s=Cnb;q_s=q;for i=1:1:datanumberRmh = Re * - * e + * e * sin(lat) * sin(lat)) + height;Rnh = Re * + e * sin(lat) * sin(lat)) + height;Wien = [ 0; wie * cos(lat); wie * sin(lat)];Wenn = [ -Vy / Rmh; Vx / Rnh; Vx * tan(lat) / Rnh];Winn = Wien + Wenn;Winb = Cnb * Winn;for j=1:3gyro(j,1) = w(j,i);acc(j,1) = f(j,i)*g; %加速度信息，单位化为m/s^2endangle = (gyro - Winb) * T;fn = Cnb'* acc;difVx = fn(1) + * wie * sin(lat) + Vx * tan(lat) / Rnh) * Vy;difVy = fn(2) - * wie * sin(lat) + Vx * tan(lat) / Rnh) * Vx;difVz = fn(3) + * wie * cos(lat) + Vx / Rnh) * Vx + Vy * Vy / Rmh -g;Vx = difVx * T + Vx;Vy = difVy * T + Vy;Vz = difVz * T + Vz;lat = lat + Vy * T / Rmh;lon = lon + Vx * T / Rnh / cos(lat);height = height + Vz * T;M = [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)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4), 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)*q(1)-q(2)*q(2)+q(3)*q(3)-q(4)*q(4), 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)*q(1)-q(2)*q(2)-q(3)*q(3)+q(4)*q(4)];Rmh = Re * - * e + * e * sin(lat) * sin(lat)) + height;Rnh = Re * + e * sin(lat) * sin(lat)) + height; %以上为纯惯导解算%%F1=[0 wie*sin(lat)+v(i,1)*tan(lat)/(Rnh) -(wie*cos(lat)+v(i,1)/(Rnh)) 0-1/(Rmh) 0 0 0 0;-(wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)) 0 -v(i,2)/(Rmh) 1/(Rnh)0 0 -wie*sin(lat) 0 0;wie*cos(lat)+v(i,1)/(Rnh) v(i,2)/(Rmh) 0 tan(lat)/(Rnh) 0 0 wie*cos(lat)+v(i,2)*sec(lat)*sec(lat)/(Rnh) 0 0;0 -fn(3) fn(2) v(i,2)*tan(lat)/(Rmh)-v(i,3)/(Rmh) 2*wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)-(2*wie*cos(lat)+v(i,1)/(Rnh))(2*wie*cos(lat)*v(i,2)+v(i,1)*v(i,2)*sec(lat)*sec(lat)/(Rnh)+2*wie*sin(lat)*v(i,3)) 0 0;fn(3) 0 -fn(1) -2*(wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)) -v(i,3)/(Rmh) -v(i,2)/(Rmh) -(2*wie*cos(lat)+v(i,1)*sec(lat)*sec(lat)/(Rnh))*v(i,1) 0 0;-fn(2) fn(1) 0 2*(wie*cos(lat)+v(i,1)/(Rnh)) 2*v(i,2)/(Rmh) 0 -2*wie*sin(lat)*v(i,1) 0 0;0 0 0 0 1/(Rmh) 0 0 0 0;0 0 0 sec(lat)/(Rnh) 0 0 v(i,1)*sec(lat)*tan(lat)/(Rnh) 0 0;0 0 0 0 0 1 0 0 0];G=[Cnb',zeros(3);zeros(3),Cnb';zeros(9,6)];H=[zeros(3),eye(3),zeros(3),zeros(3,6);zeros(3),zeros(3),diag([Rmh,Rnh*cos(lat),1]),zeros(3,6)]; %量测阵F2=[-Cnb',zeros(3);zeros(3),Cnb';zeros(3),zeros(3)];F=[F1,F2;zeros(6,15)]; %以上为kalman滤波模型参数F = F * T; %离散化temp1 = eye(15);disF = eye(15);for j = 1:10temp1 = F * temp1 / j;disF = disF + temp1;endtemp1 = Q * T;disQ = temp1;for j = 2:11temp2 = F * temp1;temp1 = (temp2 + temp2')/j;disQ = disQ + temp1;endZ(1) = Vx - gps_v(i,1); %量测量为纯惯导与GPS的速度差、位置差Z(2) = Vy - gps_v(i,2);Z(3) = Vz - gps_v(i,3);Z(4) = (lat - gps_pos(i,1)) * Rmh; %纬经度化为位移，单位m Z(5) = (lon - gps_pos(i,2)) * Rnh * cos(lat);Z(6) = height - gps_pos(i,3);X = disF * X_o; %kalman滤波五个公式P = disF * P_k * disF'+ disQ;K = P * H'/( H * P * H'+ R);X_k = X + K * (Z - H * X);P_k = (I - K * H) * P;x_kf(i,1) = X_k(1)/pi*180; %平台误差角x_kf(i,2) = X_k(2)/pi*180;x_kf(i,3) = X_k(3)/pi*180;x_kf(i,4) = X_k(4); %速度误差x_kf(i,5) = X_k(5);x_kf(i,6) = X_k(6);x_kf(i,7) = X_k(7); %位置误差x_kf(i,8) = X_k(8);x_kf(i,9) = X_k(9);x_kf(i,10) = X_k(10)/pi*180*3600; %陀螺随机常值漂移,单位°/hx_kf(i,11) = X_k(11)/pi*180*3600;x_kf(i,12) = X_k(12)/pi*180*3600;x_kf(i,13) = X_k(13)*10^6/g; %加计随机常值偏置，单位ugx_kf(i,14) = X_k(14)*10^6/g;x_kf(i,15) = X_k(15)*10^6/g;p_kf(i,1) = sqrt(abs(P_k(1,1)))/pi*180;p_kf(i,2) = sqrt(abs(P_k(2,2)))/pi*180;p_kf(i,3) = sqrt(abs(P_k(3,3)))/pi*180;p_kf(i,4) = sqrt(abs(P_k(4,4)));p_kf(i,5) = sqrt(abs(P_k(5,5)));p_kf(i,6) = sqrt(abs(P_k(6,6)));p_kf(i,7) = sqrt(abs(P_k(7,7)));p_kf(i,8) = sqrt(abs(P_k(8,8)));p_kf(i,9) = sqrt(abs(P_k(9,9)));p_kf(i,10) = sqrt(abs(P_k(10,10)))/pi*180*3600;p_kf(i,11) = sqrt(abs(P_k(11,11)))/pi*180*3600;p_kf(i,12) = sqrt(abs(P_k(12,12)))/pi*180*3600;p_kf(i,13) = sqrt(abs(P_k(13,13)))*10^6/g;p_kf(i,14) = sqrt(abs(P_k(14,14)))*10^6/g;p_kf(i,15) = sqrt(abs(P_k(15,15)))*10^6/g;Vx = Vx - X_k(4); %速度校正Vy = Vy - X_k(5);Vz = Vz - X_k(6);v(i,:) = [Vx, Vy, Vz];lat = lat - X_k(7); %位置校正lon = lon - X_k(8);height = height - X_k(9);pos(i,:) = [lat, lon, height];Atheta = X_k(1); %kalman滤波估计得出的失准角theta Agama = X_k(2); %kalman滤波估计得出的失准角gama Afai = X_k(3); %kalman滤波估计得出的失准角fai Ctn = [ 1, Afai, -Agama;-Afai, 1, Atheta;Agama, -Atheta, 1];Cnb = Cnb*Ctn; %更新姿态阵fai = atan(-Cnb(2,1) / Cnb(2,2));theta = asin(Cnb(2,3));gama = atan(-Cnb(1,3) / Cnb(3,3));if (Cnb(2,2) < 0)fai = fai + pi;elseif (fai < 0)fai = fai + 2*pi;endif (Cnb(3,3) < 0)if(gama > 0)gama = gama - pi;elsegama = gama + pi;endendatti(i,:) = [fai/pi*180, theta/pi*180, gama/pi*180];q(2) = sqrt(abs(1 + Cnb(1,1) - Cnb(2,2) - Cnb(3,3))) / 2;q(3) = sqrt(abs(1 - Cnb(1,1) + Cnb(2,2) - Cnb(3,3))) / 2;q(4) = sqrt(abs(1 - Cnb(1,1) - Cnb(2,2) + Cnb(3,3))) / 2;q(1) = sqrt(abs(1 - q(2) * q(2) - q(3) * q(3) - q(4) * q(4))); if (Cnb(2,3) < Cnb(3,2))q(2) = - q(2);endif (Cnb(3,1) < Cnb(1,3))q(3) = - q(3);endif (Cnb(1,2) < Cnb(2,1))q(4) = - q(4);endX_k(1:9) = 0;X_o=X_k;iend%%%绘图%%%%%%%%%%t=1:datanumber;figure(1)subplot(311);plot(t,pos(:,1)*180/pi,'r',t,gps_pos(:,1)*180/pi,'b') title('纬度');xlabel('');ylabel('°');subplot(312);plot(t,pos(:,2)*180/pi,'r',t,gps_pos(:,2)*180/pi,'b') title('经度');xlabel('');ylabel('°');subplot(313);plot(t,pos(:,3),'r',t,gps_pos(:,3),'b')title('高度');xlabel('');ylabel('m');legend('组合滤波','GPS')figure(2)subplot(311);plot(t,v(:,1),'r',t,gps_v(:,1),'b')title('东向速度');xlabel('');ylabel('m/s');subplot(312);plot(t,v(:,2),'r',t,gps_v(:,2),'b')title('北向速度');xlabel('');ylabel('m/s'); subplot(313);plot(t,v(:,3),'r',t,gps_v(:,3),'b')title('天向速度');xlabel('');ylabel('m/s'); legend('组合滤波','GPS')figure(3)subplot(311);plot(t,atti(:,1))title('航向角');xlabel('');ylabel('°'); subplot(312);plot(t,atti(:,2))title('俯仰角');xlabel('');ylabel('°'); subplot(313);plot(t,atti(:,3))title('横滚角');xlabel('');ylabel('°'); figure(4)subplot(311);plot(t,p_kf(:,1))title('P航向角误差');xlabel('');ylabel('度'); subplot(312);plot(t,p_kf(:,2))title('P俯仰角误差');xlabel('');ylabel('度'); subplot(313);plot(t,p_kf(:,3))title('P横滚角误差');xlabel('');ylabel('度'); figure(5)subplot(311);plot(t,p_kf(:,4))title('P东向速度误差');xlabel('');ylabel('m/s'); subplot(312);plot(t,p_kf(:,5))title('P北向速度误差');xlabel('');ylabel('m/s'); subplot(313);plot(t,p_kf(:,6))title('P天向速度误差');xlabel('');ylabel('m/s'); figure(6)subplot(311);plot(t,p_kf(:,7))title('P纬度误差');xlabel('');ylabel('度'); subplot(312);plot(t,p_kf(:,8))title('P经度误差');xlabel('');ylabel('度'); subplot(313);plot(t,p_kf(:,9))title('P高度误差');xlabel('');ylabel('m');figure(7)subplot(311);plot(t,p_kf(:,10))title('P东向陀螺');xlabel('');ylabel('度/小时'); subplot(312);plot(t,p_kf(:,11))title('P北向陀螺');xlabel('');ylabel('度/小时'); subplot(313);plot(t,p_kf(:,12))title('P天向陀螺');xlabel('');ylabel('度/小时'); figure(8)subplot(311);plot(t,p_kf(:,13))title('P东向加计');xlabel('');ylabel('ug'); subplot(312);plot(t,p_kf(:,14))title('P北向加计');xlabel('');ylabel('ug'); subplot(313);plot(t,p_kf(:,15))title('P天向加计');xlabel('');ylabel('ug');。