捷聯(lián)慣導(dǎo)的解算程序

1、%=本程序?yàn)榻萋?lián)慣導(dǎo)的解算程序(由慣性器件的輸出解算出飛行器的位置、速度、姿態(tài)信息)= clear all; close all; clc; deg_rad=pi/180; %由度轉(zhuǎn)化成弧度 rad_deg=180/pi; %由弧度轉(zhuǎn)化成度 %-從源文件中讀入數(shù)據(jù)- fid_read=fopen('IMUout.txt','r'); %path1_Den.dat 是由軌跡發(fā)生器產(chǎn)生的數(shù)據(jù) AllData NumofAllData=fscanf(fid_read,'%g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %

2、g',17 inf); AllData=AllData' NumofEachData=round(NumofAllData/17); Time=AllData(:,1); longitude=AllData(:,2); %經(jīng)度 單位：弧度 latitude=AllData(:,3); %緯度 單位：弧度 High=AllData(:,4); %高度 單位：米 Ve=-AllData(:,6); % 東向、北向、天向速度 單位：米/妙 Vn=AllData(:,5); Vu=AllData(:,7); fb_x=AllData(:,9); %比力（fx，fy，fz） fb_y=A

3、llData(:,8); %指向右機(jī)翼方向?yàn)閤正方向，指向機(jī)頭方向?yàn)閥正向，z軸與x軸和y軸構(gòu)成右手坐標(biāo)系 單位：米/秒2 fb_z=-AllData(:,10); %右前上 pitch=AllData(:,11); %俯仰角（向上為正） 單位:弧度 head=-AllData(:,13); %偏航角（偏西為正） roll=AllData(:,12); %滾轉(zhuǎn)角（向右為正） omigax=AllData(:,15); %陀螺輸出（單位：弧度/秒，坐標(biāo)軸的定義與比力的相同） omigay=AllData(:,14); omigaz=-AllData(:,16); %-程序初始化- latitud

4、e0=latitude(1); longitude0=longitude(1); %初始位置 High0=High(1); Ve0=Ve(1); Vn0=Vn(1); %初始速度 Vu0=Vu(1); pitch0=pitch(1); head0=head(1); %初始姿態(tài) roll0=roll(1); TimeEach=0.005; %周期 和仿真總時(shí)間 TimeAll=(NumofEachData-1)*TimeEach; Omega_ie=0.7292115147E-4;%0.00007272205216643040; %地球自轉(zhuǎn)角速度 單位：弧度每妙 g0=9.78; %-導(dǎo)航解算開(kāi)

5、始- %假設(shè)沒(méi)有初始對(duì)準(zhǔn)誤差 pitch_err0=pitch0+0*deg_rad; head_err0=head0+0*deg_rad; roll_err0=roll0+0*deg_rad; %初始捷聯(lián)矩陣的計(jì)算 捷聯(lián)慣導(dǎo)系統(tǒng)P63 旋轉(zhuǎn)順序 head - pitch - roll %導(dǎo)航坐標(biāo)系n為東北天方向 載體坐標(biāo)系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_err

