導(dǎo)航及制導(dǎo)原理實(shí)驗(yàn)2

1、導(dǎo)航與制導(dǎo)原理實(shí)驗(yàn)-INS與 GPS位置組合導(dǎo)航的仿真姓名：戴 怡 軒 班級(jí)：自動(dòng)化 12 學(xué)號(hào)： 2110504027 日期： 2014年 5月 1日導(dǎo)航與制導(dǎo)原理仿真實(shí)驗(yàn)INS 與 GPS位置組合導(dǎo)航的仿真一、要求：1、完成 INS與GPS位置組合導(dǎo)航的仿真 ;2、畫(huà)出組合導(dǎo)航后的位置誤差、速度誤差曲線3、畫(huà)出原始軌跡與組合導(dǎo)航后的軌跡比較圖； （畫(huà)圖時(shí)，弧度制單位要轉(zhuǎn)換成度分秒制單位）4、結(jié)果分析5、提交紙版實(shí)驗(yàn)報(bào)告（附上代碼）、全局變量：R=6378160;%地球半徑（長(zhǎng)半軸）f=1/298.3;%地球扁率wie=7.2921151467e-5; %地球自轉(zhuǎn)角速率g0=9.780326

2、7714;%重力加速度基礎(chǔ)值deg=/180;%角度min=deg/60;%角分sec=min/60;%角秒hur=3600;%小時(shí)dph=deg/hur;%度/時(shí)ts=0.1; %仿真采樣時(shí)間三、組合導(dǎo)航仿真變量：GPS_Sample_Rate=10; %GPS 采樣時(shí)間Runs=10;%Tg = 3600;%Ta = 1800;%由于隨機(jī)誤差，使用 Kalman濾波時(shí)，應(yīng)多次濾波， 以求平均值陀螺儀 Markov過(guò)程相關(guān)時(shí)間加速度計(jì) Markov過(guò)程相關(guān)時(shí)間四、Kalman Filter:1、估計(jì)狀態(tài)初始值：Xk = zeros(18,1);2、估計(jì)協(xié)方差初始值：Pk=diag(min,m

3、in,min,0.5,0.5,0.5,30/Re,30/Re,30, 0.1*dph,0.1*dph, 0.1*dph, 0.1*dph, 0.1*dph, 0.1*dph,1.e-3,1.e-3,1.e-3.2); %18*18 矩陣3、系統(tǒng)噪聲方差：Qk=1e-6*diag(0.01,0.01,0.01,0.01,0.01,0.01,0.9780,0.9780,0.9 780).24、量測(cè)噪聲方差：Rk=diag(1e-5,1e-5,10.3986).25、系數(shù)矩陣 F， G，H的表示，參考課件 。五、實(shí)驗(yàn)用到的數(shù)據(jù)文件：dataWbibN.txt疊加噪聲的陀螺儀角速度輸出dataFbib

4、N.txt疊加噪聲的加速度計(jì)比力輸出dataPos.txt原始軌跡的位置數(shù)據(jù)依次是緯度 ? 經(jīng)度 ? 高度h）dataVn.txt原始軌跡的速度數(shù)據(jù)依次是東速度、 北速度、天速度）att0=0;0;0.3491 %姿態(tài)解算矩陣初始值依次是俯仰角 ? 橫滾角 ? 航向角）dataGPSposN.txt %疊加噪聲的 GPS位置數(shù)據(jù)（即等間隔采樣原始軌跡的位置數(shù)據(jù)，采樣間隔是 10，即第 10、20, 的數(shù)據(jù)，并疊加噪聲）六、仿真粗略流程圖：1、在仿真流程圖中， Kalman 濾波可以由實(shí)驗(yàn)指導(dǎo)老師提供的子函數(shù)進(jìn)行。 捷聯(lián) 解算可按照下一步驟中的捷聯(lián)解算流程圖進(jìn)行。2、實(shí)驗(yàn)指導(dǎo)老師提供的子函數(shù)有：

5、GetConSis.mglvs.mkalman_GPS_INS_correct.mqmul.m七、捷聯(lián)解算流程圖：結(jié)束八、實(shí)驗(yàn)程序代碼如下：本實(shí)驗(yàn)程序中所有變量的命名參照了在機(jī)房實(shí)驗(yàn)時(shí)實(shí)驗(yàn)老師演示的程序以 及在本實(shí)驗(yàn)報(bào)告開(kāi)頭所涉及的全局變量名。 最終實(shí)驗(yàn)采用老師要求的方向余弦法完成ts=0.1;% 采樣時(shí)間Re=6378160;%地球長(zhǎng)半軸 wie=7.2921151467e-5;% 地球自轉(zhuǎn)角速率 f=1/298.3;% 地球扁率 g0=9.7803;%重力加速度基礎(chǔ)值 deg=pi/180;% 角度 min=deg/60;%角分sec=min/60;%角秒 hur=3600;%小時(shí) dph

