北航惯性导航作业一

惯性导航作业

一、作业内容：

1、数据说明：

惯导系统为指北方位的平台系统。

初始经度为：116.344762072818度 纬度为：39.981430918136度 高度为：40.8236米。

初始姿态角为[0 0 0]（俯仰，横滚，航向，单位：度，航向角以逆时针为正）。 初始速度为0米/秒，飞行高度不变（即：无须计算高度通道）。

排列顺序为 一~三行分别为东、北、天向信息,共600秒数据，陀螺仪和加速度计采样周期为0.01秒。

二、作业要求：

1、以经度为横轴，纬度为纵轴（单位均转换为：度）作出系统位置坐标曲线图并附在报告中；

2、以列表形式给出系统纬度、经度、东向速度、北向速度的终点值；

3、作业以纸质报告形式提交，在报告中写“程序流程图”和小结（小结不要写套话，按条简捷的写），报告附源程序，封皮写明联系方式。

三、程序流程

开始读取数据并赋初值根据公式计算重力加速度和曲率半径否两次积分求得经度、纬度循环变量i>=60000是根据结果绘制图像结束

四、结果

经度（°） 116.3487 五、总结

本次作业是处理实际数据然后得到导航结果，以此对之前所学的进行了一下总结。

由于时间问题，对加速度和速度的积分采用的是欧拉法，相比四阶龙格库塔法，这种方法计算简单但精度较低。遗憾的是并没有比较两者的差距。计算过程中发现Z方向速度不为零，即高度并不像假设的是恒定不变的。

纬度（°） 40.0653 东向速度（m/s） 北向速度（m/s） -63.6206 63.6559 六、源程序

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)+V

y(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(1,60001)*180/pi

Lambdaend= Lambda(1,60001)*180/pi Vxend=Vx(1,60001) Vyend=Vy(1,60001)

plot(Lambda*180/pi,L*180/pi);xlabel('经度/°'),ylabel('纬度/°'); grid on