北航惯性导航作业1.docVIP

惯性导航作业  一、作业内容： 1、数据说明： 惯导系统为指北方位的平台系统。 初始经度为：116.344762072818度 纬度为：39.981430918136度 高度为：40.8236米。 初始姿态角为[0 0 0]（俯仰，横滚，航向，单位：度，航向角以逆时针为正）。 初始速度为0米/秒，飞行高度不变（即：无须计算高度通道）。 排列顺序为 一~三行分别为东、北、天向信息,共600秒数据，陀螺仪和加速度计采样周期为0.01秒。 二、作业要求： 1、以经度为横轴，纬度为纵轴（单位均转换为：度）作出系统位置坐标曲线图并附在报告中； 2、以列表形式给出系统纬度、经度、东向速度、北向速度的终点值； 3、作业以纸质报告形式提交，在报告中写“程序流程图”和小结（小结不要写套话，按条简捷的写），报告附源程序，封皮写明联系方式。 三、程序流程 四、结果 经度（°） 纬度（°） 东向速度（m/s） 北向速度（m/s） 116.3487 40.0653 -63.6206 63.6559 五、总结 本次作业是处理实际数据然后得到导航结果，以此对之前所学的进行了一下总结。 由于时间问题，对加速度和速度的积分采用的是欧拉法，相比四阶龙格库塔法，这种方法计算简单但精度较低。遗憾的是并没有比较两者的差距。计算过程中发现Z方向速度不为零，即高度并不像假设的是恒定不变的。 六、源程序 clc clear a=load(C:\Users\Administrator\Documents\MATLAB/jlfw.dat); wib_INSc=a(:,2:4);f_INSc=a(:,5:7); wib_INSc（单位：rad/s） %第一列：数据包序号 第二至四列：分别为东、北、天向陀螺仪角速率信息 %第五至七列：分别为东、北、天向比力信息f_INSc（单位：m/s^2）. L(1,:)=zeros(1,60001); Lambda(1,:)=zeros(1,60001); Vx(1,:)=zeros(1,60001); Vy(1,:)=zeros(1,60001); Vz(1,:)=zeros(1,60001); Rx(1,:)=zeros(1,60001);%定义存放卯酉圈曲率半径数据的矩阵 Ry(1,:)=zeros(1,60001);%定义存放子午圈曲率半径数据的矩阵 L(1,1)=39.981430918136/180*pi;%纬度初始值 单位：弧度 Lambda(1,1)=116.344762072818/180*pi;%经度初始值 单位：弧度 Vx(1,1)=0;%初始速度x方向分量 Vy(1,1)=0;%初始速度y方向分量 Vz(1,1)=0;%初始速度z方向分量 fx=f_INSc(1,1:end);%x方向的比力数据 fy=f_INSc(2,1:end);%y方向的比力数据 fz=f_INSc(3,1:end);%z方向的比力数据 g0=9.78049; Wie=7.2921E-5;%地球自转角速度 Re=6378245;%长半径 e=1/298.3;%椭圆度 t=0.01;%采样时间 for i=1:60000 g=g0*(1+0.0052884*sin(L(1,i))^2-0.0000059*sin(2*L(1,i))^2);%重力加速度 Rx(1,i)=Re/(1-e*(sin(L(1,i)))^2);%根据纬度计算卯酉圈曲率半径 Ry(1,i)=Re/(1+2*e-3*e*(sin(L(1,i)))^2);%根据纬度计算子午圈曲率半径 Vx(1,i+1)=(fx(1,i)+(2*Wie*sin(L(1,i))+Vx(1,i)*tan(L(1,i))/Rx(1,i))*Vy(1,i)-(2*Wie*cos(L(1,i))+Vx(1,i)/Rx(1,i))*Vz(1,i))*t+Vx(1,i); %计算速度x方向分量 Vy(1,i+1)=(fy(1,i)-(2*Wie*sin(L(1,i))+Vx(1,i)*tan(L(1,i))/Rx(1,i))*Vx(1,i)+Vy(1,i)*Vz(1,i)/Ry(1,i))*t+Vy(1,i); %计算速度y方向分量 Vz(1,i+1)=(fz(1,i)+(2*Wie*cos(L(1,i)+Vx(1,i))/Rx(1,i))*Vx(1,i)+Vy(1,i)*Vy(1,i)/Ry(1,i)-g)*t+Vz(1,i);%计算速度z方向分量 L(1,i+1)=t*Vy(1,i)/Ry(1,i)+L(1,i); Lambda(1,i+1)=t*Vx(1,i)/(Rx(1,i)*cos(L(1,i)))+ Lambda(1,i); end Lend=L