6、=deg/hur;%度/ 小時(shí)%讀取數(shù)據(jù)wbibS=dlmread('dataWbibN.txt');% 疊加噪聲的陀螺儀角速度輸出fbS=dlmread('dataFbibN.txt');% posS=dlmread('dataPos.txt');% vtetS=dlmread('dataVn.txt');%疊加噪聲的加速度比例輸出原始軌跡的位置數(shù)據(jù)原始軌跡的速度數(shù)據(jù)p_gps=dlmread('dataGPSposN.txt');% 疊加噪聲的 GPS位置數(shù)據(jù) %統(tǒng)計(jì)矩陣初始化mm,nn=size(posS);

7、 posSta=zeros(mm,nn);% 位置解算結(jié)果統(tǒng)計(jì) vtSta=posSta;% 速度解算結(jié)果統(tǒng)計(jì) attSta=posSta;% 姿態(tài)解算結(jié)果統(tǒng)計(jì)posC(:,1)=posS(:,1); % 位置向量初始值 vtC(:,1)=vtetS(:,1); % 速度向量初始值 attC(:,1)= 0;0; 0.3491; % 姿態(tài)解算矩陣初始值%噪%聲%處%理%GPSQk=1e-6*diag(0.01,0.01,0.01,0.01,0.01,0.01,0.9780,0.9780,0.9780).2 ;%系統(tǒng)噪聲方差矩陣Rk=diag(1e-5,1e-5,10.3986).2; % 測(cè)量

8、噪聲方差Tg = 3600*ones(3,1);陀螺儀 Markov過(guò)程相關(guān)時(shí)間Ta = 1800*ones(3,1); %加速度計(jì) Markov過(guò)程相關(guān)時(shí)間GPS_Sample_Rate=10; %GP采S 樣率太高效果也不好 %StaNum=10;%重復(fù)運(yùn)行次數(shù)，用于求取統(tǒng)計(jì)平均值for OutLoop=1:StaNumPk = diag(min,min,min, 0.5,0.5,0.5, 30./Re,30./Re,30, 0.1*dph,0.1*dph,0.1*dph, 0.1*dph,0.1*dph,0.1*dph, 1.e-3,1.e-3,1.e-3.2); % 初始估計(jì)協(xié)方差矩陣

