导航与制导原理实验2.docVIP

导航与制导原理实验 --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过程相关时间 四、Kalman Filter: 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.9 780]).^2 4、量测噪声方差： Rk=diag([1e-5,1e-5,10.3986]).^2 5、系数矩阵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 滤波可以由实验指导老师提供的子函数进行。捷联解算可按照下一步骤中的捷联解算流程图进行。 2、实验指导老师提供的子函数有： GetConSis.m glvs.m kalman_GPS_INS_correct.m qmul.m 七、捷联解算流程图： 八、实验程序代码如下： 本实验程序中所有变量的命名参照了在机房实验时实验老师演示的程序以及在本实验报告开头所涉及的全局变量名。最终实验采用老师要求的方向余弦法完成。 ts=0.1;%采样时间 Re=6378160;%地球长半轴 wie=7.2921151467e-5;%地球自转角速率 f=1/298.3;%地球扁率 g0=9.7803;%重