84.1節捷聯慣性導航更新的逆過程。y假設運載體為固定翼飛機，在飛行過程中攻角和側滑角始終為零，即飛機僅存在沿機體坐標系（by 0

vb

其中，ay為所需設置的縱軸向加速度，當ay0時為向前加速運動，當ay0時為運動，而當ayyy

0

vb

yy

0

vb

為設置的俯仰角速率，通常俯仰角運動是在0yy0

vb

向心加速度ac由空氣升力fLg的合力提供，這時橫滾角需滿足如下協調轉彎條件：ytanacy

g8.1-1y顯然，以上四種基本飛行動作，每種只在，和vby記歐拉角向量A T、歐拉角速率向量ω

TA AA A ppvCTy

vnC

A(t)

ppv

vb(t) 0T

p(t) h

y

6）Avnp，完成飛行軌跡設計。做攻角和側滑角變換即可（vn和p保持不變4.1Cn(m)C

C

C

M

)，Cb(m1)

in(m1/

1

vn(m)vn(m1)

sf

T

I

Cn(m1)

Δvb(m1)+Δvb(m1)

sf

in(m1/2)

Δvb(m1)1

Δvb(m1)

1

Δθ

8.1.1節飛行軌跡設計后，軌跡的姿態、速度和位置參數均為已知量，因此Cn(mCn(m)

ωnωin(m1/

sf

ie(m1/

I

b

Δθ008c，整理得

IT

Cn(m1)

1

+1

Δθsf in(m1/2)

b(m1) m

m

IT

Cn(m1)I11

1 Δθ

in(m1/2)

b(m1)

2

m

m

Δv

I

11

IT

1

Δθ

)1 2 m n(m1) 2in(m1/2) sf )1 可令初值Δv00。至此，求得角增量Δθm和速度增量Δvm，實現慣性傳感器輸出信息的反演仿真。速度vbFIR為了仿真效果更加真，可在歐拉角向量A和速度vn上添加適當的隨機振動，比如馬爾可夫AAωCAvbabpwpvC由于振動描述一般是以機體坐標系（b系）為參考的，式（8.1-12）中ωw和aw分別為b系中的隨機振動CAB中式（B-16。在微分方程組式（8.1-6）A和vn的數值求解精度要求不需太高，然而，8.1.2中A和vnp可采用梯形法。仿真程序采用的M語言編寫，這里僅給出與飛行軌跡和慣性傳感器數據生成直接相關的子程序，以及一個示例主程序，的基本子函數參見8.2節。function[att,vn,pos]=trjprofile(att0,vn0,pos0,wat, %trajectory len= att=zeros(len,3);vn=att;pos= att(1,:)=att0';vn(1,:)=vn0';pos(1,:)= vba2mat(att0)'*vn0;vby % bfir1(20,0.01, bb/sum(bxrepmat([att0;vby]',length(b),1);% for watk= for att0=att0+ vby=vby+ x[x(2:end[att0;vby]'y % att(kk+1,:)= vn(kk+1,:)= vn01= eth= pos(kk+1,:)=pos(kk,:)+ kk= att(kk:end,:)=[];vn(kk:end,:)=[];pos(kk:end,:)=function[wm,vm]=av2imu(att,vn,pos, wm0=zeros(3,1);vm0=wm0;I33= wm=att(2:end,:);vm= for eth=earth((pos(k-1,:)+pos(k,:))'/2,(vn(k- qbb=qmul(qmul(qconj(a2qua(att(k- phim= wm1= dvnsf=vn(k,:)'-vn(k-1,:)'- Cnb0=a2mat(att(k- vm1= wm(k-1,:)= vm(k-1,:)= wm0=wm1;vm0= %ts=att0=[0;0;90]*arcdeg;vn0=[0;0;0];pos0=%wat=4411 wat(:,1:3)= %-[att,vn,pos]=trjprofile(att0,vn0,pos0,wat,ts);[wm,vm]=av2imu(att,vn,pos,ts);tt=(0:length(att)-%msplot(221,tt,att/arcdeg,'Att/(\circ)');legend('\it\theta', ',msplot(222,tt,vn,'Vel/m.s^{-1}');legend('\itv\rm_E','\itv\rm_N','\itv\rm_U')msplot(223,tt,deltapos(pos),'\DeltaPos/m'); legend('\Delta\itL','\Delta\it\lambda',msplot(224,pos(:,2)/arcdeg,pos(:,1)/arcdeg,'\itL\rm/(\circ)','\it\lambda\rm/( holdon,plot(pos(1,2)/arcdeg,pos(1,1)/arcdeg, msplot(121,tt(2:end),wm/ts/arcdeg,'\it\omega^b{ib}\rm/(\circ.s^{-1} legend('\it\omega^b_{ibx}','\it\omega^b_{iby}','\it\omega^b_{ibz}');msplot(122,tt(2:end),vm/ts,'\itf^b\rm{sf}/(m.s^{-2})'); legend('\itf^b\rm_{sf\itx}','\itf^b\rm_{sf\ity}', globalGMReffwiegegpg0ugarcdegarcminarcsechurdphdpshugpsHzlsc%GlobalGM=GM= e14;Re=6.378136998405e6;wie= e- %WGS-84ff= ;ee=sqrt(2*ff-ff^2);e2=ee^2;Rp=(1-ge=9.780325333434361;gp=9.832184935381024;g0=ge;ug=g0*1e-6; %gravity,ugarcdeg=pi/180;arcmin=arcdeg/60;arcsec=arcmin/60; %angleunithur=3600;dph=arcdeg/hur;dpsh=arcdeg/sqrt(hur);%hour,deg/hour,deg/sqrt(hour)ugpsHz=ug/sqrt(1);%ug/sqrt(Hz)lsc=['-k';'-b';'-r';'-m';'--g';':c'];%lineshape&functionm=askew(v)m=m=[ functionCnb=a2mat(att) s=sin(att);c= si=s(1);sj=s(2);sk= ci=c(1);cj=c(2);ck= Cnb=[cj*ck- att含三個分量，依次為俯仰角、橫滾角和方位角，特別注意：程序中定義方位角北偏西為正（而非北偏東為正，取值范圍為(ππ。functionatt=m2att(Cnb) if att=[asin(Cnb(3,2));-atan2(Cnb(3,1),Cnb(3,3));-atan2(Cnb(1,2),Cnb(2,2)) att=[ 0 functionqnb=a2qua(att) s=sin(att/2);c= si=s(1);sj=s(2);sk=s(3);ci=c(1);cj=c(2);ck= qnb=[ci*cj*ck- si*cj*ck- ci*sj*ck+ ci*cj*sk+si*sj*ck %qnb=functionatt= att=functionqnb=m2qua(Cnb)C11C11=Cnb(1,1);C12=Cnb(1,2);C13=C21=Cnb(2,1);C22=Cnb(2,2);C23=C31=Cnb(3,1);C32=Cnb(3,2);C33= if q1=0.5*sqrt(1+C11-C22- q0=(C32-C23)/(4*q1);q2=(C12+C21)/(4*q1);q3= elseif q2=0.5*sqrt(1-C11+C22- q0=(C13-C31)/(4*q2);q1=(C12+C21)/(4*q2);q3= elseif q3=0.5*sqrt(1-C11- q0=(C21-C12)/(4*q3);q1=(C13+C31)/(4*q3);q2= q0= q1=(C32-C23)/(4*q0);q2=(C13-C31)/(4*q0);q3=(C21- qnb=[q0;q1;q2;functionCnb=q2mat(qnb) q11=qnb(1)*qnb(1);q12=qnb(1)*qnb(2);q13=qnb(1)*qnb(3);q14=q22q22=qnb(2)*qnb(2);q23=qnb(2)*qnb(3);q24= q33=qnb(3)*qnb(3);q34= q44= Cnb=[q11+q22-q33- q11-q22+q33- 2*(q24- q11-q22-q33+q44functionm=rv2m(rv) nm2= % ifnm2<1.e- % a=1-nm2*(1/6-nm2/120);b=0.5-nm2*(1/24- nm= a= b=(1- VX= m=eye(3)+a*VX+functionq=rv2q(rv) nm2= % ifnm2<1.0e- % q0=1-nm2*(1/8-nm2/384);s=1/2-nm2*(1/48- nm= q0=cos(nm/2);s= q=[q0;functionrv= if q=- nmhalf % ifnmhalf>1e- b= b= rv=首先，將四元數轉化為標量非負的四元數；其次，根據公式Q

cosusin

sin(2)，先由四元數的標量關系

cos求旋轉矢量模值的一半arccos(q22 /

量關系

sin(/2)求等效旋轉矢 v2

/

2sin(/2)functionqout=qconj(qin) qout=[qin(1);-functionqnb= nm= ifnm<1e- qnb[1;0;0; %表示姿態的四元數，其模值應約為 qnb= functionq=qmul(q1,q2) q=[q1(1)*q2(1)-q1(2)*q2(2)-q1(3)*q2(3)-q1(4)*q1(1)*q2(2)+q1(2)*q1(1)*q2(2)+q1(2)*q2(1)+q1(3)*q2(4)-q1(4)*q1(1)*q2(3)+q1(3)*q2(1)+q1(4)*q2(2)-q1(2)* q1(1)*q2(4)+q1(4)*q2(1)+q1(2)*q2(3)-q1(3)*q2(2)functionvo=qmulv(q,vi) qi= qo= vo= %vo=functionfunctionqpb=qaddphi(qnb,phi)qpb=qmul(rv2q(-等價的矩陣表示為式（4.2-5，即CnCnCn，改成四元數形式為QnQnQn。從真實導航系（ n 系）到計算導航系（n系）的失準角為，反之，從n系到n系的失準角應為，若將轉矢量，則與其對應的變換四元數為Qnqpb，qnb和phi分別代表QnQn和 functionqnb=qdelphi(qpb,phi)qnbfunctionqnb=qdelphi(qpb,phi)qnb=qmul(rv2q(phi),確的）

QQ

bbfunctionphi=qq2phi(qpb, qerr=qmul(qnb, phi=

QnQn(Qn*求得誤差四元數Qn，再由Qn求解失準角 24function[phim,dvbm]=cnscl(wm,vm) 1375,0 0 cs= [2315,4558 7834, 2-6 wmm= vmm= dphim= scullm= nsize(wm % if csw=cs(n-1,1:n-1)*wm(1:n-1,:);csv=cs(n-1,1:n-1)*vm(1:n- dphim= % scullm % phim= dvbm=地球導航參數計算（3.13.2節functioneth=earth(pos,vn)globalReffwieg0 eesqrt(2*ff- e2 eth.sl= eth.cl= eth.tl= eth.sl2= sl4= sq=1- sq2= eth.RMh=Re*(1- eth.RNh= eth.clRNh= eth.wnie=wie*[0;eth.cl; eth.vn= eth.wnen=[-vn(2)/eth.RMh;vn(1)/eth.RNh; eth.wnin=eth.wnie+ eth.wnien=eth.wnie+ gLhg0*(1+5.27094e-3*eth.sl2+2.32718e-5*sl4)-3.086e-6*pos(3);grs80 eth.gn=[0;0;- eth.gcceth.gncross(eth.wnien,vn考慮重力/哥氏力/function[wm,vm]=imuadderr(wm,vm,eb,web,db,wdb, m=size(wm,1);sts= wm=wm+[ts*eb(1)+ ts*eb(2)+ ts*eb(3)+sts*web(3)*randn(m,1)vm=vm +vm=vm +[ts*db(1)+sts*wdb(1)*randn(m,1),ts*db(2)+ ts*db(3)+sts*wdb(3)*randn(m,1)速度隨機誤差wdb。捷聯慣導更新算法（4.1節function[qnb,vn,pos,eth]=insupdate(qnb,vn,pos,wm,vm, nn= nts= [phim,dvbm]cnscl(wm %圓錐誤差/ ethearth(pos, % vn1vnrv2m(-eth.wnin*nts/2)*qmulv(qnb,dvbm % vn= pospos vn % qnbqmul(rv2q(-eth.wnin*nts),qmul(qnb, % qnb=functiondpos= cl=cos(pos(:,1));Re functionmsplot(mnp,x,y,xstr, ifmod(mnp,10)==1,figure; %如果是第一幅小圖，則新建一個 subplot(mnp);plot(x, grid ifnargin==4,ystrxstr;xstrts' %如果只輸入一個字符串，則默認xlabel xlabel(xstr);nn2;ts0.1;nts %att1130]*arcdegvn[0;0;0];pos[34*arcdeg;108*arcdeg100];qnb=a2qua(att); %姿態、速度和位置初始化eth=earth(pos,wm=qmulv(qconj(qnb),eth.wnie)*ts;vm=qmulv(qconj(qnb),-eth.gn)*ts;wmrepmat(wm'nn,1vmrepmat(vmnn,1);%仿真靜態IMU數據phi=[0.1;0.2;3]*arcmin;qnb=qaddphi(qnb,phi);lenfix(3600/ts);%avp=zeros(len,10); kk=1; t0;%[att,vn,post]fork=1:nn:len t=t+ [qnb,vn,pos]=insupdate(qnb,vn,pos,wm,vm, %vn(3)= avp(kk,:)=[q2att(qnb);vn;pos; kk= ifmod(t,100)<nts, %顯示進度avp(kk:end,:)=[];tt=msplot(221,tt,avp(:,1:2)/arcdeg,'Att/(\circ)');legend('\it\theta','\it\g msplot(222,tt,avp(:,3)/arcdeg,'\psi/\circ');msplot(223,tt,avp(:,4:6),'Vel/m.s^{-1}');legend('\itv\rm_E','\itv\rm_N','\itv\rm_U')msplot(224,tt,deltapos(avp(:,7:9)),'\DeltaPos/m'); legend('\Delta\itL','\Delta\it\lambda', 8.2KalmanSINS誤差轉移矩陣（4.27.3節functionFt=kfft15(eth,Cnb,fb)globalg0 tl=eth.tl;secl= f_RMh=1/eth.RMh;f_RNh=1/eth.RNh;f_clRNh= f_RMh2= f_RNh2= vEclRNh=eth.vn(1)*fclRNh;vE_RNh2= vNRMh2=eth.vn(2)*f Mp1=[ 0, 0, 0,0 Mp2=[ 0,- 0,- Maa=askew(- Mav=[ -f_RMh, 0 Map= Mva= Mvv=askew(eth.vn)*Mav- Mvp= scl= Mvp(3,1)=Mvp(3,1)-g0*(5.27094e-3*2*scl+2.32718e- Mvp(3,3)=Mvp(3,3)+3.086e- Mpv=[ f_RMh, f_clRNh, 1 Mpp=[ 0,- 0,- 0 O33= Ft=[ zeros(6,15)Kalmanfunctionkf=kfinit(Qk,Rk,P0,Phikk_1,Hk, [kf.m,kf.n]= kf.Qk=Qk;kf.Rk=Rk;kf.Pk=P0;kf.Xk= kf.Phikk1=Phikk1;kf.Hk= if k= k= Kalman濾波更新（5.3節functionkf=kfupdate(kf,Zk, if TimeMeasBoth= elseif TimeMeasBoth= ifTimeMeasBoth=='T|| % kf.Xkk_1= kf.Pkk_1=kf.Phikk_1*kf.Pk*kf.Phikk_1'+ k else% kf.Xkk1= kf.Pkk_1= ifTimeMeasBoth=='M|| % kf.PXZkk_1= kf.PZkk1=kf.Hk*kf.PXZkk1+ kf.Kk=kf.PXZkk1/kf.PZkk kf.Xk=kf.Xkk1+kf.Kk*(Zk- kf.Pk=kf.Pkk_1- else% kf.Xk= kf.Pk= kf.Pk Pnn2;ts0.1ntsnn*ts;子樣數和采樣時間att0=[0;0;30]*arcdeg;qnb0=a2qua(att0);vn0=[0;0;0];pos0=[34*arcdeg;108*arcdeg;qnbqnb0;vnvn0;pospos0;姿態、速度和位置初始化eth=earth(pos,vn);wm=qmulv(qconj(qnb),eth.wnie)*ts;vm=qmulv(qconj(qnb),-eth.gn)*ts;wm=repmat(wm',nn,1);vm=repmat(vm',nn,1); %·仿真靜態IMU數據phi=[0.1;0.2;3]*arcmin;qnb=qaddphi(qnb,phi); eb=[0.01;0.015;0.02]*dph;web=[0.001;0.001;0.001]*dpsh; db=[80;90;100]*ug;wdb=[1;1;1]*ugpsHz; Qk=diag([web;wdb;rk=[[0.1;0.1;0.1];[[10;10]/Re;10]];Rk=P0=diag([[0.1;0.1;10]*arcdeg;[1;1;1]; [0.1;0.1;0.1]*dph;Hk=kf=kfinit(Qk,Rk,P0,zeros(15),Hk);%kf濾波器初始化len=fix(3600/ts); %·仿真時長errzeros(len,10);xkpkzeros(len2*kfn+1);kk1;t0;%記錄導航結果fork=1:nn:len t=t+ [wm1,vm1]=imuadderr(wm,vm,eb,web,db,wdb, [qnb,vn,pos,eth]=insupdate(qnb,vn,pos,wm1,vm1, kf.Phikk1=eye(15)+kfft15(eth,q2mat(qnb), kf= if gpsvn0;pos0] GPS kf=kfupdate(kf,[vn;pos]-gps, qnbqdelphi(qnb,kf.Xk(1:3));kf.Xk(1:3) · vn=vn-kf.Xk(4:6);kf.Xk(4:6)= pos=pos-kf.Xk(7:9);kf.Xk(7:9)= err(kk,:)=[qq2phi(qnb,qnb0);vn-vn0;pos-pos0; xkpk(kk,:)=[kf.Xk;diag(kf.Pk);t];kk= ifmod(t,500)<nts,disp(fix(t)); end%顯示進度err(kk:end,:)=[];xkpk(kk:end,:)=[];t