導(dǎo)航與制導(dǎo)實(shí)驗(yàn)報(bào)告INS與GPS位置組合導(dǎo)航

1、導(dǎo)航與制導(dǎo)原理實(shí)驗(yàn)報(bào)告實(shí)驗(yàn)要求1. 完成INS與GPS位置組合導(dǎo)航的仿真；2. 畫(huà)出組合導(dǎo)航后的位置誤差、速度誤差曲線(xiàn);3. 畫(huà)出原始軌跡與組合導(dǎo)航后的軌跡比較圖；（畫(huà)圖時(shí)弧度制單位要轉(zhuǎn)換成度分秒制單位）4. 結(jié)果分析5. 提交紙版實(shí)驗(yàn)報(bào)告（附上代碼）全局變量%地球半徑（長(zhǎng)半軸）R=6378160;f=1/298.3;wie=7.2921151467e-5;g0=9.7803267714;deg二n/180;min=deg/60;sec=min/60;hur=3600;dph=deg/hur;ts=0.1;%地球扁率%地球自轉(zhuǎn)角速率%重力加速度基礎(chǔ)值%角度%角分%角秒%小時(shí)%度/時(shí)%仿真采樣時(shí)

2、間GPS_Sample_Rate=10;%GPS采樣時(shí)間Runs=10;%由于隨機(jī)誤差，使用Kalman濾波時(shí)，應(yīng)多次濾波，以求平均值Tg=3600;%陀螺儀Markov過(guò)程相關(guān)時(shí)間Ta=1800;%加速度計(jì)Markov過(guò)程相關(guān)時(shí)間四.KalmanFilter:估計(jì)狀態(tài)初始值：Xk=zeros(18,1);估計(jì)協(xié)方差初始值：Pk=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,l.e3,l.e3,l.e3.'2);%18*18矩陣系統(tǒng)噪聲方差：Qk=l

3、e-6*diag(0.01,0.01,0.01,0.01,0.01,0.01,0.9780,0.9780,0.9780).'2量測(cè)噪聲方差：Rk二diag(le-5,le-5,10.3986)."2系數(shù)矩陣F，G，H的表示，參考課件6.2.1。五可能用到的公式(1)四元數(shù)Q的即時(shí)修正(符號(hào)表示四元數(shù)乘法)rol°一唸y唸彳術(shù)1一b1717znbbbnbxnbybnbzT為向量擴(kuò)展四元數(shù)，標(biāo)量部分為0。(2)四元數(shù)Q的歸一化(3)姿態(tài)矩陣Cn的計(jì)算b能5於-鞘仍=鞏社込+%殆)-2(社啊一如磁)2如犧+q°q2(匪令-Qo<7i)能一蘇一於卜誡一sin

4、sin6siny+cos單cosycos"sinGsinysin申cosy.cos9sinysincos&sin屮sin6cosy+cos單sinycosj/cos9cossin9cosysimpsinysin&cos9cosy.(4) 提取姿態(tài)角(5) 比力的坐標(biāo)變換(6)速度計(jì)算i?n=f71+gn+6)爲(wèi))xvn其中n=00pF(7)地球速率及地理坐標(biāo)系相對(duì)地球坐標(biāo)系的轉(zhuǎn)動(dòng)角速率0切屆cosLa)iesinLRe+hVjJ1tanL/?E+h_機(jī)體系相對(duì)于地理系的轉(zhuǎn)動(dòng)角速率在機(jī)體系中的投影(8)位置計(jì)算Rn=J?(l-2/+3/stnzL)Rb=/f(l+/sm

5、2L)（10）重力加速度1十0.0019318S138639stn2Lg(L)=9.7803267714x,Vl-0.006694379990l3smL(11)連續(xù)系統(tǒng)離散化公式(簡(jiǎn)化形式)Fk=1+F(tk)屯T玨二(1十F(tk)*T)*G(tk)*THk=H(tk)其中，I是單位矩陣，T是仿真采樣時(shí)間。六數(shù)據(jù)文件說(shuō)明dataWbibN.txtdataFbibN.txtdataPos.txtdataVn.txtatt0=0;0;0.3491dataGPSposN.txt%疊加噪聲的陀螺儀角速度輸出%疊加噪聲的加速度計(jì)比力輸出%原始軌跡的位置數(shù)據(jù)（依次是緯度L，經(jīng)度九，高度h）%原始軌跡的速