9、Xk = zeros(18,1); %初始狀態(tài)%N=size(posS,2);% 獲取原始位置數(shù)據(jù)中的數(shù)據(jù)組數(shù)% j=1;for k=1:N-1si=sin(attC(1,k);ci=cos(attC(1,k);sj=sin(attC(2,k);cj=cos(attC(2,k); sk=sin(attC(3,k);ck=cos(attC(3,k);%k 時(shí)刻姿態(tài)矩陣M=cj*ck+si*sj*sk, ci*sk, sj*ck-si*cj*sk;-cj*sk+si*sj*ck, ci*ck, -sj*sk-si*cj*ck;-ci*sj, si, ci*cj; %即 Cnb矩陣q_1= sqrt

10、(abs(1.0+M(1,1)+M(2,2)+M(3,3)/2.0;sign(M(3,2)-M(2,3)*sqrt(abs(1.0+M(1,1)-M(2,2)-M(3,3)/2.0;sign(M(1,3)-M(3,1)*sqrt(abs(1.0-M(1,1)+M(2,2)-M(3,3)/2.0; sign(M(2,1)-M(1,2)*sqrt(abs(1.0-M(1,1)-M(2,2)+M(3,3)/2.0;fn(:,k)=M*fbS(:,k);% 比力的坐標(biāo)變換% 捷聯(lián)慣導(dǎo)解算wnie=wie*0;cos(posC(1,k);sin(posC(1,k);% 地球系相對(duì)慣性系 的轉(zhuǎn)動(dòng)角速度在導(dǎo)

11、航系(地理系)的投影% 計(jì)算主曲率半徑 Rm=Re*(1-2*f+3*f*sin(posC(1,k)2)+posC(3,k); Rn=Re*(1+f*sin(posC(1,k)2)+posC(3,k);wnen=-vtC(2,k)/(Rm+posC(3,k);vtC(1,k)/(Rn+posC(3,k);vtC(1,k)*tan(p osC(1,k)/(Rn+posC(3,k);% 導(dǎo)航系相對(duì)相對(duì)地球系的轉(zhuǎn)動(dòng)角速度在導(dǎo)航系的 投影g=g0+0.051799*sin(posC(1,k)2-0.94114e-6*posC(3,k);% 重力加速 度，由位置確定gn=0;0;-g;% 導(dǎo)航坐標(biāo)系的重

12、力加速度wbnbC(:,k)=wbibS(:,k)-M(wnie+wnen);%姿態(tài)角轉(zhuǎn)動(dòng)角速率計(jì)算， 需測(cè)得角速度%四%元%數(shù)法% q=1.0/2*qmul(q_1,0;wbnbC(:,k)*ts+q_1; % 姿態(tài)四元數(shù)更新% q=q/sqrt(q'*q);% 四元數(shù)歸一化% % 姿態(tài)角更新% q11=q(1)*q(1);q12=q(1)*q(2);q13=q(1)*q(3);q14=q(1)*q(4);% q22=q(2)*q(2);q23=q(2)*q(3);q24=q(2)*q(4);% q33=q(3)*q(3);q34=q(3)*q(4);% q44=q(4)*q(4);

13、% T=q11+q22-q33-q44, 2*(q23-q14), 2*(q24+q13);%2*(q23+q14), q11-q22+q33-q44, 2*(q34-q12);%2*(q24-q13), 2*(q34+q12), q11-q22-q33+q44;%四%元%數(shù)法%方%向%余弦法WbnbC=0,-wbnbC(3,k),wbnbC(2,k);wbnbC(3,k),0,-wbnbC(1,k);-wbnbC(2,k), wbnbC(1,k),0;T=M*WbnbC*ts+M;%方%向%余弦法% 姿態(tài)更新attC(:,k+1)=asin(T(3,2);atan(-T(3,1)/T(3,3

14、);atan(T(1,2)/T(2,2);% 橫滾角 gamm修a 正if T(3,3)<0if attC(2,k+1)<0attC(2,k+1)=attC(2,k+1)+pi;elseattC(2,k+1)=attC(2,k+1)-pi;endend% 航向角 psi 修正if T(2,2)<0if T(1,2)>0 attC(3,k+1)=attC(3,k+1)+pi;else attC(3,k+1)=attC(3,k+1)-pi;endendif abs(T(2,2)<1.0e-20if T(1,2)>0 attC(3,k+1)=pi/2.0;else

15、 attC(3,k+1)=-pi/2.0;endend% 速度更新vtC(:,k+1)=vtC(:,k)+ts*(fn(:,k)+gn-cross(2*wnie+wnen),vtC(:,k);% 位置更新posC(1,k+1)=posC(1,k)+ts*vtC(2,k)/(Rm+posC(3,k); % 緯 度posC(2,k+1)=posC(2,k)+ts*vtC(1,k)/(Rn+posC(3,k)*cos(posC(1,k);% 經(jīng) 度posC(3,k+1)=posC(3,k)+ts*vtC(3,k); % 高度F,G,H=GetConSis( vtC(:,k), posC(:,k),

16、T, fn(:,k), Tg, Ta );% 得到FGH矩陣if(mod(k+1,10)=0) %posC(:,k)=p_gps(:,(k/10);% 卡爾滿濾波% 求解 ZK Zk=p_gps(:,(k+1)/10)-posC(:,k+1); E_att, E_pos, E_vn, Xk, Pk = kalman_GPS_INS_correct( Xk, Qk, Pk, F, G, H, ts ,Zk,Rk );% 卡爾滿濾波posC(:,k+1) = posC(:,k+1)+E_pos;%位置修正vtC(:,k+1) = vtC(:,k+1)+E_vn;% 速度修正else E_att,

17、E_pos, E_vn, Xk, Pk = kalman_GPS_INS_correct(Xk, Qk, Pk, F, G, H,ts );end %GPS 修正慣性導(dǎo)航end% attSta=attSta+attC; vtSta=vtSta+vtC; posSta=posSta+posC;end%對(duì)統(tǒng)計(jì)矩陣取平均 attC=1./StaNum*attSta; posC=1./StaNum*posSta; vtC=1./StaNum*vtSta;%解算值與仿真值的誤差 作圖%解算矩陣均為 3x(N+1) ，需做處理%N點(diǎn)數(shù)據(jù)，相鄰兩點(diǎn)采樣間隔為 0.1s, 做作圖橫軸修正 for i=1:Nt

18、ime(i)=i*ts;Rm = Re*(1-2*f+3*f*(sin(attC(1,i)2);Rn = Re*(1+f*(sin(attC(1,i)2);Latitude_error(i)=(posC(1,i)-posS(1,i)*Rm;Longitude_error(i)=(posC(2,i)-posS(2,i)*Rn*cos(attC(1,i); end %作經(jīng)緯度和高度誤差圖posCp=posC(:,1:N);緯度誤差 ');xlabel('Time經(jīng)度誤差高度誤差東速度誤差figure; subplot(131);plot(time,Latitude_error);t

19、itle(' /s');ylabel('itdeltaL /m');grid on;subplot(132);plot(time,Longitude_error);title(' ');xlabel('Time /s');ylabel('itdeltaphi /m');grid on; subplot(133);plot(time,posCp(3,:)-posS(3,:);title(' ');xlabel('Time /s');ylabel('itdeltah /m'

20、;);grid on;%作東北天速度誤差圖 vtCp=vtC(:,1:N); figure; subplot(131);plot(time,vtCp(1,:)-vtetS(1,:);title('');xlabel('Time /s');ylabel('itdeltavelocity east /(m/s)');grid on; subplot(132);plot(time,vtCp(2,:)-vtetS(2,:);title('北速度誤差');xlabel('Time /s');ylabel('itdeltavelocity north /(m/s)');grid on; subplot(133);plot(time,vtCp(3,:)-vtetS(3,:);title('天速度誤差');xlabel('Time /s');ylabel('itdeltavelocity up /(m/s)');grid on;%三維飛