飛機慣性導(dǎo)航Matlab語言實現(xiàn)

1、%這是研究慣性導(dǎo)航的最好代碼。記得自己添加測試數(shù)據(jù)%此為基于四元素法，角增量法的捷連慣導(dǎo)系統(tǒng)程序算法%飛行器飛行過程中飛行高度不變%航向角以逆時針為正%以地理系為導(dǎo)航坐標系%運行程序時需導(dǎo)入比力信息及陀螺議角速率信息clcclearclose allData = load(Datal.txt);f_INS = Data(:,2:4);% 加載加表數(shù)據(jù)wib_INS = Data(:,5:7);% 加載陀螺數(shù)據(jù)L0 = size(Data,1);Wie = 7.292115147e-5; %地球自傳角速度Re = 6378245; %地球橢球長半徑h = 30;% 飛行高度e = 1/298.3

2、;%初始經(jīng)緯度Lamda(1) = 116.344695283*pi/180;% 初始經(jīng)度(弧度)L(1) = 39.975172*pi/180;% 初始緯度(弧度)%初始姿態(tài)角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 通道速度%重力加

3、速度計算參數(shù)g0 = 9.7803267714;gk1 = 0.00193185138639;gk2 = 0.00669437999013;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);%四元素初始值e0cos(0.5*Ksai(1)*cos(0.5*Seita(1)*cos(i0.5*Gama(1)-sin(0.5*Ksai(1)*sin(0.5*Se

4、ita(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、.5*Seita(1)*cos(0.5* Gama(1);Ctb = e0A2+e1A2-e2A2-e3A2 2*(e1*e2+e0*e3) 2*(e1*e3-e0*e2); % 用四元素表示得姿態(tài)矩陣 2*(e1*e2-e0*e3) e0A2-e1A2+e2A2-e3A2 2*(e2*e3+e0*e1); 2*(e1*e3+e0*e2) 2*(e2*e3-e0*e1) e0A2-e1A2-e2A2+e3A2;E = e0 e1 e2 e3;%四元素的四個元素值for i = 1:L0f_INSc = f_INS(i,:);wib_INSc = wib_INS(i,:);Ry(i) = Re*

6、(1-2*e+3*e*(sin(L(i)A2);% 計算子午圈主曲率半徑Rx(i) = Re*(1+e*(sin(L(i)A2); % 計算卯酉圈主曲率半徑 g = g0*(1+gk1*(sin(L(i)A2)*(1-2*h/Re)/sqrt(1-gk2*(sin(L(i)A2);% 重力加速度計算 Cbt = Ctb;f_t = Cbt*f_INSc;% 將體軸系中的比例轉(zhuǎn)化到地理系 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

7、*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);% 經(jīng)度計算 if Lamda(i)piLamda(i) = Lamda(i)-2*pi;% 經(jīng)度在-180 度(西經(jīng))到 180(東經(jīng))范圍endHi) = Vy(i)/Ry(i)/80+L(

8、i); % 緯度計算 if L(i)(pi/2)L(i) = pi-L(i);% 緯度小于90度(北緯)endWetx_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); %

9、在地理坐標系的地球角速率Wtb_b = Wib_b-Ctb*(Wie_t+Wet_t); % 姿態(tài)矩陣角速率%用角增量法計算四元素姿態(tài)矩陣Mwtb = 0 -Wtb_b(1) -Wtb_b -Wtb_b (3);Wtb_b(1) 0 Wtb_b (3) -Wtb_b ；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)A2+(Mwtb(1,3)A2+(Mwtb(1,4)A2);E =eye(4)*(1-dertaA2/8+dertaA4/384)+(1/2-dertaA2/

10、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 = e0A2+e1A2-e2A2-e3A2 2*(e1*e2+e0*e3) 2*(e1*e3-e0*e2);% 用四元素表示得姿態(tài) 矩陣2*(e1*e2-e0*e3) e0A2-e1A2+e2A2-e3A2 2*(e2*e3+e0*e1);2*(e1*e3+e0*e2) 2*(e2*e3-e0*e1) e0A2-e1A2-e2A2+e3A2;%姿態(tài)角計算S

11、eita(i) = asin(Ctb(2,3); % 俯仰角計算Gama(i) = atan(-Ctb(1,3)/Ctb(3,3); % 橫滾角計算if abs(Ctb(3,3)epsGama(i) = atan(-Ctb(1,3)/Ctb(3,3);if Ctb(3,3)0Gama(i) = Gama(i);elseif -Ctb(1,3) 0Gama(i) = Gama(i)+pi;else Gama(i) = Gama(i)-pi;endelseif -Ctb(1,3) 0Gama(i) = pi/2;else Gama(i) = -pi/2;endKsai(i) = atan(Ctb(

12、2,1)/Ctb(2,2);% 航向角計算if abs(Ctb(2,2)epsKsa i(i) = atan(Ctb(2,1)/Ctb(2,2);if Ctb(2,2)0Ksai(i) = Ksai(i);elseif Ctb(2,1) 0Ksai(i) = Ksai(i)+pi;else Ksai(i) = Ksai(i)-pi;endelseif Ctb(2,1)0Ksai(i) = pi/2;else Ksai(i) = -pi/2;endend%將弧度換算為角度Seita = Seita*180/pi;Gama = Gama*180/pi;Ksai = Ksai*180/pi;L =

13、L*180/pi;Lamda = Lamda*180/pi;t = 0.01:0.01:L0*0.01;%繪制曲線圖figureplot(L,Lamda)% 繪制經(jīng)度變化曲線圖grid onXlabel(經(jīng)度);Ylabel(維度);title(經(jīng)維度變化曲線圖);figureplot(t,Seita)% 繪制俯仰角變化曲線圖grid onXlabel(時間/秒);Ylabel(俯仰角Seita/度);title(俯仰角變化曲線圖); figureplot（t,Gama）% 繪制橫滾角變化曲線圖grid onXlabel（時間/秒）;Ylabel（橫滾角Gama/度）;title（橫滾角變化曲線圖）;figureplot（t,Ksai） %繪制航向角變化曲線grid onXlabel（時間/秒）;Ylabel（航向角Ksai/度）;title（航向角變化曲線圖）; figureplot（t,Vx）% 繪制東向速度變化曲線grid onXlabe