导航与制导实验报告INS与GPS位置组合导航

《导航与制导实验报告INS与GPS位置组合导航》由会员分享，可在线阅读，更多相关《导航与制导实验报告INS与GPS位置组合导航（18页珍藏版）》请在装配图网上搜索。

1、1完成INS与GPS位置组合导航的仿真;2. 画出组合导航后的位置误差、速度误差曲线 ；3. 画出原始轨迹与组合导航后的轨迹比较图；（画图时弧度制单位要转换成度分秒制单位）4. 结果分析5. 提交纸版实验报告（附上代码）R=6378160;%地球半径（长半轴）f=1/298.3;%地球扁率wie=7.2921151467e-5;%地球自转角速率g0=9.7803267714;%重力加速度基础值deg= n /180;%角度min=deg/60;%角分sec= min/60;%角秒hur=3600;%小时dph=deg/hur;%度/时ts=0.1;%仿真采样时间三. 组合导航仿真变量GPS_S

2、ample_Rate=10; %GPS 采样时间Runs=10;%由于随机误差，使用Kalman滤波时，应多次滤波，以求平均值Tg = 3600;%陀螺仪Markov过程相关时间Ta = 1800;%加速度计Markov过程相关时间四. Kalman Filter:估计状态初始值：Xk = zeros(18,1);估计协方差初始值：Pk=diag(mi n,min, mi n,0.5,0.5,0.5,30/Re,30/Re,30,0.1*dph,0.1*dph,0.1*dph,0.1*dph,0.1*dph, 0.1*dph, 1.e-3,1.e-3,1.e-3.A2); %18*18矩阵系统

3、噪声方差：Qk=1e-6*diag(0.01,0.01,0.01,0.01,0.01,0.01,0.9780,0.9780,0.9780).A2量测噪声方差：Rk=diag(1e-5,1e-5,10.3986).A2系数矩阵F , G, H的表示，参考课件6.2.1。五. 可能用到的公式四元数Q的即时修正（符号？表示四元数乘法）000nhy %式中-tb = 0-Sbx - bby - Abz为向量扩展四元数，标量部分为(2)四元数Q的归一化(3)姿态矩阵Cb的计算7o + gf -於一诸2(孤段+如)2(如如+如喘) 硝一亦+於一硝 2(也舸一怖) 2(fofl3 + qoqi)谄一卅一於十

4、诸-7*11 A12 迫 %以心也L A3233】sin if/sin Osin y 十 cos 申 cos ysinxp cos 0- sin sin6 cosy cos ip sinycos sin 0 sin y sin ip cos y cos ij/ cos 9cos 屮 sin 0 cos y sin ip sin ycos 0 sin ysin 0cos 0 cos yt(4) 提取姿态角r力二岂标变换cw(6)速度计算) X vni/1 = / + gn 其中矿= 0 0 gT(7)地球速率及地理坐标系相对地球坐标系的转动角速率RE + FT tanLRe + h0 a)ie

