%这是研究惯性导航的最好代码。记得自己添加测试数据
% 此为基于四元素法，角增量法的捷连惯导系统程序算法
% > 飞行器飞行过程中飞行高度不变
% > 航向角以逆时针为正
% > 以地理系为导航坐标系
% > 运行程序时需导入比力信息及陀螺议角速率信息
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)]';% > 陀螺仪测的角速率值
Wie_t = [0 Wie*cos(L(i)) Wie*sin(L(i))]'; %> 在地理坐标系的地球角速率
—
Wtb_b = Wib_b-Ctb*(Wie_t+Wet_t); %> 姿态矩阵角速率
%> 用角增量法计算四元素姿态矩阵
Mwtb = [0 -Wtb_b(1) -Wtb_b(2) -Wtb_b(3);
Wtb_b(1) 0 Wtb_b(3) -Wtb_b(2);
Wtb_b(2) -Wtb_b(3) 0 Wtb_b(1);
Wtb_b(3) Wtb_b(2) -Wtb_b(1) 0]/80;
derta = sqrt((Mwtb(1,2))^2+(Mwtb(1,3))^2+(Mwtb(1,4))^2);
E = [eye(4)*(1-derta^2/8+derta^4/384)+(1/2-derta^2/48)*Mwtb]*E;% > E = (cos*derta)*eye(4)+Mwtb*sin*derta)/derta)*E，采用四阶近似算法
e0 = E(1);e1 = E(2);e2 = E(3);e3 = E(4);
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];
%> 姿态角计算
Seita(i) = asin(Ctb(2,3)); %> 俯仰角计算
Gama(i) = atan(-Ctb(1,3)/Ctb(3,3)); %> 横滚角计算
if abs(Ctb(3,3))>eps
Gama(i) = atan(-Ctb(1,3)/Ctb(3,3));
if Ctb(3,3)>0
Gama(i) = Gama(i);
elseif -Ctb(1,3)> 0
Gama(i) = Gama(i)+pi;
else Gama(i) = Gama(i)-pi;
《
end
elseif -Ctb(1,3)> 0
Gama(i) = pi/2;
else Gama(i) = -pi/2;
end
Ksai(i) = atan(Ctb(2,1)/Ctb(2,2));% > 航向角计算
if abs(Ctb(2,2))>eps
Ksai(i) = atan(Ctb(2,1)/Ctb(2,2));
if Ctb(2,2)>0
Ksai(i) = Ksai(i);
elseif Ctb(2,1)> 0
—
Ksai(i) = Ksai(i)+pi;
else Ksai(i) = Ksai(i)-pi;
end
elseif Ctb(2,1)>0
Ksai(i) = pi/2;
else Ksai(i) = -pi/2;
end
end
%> 将弧度换算为角度
Seita = Seita*180/pi;Gama = Gama*180/pi;Ksai = Ksai*180/pi;
L = L*180/pi;Lamda = Lamda*180/pi;
{
t = ::L0*;
%> 绘制曲线图
figure
plot(L,Lamda)% > 绘制经度变化曲线图
grid on
Xlabel('经度');Ylabel('维度');title('经维度变化曲线图');
figure
plot(t,Seita)% > 绘制俯仰角变化曲线图
grid on
Xlabel('时间/秒');Ylabel('俯仰角Seita/度');title('俯仰角变化曲线图'); figure
*
plot(t,Gama)% > 绘制横滚角变化曲线图
grid on
Xlabel('时间/秒');Ylabel('横滚角Gama/度');title('横滚角变化曲线图'); figure
plot(t,Ksai) %> 绘制航向角变化曲线
grid on
Xlabel('时间/秒');Ylabel('航向角Ksai/度');title('航向角变化曲线图'); figure
plot(t,Vx)% > 绘制东向速度变化曲线
grid on
Xlabel('时间/秒');Ylabel('东向速度Vx 米/秒');title('东向速度变化曲线图'); figure
plot(t,Vy)% > 绘制北向速度变化曲线
grid on
Xlabel('时间/秒');Ylabel('北向速度Vy 米/秒');title('北向速度变化曲线图'); figure
plot(t,Vz)% > 绘制垂直速度变化曲线
grid on
Xlabel('时间/秒');Ylabel('垂直速度Vz 米/秒');title('垂直速度变化曲线图');