导航及制导实验报告INS及GPS位置组合导航.docx

导航与制导原理实验报告一．实验要求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:估计状态初始值：Xk = zeros(18,1);估计协方差初始值：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矩阵系统噪声方差：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系数矩阵 F， G， H 的表示，参考课件 6.2.1。五．可能用到的公式(1)四元数Q的即时修正（符号?表示四元数乘法）式中为向量扩展四元数，标量部分为0。(2)四元数Q 的归一化(3)姿态矩阵的计算(4)提取姿态角(5)比力的坐标变换(6)速度计算(7)地球速率及地理坐标系相对地球坐标系的转动角速率机体系相对于地理系的转动角速率在机体系中的投影(8)位置计算(9)主曲率半径(10)重力加速度(11)连续系统离散化公式（简化形式）其中，I是单位矩阵，T是仿真采样时间。六．数据文件说明dataWbibN.txt %叠加噪声的陀螺仪角速度输出 dataFbibN.txt %叠加噪声的加速度计比力输出dataPos.txt %原始轨迹的位置数据（依次是纬度L，经度，高度h） dataVn.txt %原始轨迹的速度数据（依次是东速度、北速度、天速度） att0=[0;0;0.3491] %姿态解算矩阵初始值（依次是俯仰角，横滚角，航向角ψ） dataGPSposN.txt %叠加噪声的GPS位置数据（即等间隔采样原始轨迹的位置数据，采样间隔是10，即第10、20,……的数据，并叠加噪声）七．仿真流程图整个仿真过程流程图在整体仿真流程图中，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);%利用GetConSis得到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, Pk, F, G, H, ts, Rk);%利用kalman_GPS_INS_correct进行kalman滤波elseZk = posC(:,k) - p_gps(:,(k)/10); [E_att, E_pos, E_vn, Xk, Pk] = kalman_