GPS与惯性捷联导航仿真实验报告西安交通大学Word格式.docx

1、陀螺漂移：，（2）速度误差（3）位置误差（4）陀螺漂移（5）加速度计误差该式为一阶Markov过程噪声，为相关时间（6）综合方程2解算算法框图如图3-1所示 NbaY图3-1组合导航捷联解算算法流程图四、实验要求:1.1、完成INS与GPS位置组合导航的仿真;2.2、画出组合导航后的位置误差、速度误差曲线;3.3、画出原始轨迹与组合导航后的轨迹比较图；4.（画图时，弧度制单位要转换成度分秒制单位） 5.4、结果分析 6.5、提交纸版实验报告（附上代码）五、实验步骤7.将已有程序导入到MATLAB中8.将四元素法改为方向余弦法9.修改求解F、Q、H矩阵函数10.在INS导航解算主程序中加入卡尔曼

2、滤波修正误差11.得到速度误差、位置误差以及姿态误差图12.进行结果分析六、实验结果1.组合导航位置误差曲线如图6-1所示 图 6-12、组合导航速度误差曲线如图6-2所示 图6-23、原始轨迹与组合导航后的轨迹比较曲线如图6-3所示 图6-3七、结果分析1.平均误差：（1）组合导航经度误差均值为0；维度误差均值为0；高度误差均值为-0.6042。东速度误差均值为-0.4927；北速度误差均值为-0.6110；天速度误差均值为-0.3059。（2）INS导航高度误差均值为-199.2368。东速度误差均值为-1.1347；北速度误差均值为-0.3161；天速度误差均值为-1.631。2.误差标

3、准差：经度误差标准差为0；维度误差标准差为0；高度误差标准差为22。东速度误差标准差为1.8936；北速度误差标准差为2.9869；天速度误差标准差为1.0253高度误差标准差为269.5201。东速度误差标准差为0.9635；北速度误差标准差为0.1972；天速度误差标准差为1.38523.曲线效果比较： INS导航轨迹与原始轨迹曲线如图7-1所示图7-1 INS导航轨迹与原始轨迹曲线 4.通过对单纯INS导航和INS-GPS组合导航结果的分析可知，组合导航在准确度和精度上都高于单纯INS导航。在INS导航时间较长时会出现如图6-1所示的严重偏差，而INS-GPS组合导航的结果如图5-3所示

4、能在时间较长时保持准确。（代码见附录）附录：源代码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=3600;%小时dph=deg/hur;%度/小时%读取数据wbibS=dlmread（dataWbibN.txt）;fbS=dlmread（dataFbibN.txt

5、posS=dlmread（dataPos.txtvtetS=dlmread（dataVn.txtp_gps=dlmread（dataGPSposN.txt%统计矩阵初始化mm,nn=size（posS）;posSta=zeros（mm,nn）;vtSta=posSta;attSta=posSta;posC（:,1）=posS（:,1）; %位置向量初始值vtC（:,1）=vtetS（: %速度向量初始值attC（:,1）= 0; 0; 0.3491; %姿态解算矩阵初始值Qk=1e-6*diag（0.01,0.01,0.01,0.01,0.01,0.01,0.9780,0.9780,0.978

6、0）.2;%系统噪声方差矩阵Rk=diag（1e-5,1e-5,10.3986）.2; %测量噪声方差Tg = 3600*ones（3,1）; %陀螺仪Markov过程相关时间Ta = 1800*ones（3,1）; %加速度计Markov过程相关时间GPS_Sample_Rate=10; %GPS采样率太高效果也不好StaNum=10;%重复运行次数，用于求取统计平均值for OutLoop=1:StaNum Pk = 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*d

7、ph,0.1*dph, 1.e-3,1.e-3,1.e-3.2）; %初始估计协方差矩阵 Xk = zeros（18,1）; %初始状态 % N=size（posS,2）;% j=1; for k=1:N-1 if（mod（k,10）=0） %if这一块是我加的 Zk=posC（:,k）-p_gps（:,k/10）; E_att, E_pos, E_vn, Xk, Pk = kalman_GPS_INS_correct（ Xk, Qk, Pk, F, G, H, ts ,Zk,Rk ）; attC（:,k）=attC（:,k）-E_att; posC（:,k）=posC（:,k）-E_pos;

8、 vtC（:,k）=vtC（:,k）-E_vn; end 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,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

9、（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）*sqrt（abs（1.0-M（1,1）-M（2,2）+M（3,3）/2.0; fn（:,k）=M*fbS（:,k）;%比力的坐标变换 %捷联惯导解算 wnie=wie*0;cos（posC（1,k）;sin（posC（1,k）;%地球系相对惯性系的转动角速度在导航系（地理系）的投影 %计算主曲率半径 Rm=Re*（1-2*f+3*f*s

10、in（posC（1,k）2）+posC（3,k）; Rn=Re*（1+f*sin（posC（1,k）2）+posC（3,k）; wnen=-vtC（2,k）/（Rm+posC（3,k）;vtC（1,k）/（Rn+posC（3,k）;vtC（1,k）*tan（posC（1,k）/（Rn+posC（3,k）;%导航系相对相对地球系的转动角速度在导航系的投影 g=g0+0.051799*sin（posC（1,k）2-0.94114e-6*posC（3,k）;%重力加速度 gn=0;0;-g;%导航坐标系的重力加速度 wbnbC（:,k）=wbibS（:,k）-M（wnie+wnen）; %姿态角转动

11、角速率计算 q=1.0/2*qmul（q_1,0;wbnbC（:,k）*ts+q_1; %姿态四元数更新 q=q/sqrt（q*q）;%四元数归一化 %方向余弦法 WbnbC=0,-wbnbC（3,k）,wbnbC（2,k）; wbnbC（3,k）,0,-wbnbC（1,k）; -wbnbC（2,k）,wbnbC（1,k）,0; a=WbnbC*ts; T=M+M*exp（a）; %T=M+M*WbnbC*ts; %姿态角更新 %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（

12、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+q13）; % 2*（q23+q14）, q11-q22+q33-q44, 2*（q34-q12）; % 2*（q24-q13）, 2*（q34+q12）, q11-q22-q33+q44;,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） if attC（2,k+1） attC（2

13、,k+1）=attC（2,k+1）+pi; else attC（2,k+1）=attC（2,k+1）-pi; %航向角psi修正 if T（2,2） attC（3,k+1）=attC（3,k+1）+pi; attC（3,k+1）=attC（3,k+1）-pi; if abs（T（2,2）1.0e-20 attC（3,k+1）=pi/2.0; attC（3,k+1）=-pi/2.0; %速度更新,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）;%