捷联惯导的解算程序

%====本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)======

clear all;

close all;

clc;

deg_rad=pi/180; %由度转化成弧度

rad_deg=180/pi; %由弧度转化成度

%-------------------------------从源文件中读入数据

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

fid_read=fopen('IMUout.txt','r'); %path1_Den.dat 是由轨迹发生器产生的数据

[AllData

NumofAllData]=fscanf(fid_read,'%g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g',[17 inf]);

AllData=AllData';

NumofEachData=round(NumofAllData/17);

Time=AllData(:,1);

longitude=AllData(:,2); %经度单位：弧度

latitude=AllData(:,3); %纬度单位：弧度

High=AllData(:,4); %高度单位：米

Ve=-AllData(:,6); % 东向、北向、天向速度单位：米/妙

Vn=AllData(:,5);

Vu=AllData(:,7);

fb_x=AllData(:,9); %比力（fx，fy，fz）

fb_y=AllData(:,8); %指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系单位：米/秒2

fb_z=-AllData(:,10); %右前上

pitch=AllData(:,11); %俯仰角（向上为正）单位:弧度

head=-AllData(:,13); %偏航角（偏西为正）

roll=AllData(:,12); %滚转角（向右为正）

omigax=AllData(:,15); %陀螺输出（单位：弧度/秒，坐标轴的定义与比力的相同）

omigay=AllData(:,14);

omigaz=-AllData(:,16);

%-------------------------------程序初始化

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

latitude0=latitude(1);

longitude0=longitude(1); %初始位置

High0=High(1);

Ve0=Ve(1);

Vn0=Vn(1); %初始速度

Vu0=Vu(1);

pitch0=pitch(1);

head0=head(1); %初始姿态

roll0=roll(1);

TimeEach=0.005; %周期和仿真总时间

TimeAll=(NumofEachData-1)*TimeEach;

Omega_ie=0.7292115147E-4;%0.00007272205216643040; %地球自转角速度

单位：弧度每妙

g0=9.78;

%------------------------------导航解算开始

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

%假设没有初始对准误差

pitch_err0=pitch0+0*deg_rad;

head_err0=head0+0*deg_rad;

roll_err0=roll0+0*deg_rad;

%初始捷联矩阵的计算《捷联惯导系统》P63 旋转顺序 head - pitch - roll %导航坐标系n为东北天方向载体坐标系b为右前上偏航角北偏西为正

Tbn(1,1)=cos(roll_err0)*cos(head_err0)-sin(roll_err0)*sin(pitch_err0) *sin(head_err0);

Tbn(1,2)=cos(roll_err0)*sin(head_err0)+sin(roll_err0)*sin(pitch_err0) *cos(head_err0);

Tbn(1,3)=-sin(roll_err0)*cos(pitch_err0);

Tbn(2,1)=-cos(pitch_err0)*sin(head_err0);

Tbn(2,2)=cos(pitch_err0)*cos(head_err0);

Tbn(2,3)=sin(pitch_err0);

Tbn(3,1)=sin(roll_err0)*cos(head_err0)+cos(roll_err0)*sin(pitch_err0) *sin(head_err0);

Tbn(3,2)=sin(roll_err0)*sin(head_err0)-cos(roll_err0)*sin(pitch_err0) *cos(head_err0);

Tbn(3,3)=cos(roll_err0)*cos(pitch_err0);

Tnb=Tbn';

%位置矩阵的初始化《捷联惯导系统》P46 其中游动方位角 a＝0 假使初始经纬度确知

Cne(1,1) = - sin(longitude0);

Cne(1,2) = cos(longitude0);

Cne(1,3) = 0;

Cne(2,1) = - sin(latitude0) * cos(longitude0);