6、0)+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(

7、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' %位置矩陣的初始化 捷聯(lián)慣導(dǎo)系統(tǒng)P46 其中游動(dòng)方位角 a0 假使初始經(jīng)緯度確知 Cne(1,1) = - sin(longitude0); Cne(1,2) = cos(longitude0); Cne(1,3) = 0; Cne(2,1) = - sin(latitude0) * cos(longitude0); Cne(2,2) = -

8、sin(latitude0) * sin(longitude0); Cne(2,3) = cos(latitude0); Cne(3,1) = cos(latitude0) * cos(longitude0); Cne(3,2) = cos(latitude0) * sin(longitude0); Cne(3,3) = sin(latitude0); Cen=Cne' %初始四元數(shù)的確定 捷聯(lián)慣導(dǎo)系統(tǒng) P151-152 方法本身保證了q12+q22+q32+q42=1 q(2,1) = sqrt(abs(1.0 + Tnb(1,1) - Tnb(2,2) - Tnb(3,3) / 2

9、.0; q(3,1) = sqrt(abs(1.0 - Tnb(1,1) + Tnb(2,2) - Tnb(3,3) / 2.0; q(4,1) = sqrt(abs(1.0 - Tnb(1,1) - Tnb(2,2) + Tnb(3,3) / 2.0; q(1,1) = sqrt(abs(1.0 - q(2,1) 2 - q(3,1) 2 - q(4,1) 2); % 判斷q(1,1)的符號(hào) flag_q11=cos(head_err0/2.0)*cos(pitch_err0/2.0)*cos(roll_err0/2.0)-sin(head_err0/2.0)*sin(pitch_err0/

10、2.0)*sin(roll_err0/2.0); if (flag_q11 >0) %此時(shí)q(1,1)取正 if (Tnb(3,2) < Tnb(2,3) q(2,1) = - q(2,1); end if (Tnb(1,3) < Tnb(3,1) q(3,1) = - q(3,1); end if (Tnb(2,1) < Tnb(1,2) q(4,1) = - q(4,1); end else %此時(shí)q(1,1)取負(fù) 或0 q(1,1) = - q(1,1); if (Tnb(3,2) > Tnb(2,3) q(2,1) = - q(2,1); end if (

11、Tnb(1,3) > Tnb(3,1) q(3,1) = - q(3,1); end if (Tnb(2,1) > Tnb(1,2) q(4,1) = - q(4,1); end end %-迭代推算用到的參數(shù)的初始化- Wiee_e = 0; Wiee_n = 0; Wiee_u = Omega_ie; Wiee = Wiee_e Wiee_n Wiee_u' %地球速率在地球系中的投影 東北天 Lat_err(1)=latitude0; Lon_err(1)=longitude0; High_err(1)=High0; Ve_err(1)=Ve0; Vn_err(1)=

12、Vn0; Vu_err(1)=Vu0; pitch_err(1)=pitch_err0; head_err(1)=head_err0; roll_err(1)=roll_err0; Re=6378137.0;%6378245.0; %地球長(zhǎng)軸 慣性導(dǎo)航系統(tǒng) P28 e=0.0033528106647474807198455286185206; %地球扁率精確值 ee=0.00669437999014131699614; %-迭代推算開(kāi)始- for i=1:NumofEachData %-慣性儀表數(shù)據(jù)的獲得- Wibb(1,1)=omigax(i); %指向右機(jī)翼方向?yàn)閤正方向，指向機(jī)頭方向?yàn)閥

13、正向，z軸與x軸和y軸構(gòu)成右手坐標(biāo)系 Wibb(2,1)=omigay(i); %單位：弧度/妙 Wibb(3,1)=omigaz(i); %右前上 fb(1,1)=fb_x(i); %指向右機(jī)翼方向?yàn)閤正方向，指向機(jī)頭方向?yàn)閥正向，z軸與x軸和y軸構(gòu)成右手坐標(biāo)系 fb(2,1)=fb_y(i); %單位：米/秒2 fb(3,1)=fb_z(i); %右前上 %-計(jì)算在姿態(tài)矩陣和位置矩陣更新時(shí)用到的參數(shù)- RM=Re*(1.0-2.0*e+3.0*e*Cne(3,3)2)+High_err(i); %捷聯(lián)慣導(dǎo)系統(tǒng) P233 P235 RN=Re*(1.0+e*Cne(3,3)2)+High_e

14、rr(i); % RN=Re*(1-ee)/(sqrt(1-ee*sin(Lat_err(i)3+High_err(i); % RM=Re/sqrt(1-ee*sin(Lat_err(i)+High_err(i); %實(shí)驗(yàn)當(dāng)?shù)刂亓铀俣扔?jì)算 捷聯(lián)慣導(dǎo)系統(tǒng)P150 慣性導(dǎo)航系統(tǒng) P35 g=g0*(1.0+0.0052884*Cne(3,3)2)-0.0000059*(1-(1-2*Cne(3,3)2)2)*(1.0-2.0*High_err(i)/Re); tmp_slat=sin(Lat_err(i)*sin(Lat_err(i); Wien = Cne * Wiee; %地球速率在導(dǎo)航系中

15、的投影 Wenn(1,1) = -Vn_err(i)/RM; Wenn(2,1) = Ve_err(i)/RN; % <<慣性導(dǎo)航系統(tǒng)>> P45 考慮了地球轉(zhuǎn)動(dòng)的影響. Wenn(3,1) = Ve_err(i)*tan(Lat_err(i)/RN; %計(jì)算Wenn(不太精確)，更新速度和位置矩陣時(shí)用 Winn=Wien+Wenn; Winb=Tbn*Winn; Wnbb=Wibb-Winb; %姿態(tài)速率 在姿態(tài)更新時(shí)用到 fn=Tnb*fb; % x-y-z 東北天 % 速度的更新 捷聯(lián)慣導(dǎo)系統(tǒng) P30 33 東北天 difVe_err=fn(1,1)+(2*Wie

16、n(3,1)+Wenn(3,1)*Vn_err(i)-(2*Wien(2,1)+Wenn(2,1)*Vu_err(i); difVn_err=fn(2,1)-(2*Wien(3,1)+Wenn(3,1)*Ve_err(i)+(2*Wien(1,1)+Wenn(1,1)*Vu_err(i); difVu_err=fn(3,1)+(2*Wien(2,1)+Wenn(2,1)*Ve_err(i)-(2*Wien(1,1)+Wenn(1,1)*Vn_err(i)-g; Ve_err(i+1)=Ve_err(i)+difVe_err*TimeEach; Vn_err(i+1)=Vn_err(i)+dif

17、Vn_err*TimeEach; Vu_err(i+1)=Vu_err(i)+difVu_err*TimeEach; High_err(i+1)=High_err(i)+Vu_err(i)*TimeEach; % 位置矩陣的實(shí)時(shí)更新 慣性導(dǎo)航系統(tǒng) P190 Cne(1,1)=Cne(1,1)+TimeEach*(Wenn(3,1)*Cne(2,1)-Wenn(2,1)*Cne(3,1); Cne(1,2)=Cne(1,2)+TimeEach*(Wenn(3,1)*Cne(2,2)-Wenn(2,1)*Cne(3,2); Cne(1,3)=Cne(1,3)+TimeEach*(Wenn(3,1)

18、*Cne(2,3)-Wenn(2,1)*Cne(3,3); Cne(2,1)=Cne(2,1)+TimeEach*(-Wenn(3,1)*Cne(1,1)+Wenn(1,1)*Cne(3,1); Cne(2,2)=Cne(2,2)+TimeEach*(-Wenn(3,1)*Cne(1,2)+Wenn(1,1)*Cne(3,2); Cne(2,3)=Cne(2,3)+TimeEach*(-Wenn(3,1)*Cne(1,3)+Wenn(1,1)*Cne(3,3); Cne(3,1)=Cne(3,1)+TimeEach*(Wenn(2,1)*Cne(1,1)-Wenn(1,1)*Cne(2,1);

19、 Cne(3,2)=Cne(3,2)+TimeEach*(Wenn(2,1)*Cne(1,2)-Wenn(1,1)*Cne(2,2); Cne(3,3)=Cne(3,3)+TimeEach*(Wenn(2,1)*Cne(1,3)-Wenn(1,1)*Cne(2,3); % Mat_Wenn(1,1)=0; % Mat_Wenn(1,2)=Wenn(3,1); % Mat_Wenn(1,3)=-Wenn(2,1); %Wenn的反對(duì)陣矩陣取負(fù) % Mat_Wenn(2,1)=-Wenn(3,1); %這里位置矩陣的及時(shí)修正為: dCne/dt=Mat_Wenn*Cne % Mat_Wenn(2,

20、2)=0; % Mat_Wenn(2,3)=Wenn(1,1); % Mat_Wenn(3,1)=Wenn(2,1); % Mat_Wenn(3,2)=-Wenn(1,1); % Mat_Wenn(3,3)=0; % % Mat_Wenn=Mat_Wenn*Cne*TimeEach; % Cne=Cne+Mat_Wenn; Cen=Cne' % 計(jì)算經(jīng)緯度 Lat_err(i+1)=asin(Cne(3,3); Lon_err(i+1)=atan(Cne(3,2)/Cne(3,1); %這是經(jīng)度的主值 if (Cne(3,1) < 0) if (Lon_err(i+1) >

21、 0) Lon_err(i+1) = Lon_err(i+1) - pi; else Lon_err(i+1) = Lon_err(i+1) + pi; end end % 四元數(shù)的及時(shí)修正 慣性導(dǎo)航系統(tǒng) P194 % Mat_Wnbb= 0, -Wnbb(1,1), -Wnbb(2,1), -Wnbb(3,1); % Wnbb(1,1), 0, Wnbb(3,1), -Wnbb(2,1); % Wnbb(2,1), -Wnbb(3,1), 0, Wnbb(1,1); % Wnbb(3,1), Wnbb(2,1), -Wnbb(1,1), 0; % q=q+Mat_Wnbb*q*TimeEac

22、h/2.0; q(1,1)=q(1,1)+TimeEach*(-Wnbb(1,1)*q(2,1)-Wnbb(2,1)*q(3,1)-Wnbb(3,1)*q(4,1)/2.0; q(2,1)=q(2,1)+TimeEach*(Wnbb(1,1)*q(1,1)+Wnbb(3,1)*q(3,1)-Wnbb(2,1)*q(4,1)/2.0; q(3,1)=q(3,1)+TimeEach*(Wnbb(2,1)*q(1,1)-Wnbb(3,1)*q(2,1)+Wnbb(1,1)*q(4,1)/2.0; q(4,1)=q(4,1)+TimeEach*(Wnbb(3,1)*q(1,1)+Wnbb(2,1)*q

23、(2,1)-Wnbb(1,1)*q(3,1)/2.0; % 四元數(shù)歸一化處理 q_norm=sqrt(sum(q.*q); q=q/q_norm; % 計(jì)算姿態(tài)矩陣 Tnb Tnb(1,1) = q(1,1) 2 + q(2,1) 2 - q(3,1)2 - q(4,1)2; Tnb(1,2) = 2.0 * (q(2,1) * q(3,1) - q(1,1) * q(4,1); Tnb(1,3) = 2.0 * (q(2,1) * q(4,1) + q(1,1) * q(3,1); Tnb(2,1) = 2.0 * (q(2,1) * q(3,1) + q(1,1) * q(4,1); Tn

24、b(2,2) = q(1,1)2 - q(2,1)2 + q(3,1)2 - q(4,1)2; Tnb(2,3) = 2.0 * (q(3,1) * q(4,1) - q(1,1) * q(2,1); Tnb(3,1) = 2.0 * (q(2,1) * q(4,1) - q(1,1) * q(3,1); Tnb(3,2) = 2.0 * (q(3,1) * q(4,1) + q(1,1) * q(2,1); Tnb(3,3) = q(1,1)2 - q(2,1)2 - q(3,1)2 + q(4,1)2; Tbn=Tnb' flag_pitch=asin(Tnb(3,2); flag

25、_roll=atan(-Tnb(3,1)/Tnb(3,3); flag_head=atan(-Tnb(1,2)/Tnb(2,2); if(Tnb(3,3)<0) if(flag_roll<0) flag_roll=flag_roll+pi; end if(flag_roll>0) flag_roll=flag_roll-pi; end end % 偏航角范圍 -180度180度 北偏西為正 if(Tnb(2,2)<0) if(flag_head<0) flag_head=flag_head+pi; end if(flag_head>0) flag_head=

26、flag_head-pi; end end % 姿態(tài)角更新 pitch_err(i+1)=flag_pitch; head_err(i+1)=flag_head; roll_err(i+1)=flag_roll; % 解算完畢 由對(duì)準(zhǔn)結(jié)果、陀螺、加表的輸出解算出載體的位置、速度、姿態(tài) %-計(jì)算解算誤差- ddLat(i)=(Lat_err(i)-latitude(i)*rad_deg; %緯度誤差 單位：度 ddLog(i)=(Lon_err(i)-longitude(i)*rad_deg; %經(jīng)度誤差 單位：度 ddHigh(i)=High_err(i)-High(i); %高度誤差 單位：米 ddVe(i)=Ve_err(i)-Ve(i); ddVn(i)=Vn_err(i)-Vn(i); % 速度誤差 單位：米/妙2 ddVu(i)=Vu_err(i)-Vu(i); ddpitch(i)=(pitch_err(i)-pitch