飞机惯性导航Matlab语言实现

%这是研究惯性导航的最好代码。记得自己添加测试数据

% 此为基于四元素法，角增量法的捷连惯导系统程序算法

% > 飞行器飞行过程中飞行高度不变

% > 航向角以逆时针为正

% > 以地理系为导航坐标系

% > 运行程序时需导入比力信息及陀螺议角速率信息

clc

clear

close all

Data = load('');

f_INS = Data(:,2:4);% 加载加表数据

.

wib_INS = Data(:,5:7);% 加载陀螺数据

L0 = size(Data,1);

Wie = ; %> 地球自传角速度

Re = 6378245; %> 地球椭球长半径

h = 30;% > 飞行高度

e = 1/;

%> 初始经纬度

Lamda(1) = *pi/180;% > 初始经度（弧度）

L(1) = *pi/180;% > 初始纬度（弧度）

%> 初始姿态角

Seita(1) = *pi/180; %> 俯仰角（弧度）

%

Gama(1) = *pi/180; %> 横滚角（弧度）

Ksai(1) = *pi/180;% > 航向角（弧度）

%> 初始速度

Vx(1) = ; %> x通道速度

Vy(1) = ;% > y通道速度

Vz(1) = ; %> z通道速度

%> 重力加速度计算参数

g0 = ;

gk1 = ;

gk2 = ;

Vx = zeros(1,L0);Vy = zeros(1,L0);Vz = zeros(1,L0);

…

Lamda = zeros(1,L0);L = zeros(1,L0);Seita = zeros(1,L0);Gama = zeros(1,L0);Ksai = zeros(1,L0);

%> 四元素初始值

e0 = cos*Ksai(1))*cos*Seita(1))*cos*Gama(1))-sin*Ksai(1))*sin*Seita(1))*sin*Gama(1)) ;

e1 = -cos*Ksai(1))*sin*Seita(1))*cos*Gama(1))+sin*Ksai(1))*cos*Seita(1))*sin*Gama(1)

);

e2 = -cos*Ksai(1))*cos*Seita(1))*sin*Gama(1))-sin*Ksai(1))*sin*Seita(1))*cos*Gama(1) );

e3 = cos*Ksai(1))*sin*Seita(1))*sin*Gama(1))+sin*Ksai(1))*cos*Seita(1))*cos*Gama(1)) ;

Ctb = [e0^2+e1^2-e2^2-e3^2 2*(e1*e2+e0*e3) 2*(e1*e3-e0*e2); %> 用四元素表示得姿态矩阵

2*(e1*e2-e0*e3) e0^2-e1^2+e2^2-e3^2 2*(e2*e3+e0*e1);

2*(e1*e3+e0*e2) 2*(e2*e3-e0*e1) e0^2-e1^2-e2^2+e3^2];

E = [e0 e1 e2 e3]';%> 四元素的四个元素值

for i = 1:L0

?

f_INSc = f_INS(i,:)';

wib_INSc = wib_INS(i,:)';

Ry(i) = Re*(1-2*e+3*e*(sin(L(i)))^2);% > 计算子午圈主曲率半径

Rx(i) = Re*(1+e*(sin(L(i)))^2); %> 计算卯酉圈主曲率半径

g = g0*(1+gk1*(sin(L(i)))^2)*(1-2*h/Re)/sqrt(1-gk2*(sin(L(i)))^2);% > 重力加速度计算

Cbt = Ctb';

f_t = Cbt*f_INSc;% > 将体轴系中的比例转化到地理系

Vx(i) = (f_t(1)+2*Wie*sin(L(i))*Vy(i)+Vx(i)*Vy(i)*tan(L(i))/Rx(i))/80+Vx(i);% > x通道速度计算

Vy(i) = (f_t(2)-2*Wie*sin(L(i))*Vx(i)-Vx(i)*Vx(i)*tan(L(i))/Rx(i))/80+Vy(i);% > y通道速度计算

Vz(i) = (f_t(3)+2*Wie*cos(L(i))*Vx(i)+Vx(i)*Vx(i)/Rx(i)+Vy(i)*Vy(i)/Ry(i)-g)/80+Vz(i);% > z通道速度计算

Lamda(i) = Vx(i)/cos(L(i))/Rx(i)/80+Lamda(i);% > 经度计算

~

if Lamda(i)>pi

Lamda(i) = Lamda(i)-2*pi;% >经度在-180度(西经)到180(东经)范围

end

L(i) = Vy(i)/Ry(i)/80+L(i); %> 纬度计算

if L(i)>(pi/2)

L(i) = pi-L(i);% >纬度小于90度(北纬)

end

Wetx_t(i) = -Vy(i)/Ry(i);Wety_t(i) = Vx(i)/Rx(i);Wetz_t(i) = Vx(i)*tan(L(i))/Rx(i);% > 在地理坐标系的位移角速率

Wet_t = [Wetx_t(i) Wety_t(i) Wetz_t(i)]'; %> 在地理坐标系的位移角速率

Wib_b = [wib_INSc(1) wib_INSc(2) wib_INSc(3)]';% > 陀螺仪测的角速率值