搜索
导航与制导原理实验--INS与GPS位置组合导航的仿真姓名：戴怡轩班级：自动化12学号：2110504027日期：2014年5月1日导航与制导原理仿真实验——INS与GPS位置组合导航的仿真一、要求：1、完成INS与GPS位置组合导航的仿真;2、画出组合导航后的位置误差、速度误差曲线;3、画出原始轨迹与组合导航后的轨迹比较图；（画图时，弧度制单位要转换成度分秒制单位）4、结果分析5、提交纸版实验报告（附上代码）二、全局变量：R=6378160;%地球半径（长半轴）f=1/298.3;%地球扁率wie=7.2921151467e-5;%地球自转角速率g0=9.7803267714;%重力加速度基础值deg=π/180;%角度min=deg/60;%角分sec=min/60;%角秒hur=3600;%小时dph=deg/hur;%度/时ts=0.1;%仿真采样时间三、组合导航仿真变量：GPS_Sample_Rate=10;%GPS采样时间Runs=10;%由于随机误差，使用Kalman滤波时，应多次滤波，以求平均值Tg=3600;%陀螺仪Markov过程相关时间Ta=1800;%加速度计Markov过程相关时间四、KalmanFilter:1、估计状态初始值：Xk=zeros(18,1);2、估计协方差初始值：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*dph,0.1*dph,1.e-3,1.e-3,1.e-3].^2);%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]).^24、量测噪声方差：Rk=diag([1e-5,1e-5,10.3986]).^25、系数矩阵F，G，H的表示，参考课件6.2.1。五、实验用到的数据文件：dataWbibN.txt%叠加噪声的陀螺仪角速度输出dataFbibN.txt%叠加噪声的加速度计比力输出dataPos.txt%原始轨迹的位置数据（依次是纬度�、经度�、高度h）dataVn.txt%原始轨迹的速度数据（依次是东速度、北速度、天速度）att0=[0;0;0.3491]%姿态解算矩阵初始值（依次是俯仰角�、横滚角�、航向角ψ）dataGPSposN.txt%叠加噪声的GPS位置数据（即等间隔采样原始轨迹的位置数据，采样间隔是10，即第10、20,……的数据，并叠加噪声）六、仿真粗略流程图：1、在仿真流程图中，Kalman滤波可以由实验指导老师提供的子函数进行。捷联解算可按照下一步骤中的捷联解算流程图进行。开始读取相关数据位置、速度、姿态、赋初值K=k+1K是否是10的倍数Kalman滤波，修正位置、速度数据继续捷联解算捷联解算，并只更新Xk,Pk循环是否结束误差计算，画图显示结束NNYY2、实验指导老师提供的子函数有：GetConSis.mglvs.mkalman_GPS_INS_correct.mqmul.m七、捷联解算流程图：读入已存的飞行轨迹相关数据飞机姿态、速度、位置赋初值开始姿态四元数即使修正并归一化姿态矩阵计算姿态角解算比力的坐标变换速度解算位置解算K=k+1加速度计、陀螺仪采样值输入姿态速率计算获得前一时刻姿态四元数循环是否结束N误差计算，画图显示结束Y八、实验程序代码如下：本实验程序中所有变量的命名参照了在机房实验时实验老师演示的程序以及在本实验报告开头所涉及的全局变量名。最终实验采用老师要求的方向余弦法完成。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');%叠加噪声的加速度比例输出posS=dlmread('dataPos.txt');%原始轨迹的位置数据vtetS=dlmread('dataVn.txt');%原始轨迹的速度数据p_gps=dlmread('dataGPSposN.txt');%叠加噪声的GPS位置数据%统计矩阵初始化[mm,nn]=size(posS);posSta=zeros(mm,nn);%位置解算结果统计vtSta=posSta;%速度解算结果统计attSta=posSta;%姿态解算结果统计posC(:,1)=posS(:,1);%位置向量初始值vtC(:,1)=vtetS(:,1);%速度向量初始值attC(:,1)=[0;0;0.3491];%姿态解算矩阵初始值%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%GPS噪声处理Qk=1e-6*diag([0.01,0.01,0.01,0.01,0.01,0.01,0.9780,0.9780,0.9780]).^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;%重复运行次数，用于求取统计平均值forOutLoop=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].^2);%初始估计协方差矩阵Xk=zeros(18,1);%初始状态%%N=size(posS,2);%获取原始位置数据中的数据组数%j=1;fork=1:N-1si=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(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*sin(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);%姿态角转动角速率计算，需测得角速度%%%%%%%%%%%%四元数法%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+q13);%2*(q23+q14),q11-q22+q33-q44,2*(q34-q12);%2*(q24-q13),2*(q34+q12),q11-q22-q33+q44];%%%%%%%%%%%%四元数法%%%%%%%%%%%%方向余弦法WbnbC=[0,-wbnbC(3,k),wbnbC(2,k);wbnbC(3,k),0,-wbnbC(1,k);-wbnbC(2,k),wbnbC(1,k),0];T=M*WbnbC*ts+M;%%%%%%%%%%%%方向余弦法%姿态更新attC(:,k+1)=[asin(T(3,2));atan(-T(3,1)/T(3,3));atan(T(1,2)/T(2,2))];%横滚角gamma修正ifT(3,3)0ifattC(2,k+1)0attC(2,k+1)=attC(2,k+1)+pi;elseattC(2,k+1)=attC(2,k+1)-pi;endend%航向角psi修正ifT(2,2)0ifT(1,2)0attC(3,k+1)=attC(3,k+1)+pi;elseattC(3,k+1)=attC(3,k+1)-pi;endendifabs(T(2,2))1.0e-20ifT(1,2)0attC(3,k+1)=pi/2.0;elseattC(3,k+1)=-pi/2.0;endend%速度更新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)));%经度posC(3,k+1)=posC(3,k)+ts*vtC(3,k);%高度[F,G,H]=GetConSis(vtC(:,k),posC(:,k),T,fn(:,k),Tg,Ta);%得到FGH矩阵if(mod(k+1,10)==0)%posC(:,k)=p_gps(:,(k/10));%卡尔满滤波%求解ZKZk=p_gps(:,((k+1)/10))-posC(:,k+1);[E_att,E_pos,E_vn,Xk,Pk]=kalman_GPS_INS_correct(Xk,Qk,Pk,F,G,H,ts,Zk,Rk);%卡尔满滤波posC(:,k+1)=posC(:,k+1)+E_pos;%位置修正vtC(:,k+1)=vtC(:,k+1)+E_vn;%速度修正else[E_att,E_pos,E_vn,Xk,Pk]=kalman_GPS_INS_correct(Xk,Qk,Pk,F,G,H,ts);end%GPS