Nav 利用陀螺仪和加表的输出进行惯导结算

利用陀螺仪和加表的输出进行惯导结算，得到飞机的位置，速度，姿态角

%====本程序为无人机1的捷联惯导的解算程序（导航系：东北天，由惯性器件的输出解算出飞行器的位置、速度、姿态信息）======

%%%%%test1为理想轨迹数据，SINSout1为加陀螺加表误差的导航解算数据%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all;
close all;
clc;
deg_rad=pi/180;   %由度转化成弧度
rad_deg=180/pi;   %由弧度转化成度
%-----------------------常值定义区----------------------
Omiga=0.00007272205216643040;
Wiee=[0;0;Omiga];
f=1.0/298.257;
Re=6378245;
g0=9.78;
%-------------------------------从源文件中读入数据----------------------------------
fid_read=fopen（‘text11.txt‘‘r‘）;                  %无人机1由轨迹发生器产生的理想数据
fid_write=fopen（‘SINSout1.txt‘‘w‘）;

[AllData NumofAllData]=fscanf（fid_read‘%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n‘[20 inf]）;
AllData=AllData‘; 
NumofEachData=round（NumofAllData/20）;
Time=AllData（:1）;
Time（NumofEachData+1）=Time（NumofEachData）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      12004  2011-03-17 15:30  SINSNavDBT11.m

----------- ---------  ---------- -----  ----

                12004                    1