5、cosL ci)lG机体系相对于地理系的转动角速率在机体系中的投影0 sin Y cos fl 10 1- sin 6 y0 cosy cos 6 0.siny (8) 位置计算vn1= 3 RN+h2 = (RE +h) cos Lh = vA(9) 主曲率半径R = R(1 - 2/ + 3/ sinzL)RN = /f(l + f sin2L)(10) 重力加速度1 + 0.001 931 851 386 39sin2L = 9.780 3267714 x ,VI - OJ06 694 379 990 I3snL(11) 连续系统离散化公式(简化形式) Fic =】+ F (小 TGk

6、= (l + F(tk)*T)*G(tk)*THk = H(tk)其中，1是单位矩阵，T是仿真米样时间。六.数据文件说明dataWbibN.txt % dataFbibN.txt % dataPos.txt叠加噪声的陀螺仪角速度输出dataVn.txt叠加噪声的加速度计比力输出%原始轨迹的位置数据（依次是纬度L ,经度，高度h）%原始轨迹的速度数据（依次是东速度、北速度、天速度）att0=0;0;0.3491 % 姿态解算矩阵初始值（依次是俯仰角 -横滚角 ，航向角“）dataGPSposN.txt %叠加噪声的GPS位置数据（即等间隔采样原始轨迹的位置数据，样间隔是10,即第10、20,的数

7、据，并叠加噪声）七.仿真流程图整个仿真过程流程图在整体仿真流程图中， Kalman 滤波可以由子函数 kalman_GPS_INS_correct.m 进行。捷联解算可按照如下捷联解算流程图进行。开執八. 实验思路根据上述两个流程图，要实现 INS 与 GPS 位置组合导航，需要在已给的 INS 导航代码中， 加入当 k 为 10 的倍数时进行 Kalman 滤波，修正位置、速度数据环节，则添加代码如下(添 加位置见完整程序)： %添加程序F,G,H = GetConSis(vtC(:,k), posC(:,k), q_1, fn(:,k), Tg, Ta);% 利用GetC on Sis 得

8、到 F,G,H 矩阵if rem(k,10) = 0%判断k是否为10的倍数，若是则进行kalman滤波，修正。E_att,E_pos, E_vn, Xk, Pk = kalman_GPS_INS_correct(Xk, Qk, else Zk = posC(:,k) - p_gps(:,(k)/10);Pk, F, G, H, ts, Rk);%利用 kalman_GPS_INS_correct进行kalman滤波Qk,E_att, E_pos, E_vn, Xk, Pk = kalman_GPS_INS_correct(Xk, Pk, F, G, H, ts, Rk, Zk); end a

9、ttC(:,k) = attC(:,k) - E_att;posC(:,k) = posC(:,k) - E_pos; vtC(:,k) = vtC(:,k) - E_vn;%修正位置，速度数据九完整代码及注释clc;clear;close all ; %捷联惯导更新仿真，四元数法，一阶近似算法，不考虑圆锥补偿和划桨补偿ts=0.1;%采样时间Re=6378160;%地球长半轴wie=7.2921151467e-5;%地球自转角速度f=1/298.3;%地球扁率g0=9.7803; %重力加速度基础值deg=pi/180;%角度min=deg/60;%角分sec=min/60;%角秒hur=3

10、600; %小时%度 /小时dph=deg/hur;%读取数据wbibS=dlmread(dataWbibN.txt);fbS=dlmread(dataFbibN.txt);posS=dlmread(dataPos.txt);vtetS=dlmread(dataVn.txt);p_gps=dlmread(dataGPSposN.txt%统计矩阵初始化mm,nn=size(posS); posSta=zeros(mm,nn); vtSta=posSta; attSta=posSta; posC(:,1)=posS(:,1); vtC(:,1)=vtetS(:,1); attC(:,1)= 0;0

11、;%位置向量初始值%速度向量初始值0.3491;%姿态解算矩阵初始值%测量噪声方差Qk=1e-6*diag(0.01,0.01,0.01,0.01,0.01,0.01,0.9780,0.9780,0.9780)A2 ; % 系统噪声方差矩阵Rk=diag(1e-5,1e-5,10.3986)A2;Tg = 3600*ones(3,1);%陀螺仪 Markov 过程相关时间Ta = 1800*ones(3,1);%加速度计 Markov 过程相关时间%GPS_Sample_Rate=10; %GPS 采样率太高效果也不好StaNum=10; %重复运行次数，用来求取统计平均值for OutLoo

12、p=1:StaNumPk = diag(min,min,min, 0.5,0.5,0.5, 30./Re,30./Re,30, 0.1*dph,0.1*dph,0.1*dph, 0.1*dph,0.1*dph,0.1*dph,1.e-3,1.e-3,1.e-3.A2);%初始估计协方差矩阵Xk = zeros(18,1);%初始状态%N=size(posS,2);% j=1;for k=1:N-1 si=sin(attC(1,k);ci=cos(attC(1,k); sj=sin(attC(2,k);cj=cos(attC(2,k);sk=sin(attC(3,k);ck=cos(attC(3

13、,k);%k 时刻姿态矩阵M=cj*ck+si*sj*sk, ci*sk, sj*ck-si*cj*sk; -cj*sk+si*sj*ck, ci*ck, -sj*sk-si*cj*ck; -ci*sj, si, ci*cj; %Cnb 矩阵 q_1=sqrt(abs(1.0+M(1,1)+M(2,2)+M(3,3)/2.0;sign(M(3,2)-M(2,3)*sqrt(abs(1.0+M(1,1)-M(2,2)-M(3,3)/2.0;sign(M(1,3)-M(3,1)*sqrt(abs(1.0-M(1,1)+M(2,2)-M(3,3)/2.0; sign(M(2,1)-M(1,2)*sq

14、rt(abs(1.0-M(1,1)-M(2,2)+M(3,3)/2.0; fn(:,k)=M*fbS(:,k); %比力的坐标变换%利用%添加程序F,G,H = GetConSis(vtC(:,k), posC(:,k), q_1, fn(:,k), Tg, Ta);GetConSis 得到 F,G,H 矩阵if rem(k,10) = 0E_att,Pk, F, G, H, ts, Rk);%判断k是否为10的倍数，若是则进行kalman滤波，修正。E_pos, E_vn, Xk, Pk = kalman_GPS_INS_correct(Xk,%利用 kalman_GPS_INS_corre

15、ct进行 kalman 滤波Qk,elseZk = posC(:,k) - p_gps(:,(k)/10);Qk,E_att, E_pos, E_vn, Xk, Pk = kalman_GPS_INS_correct(Xk, Pk, F, G, H, ts, Rk, Zk);end attC(:,k) = attC(:,k) - E_att; posC(:,k) = posC(:,k) - E_pos;vtC(:,k) = vtC(:,k) - E_vn;%修正位置，速度数据%地球系相对惯性系的转动角速度在%捷联惯导解算 wnie=wie*0;cos(posC(1,k);sin(posC(1,

16、k); 导航系(地理系)的投影%计算主曲率半径Rm=Re*(1-2*f+3*f*si n(posC(1,k)f2)+posC(3,k);Rn=Re*(1+f*si n(posC(1,k)F2)+posC(3,k);wnen=-vtC(2,k)/(Rm+posC(3,k);vtC(1,k)/(Rn+posC(3,k);vtC(1,k)*tan(p osC(1,k)/(Rn+posC(3,k); % 导航系相对地球系的转动角速度在导航系的投影g=g0+0.051799*si n(posC(1,k)F2-0.94114e-6*posC(3,k);%重力加速度gn=0;0;-g;%导航坐标系的重力加速

17、度wbnbC(:,k)=wbibS(:,k)-M(wnie+wnen);%姿态角转动角速度计算q=1.0/2*qmul(q_1,0;wbnbC(:,k)*ts+q_1;%姿态四元数更新q=q/sqrt(q*q);%四元数归一化%姿态角更新 q11=q(1)*q(1);q12=q(1)*q(2);q13=q(1)*q(3);q14=q(1)*q(4); q22=q(2)*q(2);q23=q(2)*q(3);q24=q(2)*q(4);q33=q(3)*q(3);q34=q(3)*q(4); q44=q(4)*q(4);T=q11+q22-q33-q44, 2*(q23-q14), 2*(q24

18、+q13);2*(q23+q14), q11-q22+q33-q44, 2*(q34-q12);2*(q24-q13), 2*(q34+q12), q11-q22-q33+q44;attC(:,k+1)=asin(T(3,2);atan(-T(3,1)/T(3,3);atan(T(1,2)/T(2,2);%横滚角gamma修正if T(3,3)0if attC(2,k+1)0attC(2,k+1)=attC(2,k+1)+pi;else attC(2,k+1)=attC(2,k+1)-pi;endend%航向角 psi 修正if T(2,2)0 attC(3,k+1)=attC(3,k+1)+

19、pi;else attC(3,k+1)=attC(3,k+1)-pi;endendif abs(T(2,2)0 attC(3,k+1)=pi/2.0;else attC(3,k+1)=-pi/2.0;end end%速度更新vtC(:,k+1)=vtC(:,k)+ts*(fn(:,k)+gn-cross(2*wnie+wnen),vtC(:,k); %位置更新 posC(1,k+1)=posC(1,k)+ts*vtC(2,k)/(Rm+posC(3,k);%纬度 posC(2,k+1)=posC(2,k)+ts*vtC(1,k)/(Rn+posC(3,k)*cos(posC(1,k);度pos

20、C(3,k+1)=posC(3,k)+ts*vtC(3,k);%高度%end% attSta=attSta+attC; vtSta=vtSta+vtC; posSta=posSta+posC;end %对统计矩阵取平均 attC=1./StaNum*attSta; posC=1./StaNum*posSta; vtC=1./StaNum*vtSta;%解算值与仿真值的误差，作图 %结算矩阵均为 3x(N+1) ，需做处理%N点数据，相邻两点采样间隔为0.1s，做作图横轴修正for i=1:N time(i)=i*ts;Rm = Re*(1-2*f+3*f*(si n( attC(1,i)f2)

21、;Rn = Re*(1+f*(si n( attC(1,i)A2);Latitude_error(i)=(posC(1,i)-posS(1,i)*Rm; Longitude_error(i)=(posC(2,i)-posS(2,i)*Rn*cos(attC(1,i); end posCp=posC(:,1:N);figure;subplot(131);plot(time,Latitude_error);title(/s );ylabel( itdeltaL /m);grid on;subplot(132);plot(time,Longitude_error);title(/s );ylabel

22、( itdeltaphi /m);gridsubplot(133);plot(time,posCp(3,:)-posS(3,:);title( );xlabel( Time /s );ylabel( itdeltah /m vtCp=vtC(:,1:N); figure;subplot(131);plot(time,vtCp(1,:)-vtetS(1,:);title( );xlabel( Time /s );ylabel( itdeltavelocity east /(m/s) subplot(132);plot(time,vtCp(2,:)-vtetS(2,:);title( );xlab

23、el( Time /s );ylabel( itdeltavelocity north /(m/s) subplot(133);plot(time,vtCp(3,:)-vtetS(3,:);title( );xlabel( Time /s );ylabel( itdeltavelocity up /(m/s)%三维飞行轨迹图 figure;plot3(posS(2,:)/pi*180,posS(1,:)/pi*180,posS(3,:), hold on;plot3(posCp(2,:)/pi*180,posCp(1,:)/pi*180,posCp(3,:), ylabel( 纬度 L /ar

24、cdeg );xlabel( 经度 phi /arcdeg 纬度误差 );xlabel( Time 经度误差 );xlabel( Time on; 高度误差);grid on;);grid北速度误差);grid天速度误差);gridonk );r );grid on; );zlabel( 高度 hon;on;/m);title(黑线-仿真器飞行轨迹，红线-INS解算飞行轨迹);高ELh度100008000600040002000109纬度误差500Time /sO mLd50-200经度误差高度误差东速度误差天速度误差1000北速度误差xA eh vvoI ons/Time /sTime /s1000Time /s以上所有结果与所给参考结果完全一致，仿真正确。原始INS代码飞行轨迹仿真结果如图:60004000Fh 度 高80002000从以上两种方法的飞行轨迹仿真图可以看出，加入kalman滤波的INS/GPS组合导航的导航精度明显高于INS导航，飞行轨迹肉眼看来完全贴合原始轨迹，即组合导航结果基本可以满足一般及稍高精度要求的导航需求。