6、度數(shù)據(jù)（依次是東速度、北速度、天速度）%姿態(tài)解算矩陣初始值（依次是俯仰角9，橫滾角丫，航向角龍）%疊加噪聲的GPS位置數(shù)據(jù)（即等間隔采樣原始軌跡的位置數(shù)據(jù)，采樣間隔是10,即第10、20,的數(shù)據(jù)，并疊加噪聲）七仿真流程圖整個(gè)仿真過(guò)程流程圖Y循環(huán)是否給束K足否足丄0旳位置、遠(yuǎn)度、姿態(tài)呎初值捷麻解篇,并只更新gpk細(xì)mpn濾波修止位登、速度數(shù)摭諫取用.光數(shù)抵誤濰計(jì)算.湎圏表示K=k+1幵妁結(jié)束在整體仿真流程圖中，Kalman濾波可以由子函數(shù)kalman_GPS_INS_correct.m進(jìn)行。捷聯(lián)解算可按照如下捷聯(lián)解算流程圖進(jìn)行。開(kāi)始速險(xiǎn)解卻循壞是否結(jié)束姿態(tài)何解篦姿態(tài)距即計(jì)算姿態(tài)四元數(shù)即時(shí)諄正井歸

7、一化讀入己存的E存軌跡相關(guān)數(shù)按E機(jī)逵態(tài)、速應(yīng).也置収初值加速度計(jì)、陀燼住采擇値輸入荻得前一時(shí)刻晏態(tài)四元數(shù)保怎計(jì)算，畫(huà)圖顯示比力的塑杯變喚姿態(tài)速率計(jì)詩(shī)加好1八實(shí)驗(yàn)思路根據(jù)上述兩個(gè)流程圖，要實(shí)現(xiàn)INS與GPS位置組合導(dǎo)航，需要在已給的INS導(dǎo)航代碼中，加入當(dāng)k為10的倍數(shù)時(shí)進(jìn)行Kalman濾波，修正位置、速度數(shù)據(jù)環(huán)節(jié)，則添加代碼如下(添加位置見(jiàn)完整程序)：%添加程序F,G,H=GetConSis(vtC(:,k),posC(:,k),q_1,fn(:,k),Tg,Ta);%利用GetConSis得至UF,G,H矩陣ifrem(k,10)=0%判斷k是否為10的倍數(shù)，若是則進(jìn)行kalman濾波，修正

8、。E_attE,_pos,E_vn,Xk,Pk=kalman_GPS_INS_correct(Xk,Qk,Pk,F,G,H,ts,Rk);%利用kalman_GPS_INS_correct進(jìn)行kalman濾波elseZk=posC(:,k)-p_gps(:,(k)/10);E_attE,_pos,E_vn,Xk,Pk=kalman_GPS_INS_correct(Xk,Qk,Pk,F,G,H,ts,Rk,Zk);endattC(:,k)=attC(:,k)-E_att;posC(:,k)=posC(:,k)-E_pos;vtC(:,k)=vtC(:,k)-E_vn;%修正位置，速度數(shù)據(jù)九完整代

9、碼及注釋clc;clear;closeall;%捷聯(lián)慣導(dǎo)更新仿真，四元數(shù)法，一階近似算法，不考慮圓錐補(bǔ)償和劃槳補(bǔ)償ts=0.1;%采樣時(shí)間Re=6378160;%地球長(zhǎng)半軸wie=7.2921151467e-5;%地球自轉(zhuǎn)角速度f(wàn)=1/298.3;%地球扁率g0=9.7803;%重力加速度基礎(chǔ)值deg=pi/180;%角度min=deg/60;%角分sec=min/60;%角秒hur=3600;%小時(shí)dph=deg/hur;%度/小時(shí)%讀取數(shù)據(jù)wbibS=dlmread('dataWbibN.txt');fbS=dlmread('dataFbibN.txt');

10、posS=dlmread('dataPos.txt');vtetS=dlmread('dataVn.txt');p_gps=dlmread('dataGPSposN.txt');%統(tǒng)計(jì)矩陣初始化mm,nn=size(posS);posSta=zeros(mm,nn);vtSta=posSta;attSta=posSta;posC(:,1)=posS(:,1);%位置向量初始值vtC(:,1)=vtetS(:,1);%速度向量初始值attC(:,1)=0;0;0.3491;%姿態(tài)解算矩陣初始值Qk=1e-6*diag(0.01,0.01,0.01,

11、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è)量噪聲方差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;%GPS采樣率太高效果也不好StaNum=10;%重復(fù)運(yùn)行次數(shù)，用來(lái)求取統(tǒng)計(jì)平均值forOutLoop=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

12、*dph,0.1*dph,0.1*dph,0.1*dph,1.e-3,1.e-3,1.e-3/2);%初始估計(jì)協(xié)方差矩陣Xk=zeros(18,1);%初始狀態(tài)%N=size(posS,2);%j=1;fork=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

13、*sj,si,ci*%Cnb矩陣q_1=sqrt(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)變換%添加程序F,G,H=GetConSis(vtC(:,k),posC(:,k),q_1,

14、fn(:,k),Tg,Ta);%利用GetConSis得至UF,G,H矩陣ifrem(k,10)=0%判斷k是否為10的倍數(shù)，若是則進(jìn)行kalman濾波，修正。E_attE,_pos,E_vn,Xk,Pk=kalman_GPS_INS_correct(Xk,Qk,Pk,F,G,H,ts,Rk);%利用kalman_GPS_INS_correct進(jìn)行kalman濾波elseZk=posC(:,k)-p_gps(:,(k)/10);E_attE,_pos,E_vn,Xk,Pk=kalman_GPS_INS_correct(Xk,Qk,Pk,F,G,H,ts,Rk,Zk);endattC(:,k)=

15、attC(:,k)-E_att;posC(:,k)=posC(:,k)-E_pos;vtC(:,k)=vtC(:,k)-E_vn;%修正位置，速度數(shù)據(jù)%捷聯(lián)慣導(dǎo)解算wnie=wie*0;cos(posC(1,k);sin(posC(1,k);%地球系相對(duì)慣性系的轉(zhuǎn)動(dòng)角速度在導(dǎo)航系(地理系)的投影%計(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)*t

16、an(posC(1,k)/(Rn+posC(3,k);%導(dǎo)航系相對(duì)地球系的轉(zhuǎn)動(dòng)角速度在導(dǎo)航系的投影gn=g=g0+0.051799*sin(posC(1,k)入20.94114e6*posC(3,k)%重力加速度0;0;-g;%導(dǎo)航坐標(biāo)系的重力加速度wbnbC(:,k)=wbibS(:,k)-M(wnie+wnen);%姿態(tài)角轉(zhuǎn)動(dòng)角速度計(jì)算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(

17、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);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;attC(:,k+1)=asin(T(3,2);atan(-T(3,1)/T(3,3);atan(T(1,2)/T(2,2);%橫滾角gamma修正ifT(3,3)<0ifat

18、tC(2,k+1)<0attC(2,k+1)=attC(2,k+1)+pi;elseattC(2,k+1)=attC(2,k+1)-pi;endend%航向角psi修正ifT(2,2)<0ifT(1,2)>0attC(3,k+1)=attC(3,k+1)+pi;elseattC(3,k+1)=attC(3,k+1)-pi;endendifabs(T(2,2)<1.0e-20ifT(1,2)>0attC(3,k+1)=pi/2.0;elseattC(3,k+1)=-pi/2.0;endend%速度更新vtC(:,k+1)=vtC(:,k)+ts*(fn(:,k)+g

19、n-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);%高度end%attSta=attSta+attC;vtSta=vtSta+vtC;posSta=posSta+posC;end%對(duì)統(tǒng)計(jì)矩陣取平均attC=1./StaNum*attSta;posC=1./StaNum*posSt

20、a;vtC=1./StaNum*vtSta;%解算值與仿真值的誤差，作圖%結(jié)算矩陣均為3x(N+1)，需做處理%N點(diǎn)數(shù)據(jù)，相鄰兩點(diǎn)采樣間隔為0.1s,做作圖橫軸修正fori=1:Ntime(i)=i*ts;Rm=Re*(1-2*f+3*f*(sin(attC(1,i)入2);Rn=Re*(1+f*(sin(attC(1,i)A2);Latitude_error(i)=(posC(1,i)-posS(1,i)*Rm;Longitude_error(i)=(posC(2,i)-posS(2,i)*Rn*cos(attC(1,i);endposCp=posC(:,1:N);figure;subplo

21、t(131);plot(time,Latitude_error);title('緯度誤差');xlabel('Time/s');ylabel('itdeltaL/m');gridon;subplot(132);plot(time,Longitude_error);title('經(jīng)度誤差');xlabel('Time/s');ylabel('itdeltaphi/m');gridon;subplot(133);plot(time,posCp(3,:)-posS(3,:);title('高度誤差

22、');xlabel('Time/s');ylabel('itdeltah/m');gridon;vtCp=vtC(:,1:N);figure;subplot(131);plot(time,vtCp(1,:)-vtetS(1,:);title('東速度誤差');xlabel('Time/s');ylabel('itdeltavelocityeast/(m/s)');gridon;subplot(132);plot(time,vtCp(2,:)-vtetS(2,:);title('北速度誤差');xlabel('Time/s');ylabel('itdeltavelocitynorth/(m/s)');gridon;subplot(133);plot(time,vtCp(3,:)-vtetS(3,:);title('天速度誤差');xlabel(