?? jielian_guandao_chengxu.m
字號:
% 此為基于四元素法，角增量法的捷連慣導系統程序算法
% 飛行器飛行過程中飛行高度不變
% 航向角以逆時針為正
% 以地理系為導航坐標系
% 運行程序時需導入比力信息及陀螺議角速率信息
clear clc
Wie=7.292115147e-5; % 地球自傳角速度
Re=6378245; % 地球橢球長半徑
h=30; % 飛行高度
e=1/298.3;
% 初始經緯度
Lamda(1)=116.344695283*pi/180; % 初始經度（弧度）
L(1)=39.975172*pi/180; % 初始緯度（弧度）
% 初始姿態角
Seita(1)=0.120992605*pi/180; % 俯仰角（弧度）
Gama(1)=0.010445947*pi/180; % 橫滾角（弧度）
Ksai(1)=91.637207*pi/180; % 航向角（弧度）
% 初始速度
Vx(1)=0.000048637; % x通道速度
Vy(1)=0.000206947; % y通道速度
Vz(1)=0.007106781; % z通道速度
% 重力加速度計算參數
g0=9.7803267714;
gk1=0.00193185138639;
gk2=0.00669437999013;
Vx=zeros(1,48001);Vy=zeros(1,48001);Vz=zeros(1,48001);
Lamda=zeros(1,48001);L=zeros(1,48001);Seita=zeros(1,48001);Gama=zeros(1,48001);Ksai=zeros(1,48001);
% 四元素初始值
e0=cos(0.5*Ksai(1))*cos(0.5*Seita(1))*cos(0.5*Gama(1))-sin(0.5*Ksai(1))*sin(0.5*Seita(1))*sin(0.5*Gama(1));
e1=-cos(0.5*Ksai(1))*sin(0.5*Seita(1))*cos(0.5*Gama(1))+sin(0.5*Ksai(1))*cos(0.5*Seita(1))*sin(0.5*Gama(1));
e2=-cos(0.5*Ksai(1))*cos(0.5*Seita(1))*sin(0.5*Gama(1))-sin(0.5*Ksai(1))*sin(0.5*Seita(1))*cos(0.5*Gama(1));
e3=cos(0.5*Ksai(1))*sin(0.5*Seita(1))*sin(0.5*Gama(1))+sin(0.5*Ksai(1))*cos(0.5*Seita(1))*cos(0.5*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:48000
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+1)=(f_t(1,i)+2*Wie*sin(L(i))*Vy(i)+Vx(i)*Vy(i)*tan(L(i))/Rx(i))/80+Vx(i); % x通道速度計算
Vy(i+1)=(f_t(2,i)-2*Wie*sin(L(i))*Vx(i)-Vx(i)*Vx(i)*tan(L(i))/Rx(i))/80+Vy(i); % y通道速度計算
Vz(i+1)=(f_t(3,i)+2*Wie*cos(L(i))*Vx(i)+Vx(i)*Vx(i)/Rx(i)+Vy(i)*Vy(i)/Ry(i)-g)/80+Vz(i);
Lamda(i+1)=Vx(i)/cos(L(i))/Rx(i)/80+Lamda(i); % 經度計算
if Lamda(i+1)>pi
Lamda(i+1)=Lamda(i+1)-2*pi; %經度在-180度(西經)到180(東經)范圍
end
L(i+1)=Vy(i)/Ry(i)/80+L(i); % 緯度計算
if L(i+1)>(pi/2)
L(i+1)=pi-L(i+1); %緯度小于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,i) wib_INSc(2,i) wib_INSc(3,i)]'; % 陀螺儀測的角速率值
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(0.5*derta)*eye(4)+Mwtb*sin(0.5*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+1)=asin(Ctb(2,3)); % 俯仰角計算
Gama(i+1)=atan(-Ctb(1,3)/Ctb(3,3)); % 橫滾角計算
if abs(Ctb(3,3))>eps
Gama(i+1)=atan(-Ctb(1,3)/Ctb(3,3));
if Ctb(3,3)>0
Gama(i+1)=Gama(i+1);
elseif -Ctb(1,3)> 0
Gama(i+1)=Gama(i+1)+pi;
else Gama(i+1)=Gama(i+1)-pi;
end
elseif -Ctb(1,3)> 0
Gama(i+1)=pi/2;
else Gama(i+1)=-pi/2;
end
Ksai(i+1)=atan(Ctb(2,1)/Ctb(2,2)); % 航向角計算
if abs(Ctb(2,2))>eps
Ksai(i+1)=atan(Ctb(2,1)/Ctb(2,2));
if Ctb(2,2)>0
Ksai(i+1)=Ksai(i+1);
elseif Ctb(2,1)> 0
Ksai(i+1)=Ksai(i+1)+pi;
else Ksai(i+1)=Ksai(i+1)-pi;
end
elseif Ctb(2,1)>0
Ksai(i+1)=pi/2;
else Ksai(i+1)=-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=0:1/80:600;
% 繪制曲線圖
figure(1);
plot(t,Lamda) % 繪制經度變化曲線圖
grid on
Xlabel('時間/秒');Ylabel('經度Lamda/度');title('經度變化曲線圖');
figure(2);
plot(t,L) % 繪制緯度變化曲線圖
grid on
Xlabel('時間/秒');Ylabel('緯度L/度');title('緯度變化曲線圖');
figure(3);
plot(Lamda,L) % 繪制經-緯度變化曲線圖
grid on
Xlabel('經度Lamda/度');Ylabel('緯度L/度');title('經—緯度坐標曲線圖');
figure(4);
plot(t,Seita) % 繪制俯仰角變化曲線圖
grid on
Xlabel('時間/秒');Ylabel('俯仰角Seita/度');title('俯仰角變化曲線圖');
figure(5);
plot(t,Gama) % 繪制橫滾角變化曲線圖
grid on
Xlabel('時間/秒');Ylabel('橫滾角Gama/度');title('橫滾角變化曲線圖');
figure(6);
plot(t,Ksai) % 繪制航向角變化曲線
grid on
Xlabel('時間/秒');Ylabel('航向角Ksai/度');title('航向角變化曲線圖');
figure(7);
plot(t,Vx) % 繪制東向速度變化曲線
grid on
Xlabel('時間/秒');Ylabel('東向速度Vx 米/秒');title('東向速度變化曲線圖');
figure(8);
plot(t,Vy) % 繪制北向速度變化曲線
grid on
Xlabel('時間/秒');Ylabel('北向速度Vy 米/秒');title('北向速度變化曲線圖');
figure(9);
plot(t,Vz) % 繪制垂直速度變化曲線
grid on
Xlabel('時間/秒');Ylabel('垂直速度Vz 米/秒');title('垂直速度變化曲線圖');
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -