北航慣性導(dǎo)航綜合實(shí)驗(yàn)四實(shí)驗(yàn)報(bào)告

1、基于運(yùn)動規(guī)劃的慣性導(dǎo)航系統(tǒng)動態(tài)實(shí)驗(yàn)二零一三年六月十日實(shí)驗(yàn)4.1 慣性導(dǎo)航系統(tǒng)運(yùn)動軌跡規(guī)劃與設(shè)計(jì)實(shí)驗(yàn)一、實(shí)驗(yàn)?zāi)康臑檫M(jìn)行動態(tài)下簡化慣性導(dǎo)航算法的實(shí)驗(yàn)研究，進(jìn)行路徑和運(yùn)動狀態(tài)規(guī)劃，以驗(yàn)證不同運(yùn)動狀態(tài)下慣導(dǎo)系統(tǒng)的性能。通過實(shí)驗(yàn)掌握步進(jìn)電機(jī)控制方法，并產(chǎn)生不同運(yùn)動路徑和運(yùn)動狀態(tài)。二、實(shí)驗(yàn)內(nèi)容學(xué)習(xí)利用6045B 控制板對步進(jìn)電機(jī)進(jìn)行控制的方法，并控制電機(jī)使運(yùn)動滑軌產(chǎn)生定長運(yùn)動和不同加速度下的定長運(yùn)動。三、實(shí)驗(yàn)系統(tǒng)組成USB_PCL6045B 控制板（評估板）、運(yùn)動滑軌和控制計(jì)算機(jī)組成。四、實(shí)驗(yàn)原理 IMU安裝誤差系數(shù)的計(jì)算方法USB_PCL6045B 控制板采用了USB 串行總線接口通信方式，不必拆卸計(jì)算

2、機(jī)箱就可以在臺式機(jī)或筆記本電腦上進(jìn)行運(yùn)動控制芯片PCL6045B 的學(xué)習(xí)和評估。USB_PCL6045B 評估板采用USB 串行總線方式實(shí)現(xiàn)評估板同計(jì)算機(jī)的數(shù)據(jù)交換，由評估板的FIFO 控制回路完成步進(jìn)電機(jī)以及伺服電機(jī)的高速脈沖控制，任意2 軸的圓弧插補(bǔ)，2-4 軸的直線插補(bǔ)等運(yùn)動控制功能。USB_PCL6045B 評估板上配置了全部PCL6045B 芯片的外部信號接口和增量編碼器信號輸入接口。由USB_PCL6045B 評估測試軟件可以進(jìn)行PCL6045B 芯片的主要功能的評估測試。圖4-1-1USB_PCL6045B 評估板原理框圖如圖4-1-1 所示，CN11 接口主要用于外部電源連接，

3、可以選擇DC5V 單一電源或DC5V/24V 電源。CN12 接口是USB 信號接口，用于USB_PCL6045B 評估板同計(jì)算機(jī)的數(shù)據(jù)交換。USB_PCL6045B 評估板已經(jīng)完成對PCL6045B 芯片的底層程序開發(fā)和硬件資源與端口的驅(qū)動，并封裝成156 個(gè)API 接口函數(shù)。用戶可直接在VC 環(huán)境下利用API 接口函數(shù)進(jìn)行編程。五、實(shí)驗(yàn)內(nèi)容1、操作步驟1）檢查電機(jī)驅(qū)動電源（24V）2）檢查USB_PCL6045B 控制板與上位機(jī)及電機(jī)驅(qū)動器間的連接電纜3）啟動USB_PCL6045B 控制板評估測試系統(tǒng)檢查系統(tǒng)是否正常工作。4）運(yùn)行編寫的定長運(yùn)動程序，并比較實(shí)際位移與設(shè)定位移。5）修改程序

4、設(shè)定不同運(yùn)動長度，并重復(fù)執(zhí)行步驟4）。6）對記錄實(shí)驗(yàn)數(shù)據(jù)，并進(jìn)行誤差分析。2、實(shí)驗(yàn)數(shù)據(jù)處理基于VC的控制界面：本次實(shí)驗(yàn)必須先設(shè)計(jì)控制系統(tǒng)的上位機(jī)，通過上位機(jī)的串口向下位機(jī)發(fā)送控制命令，下位機(jī)接收到命令后，產(chǎn)生PWM波，控制電機(jī)的正反轉(zhuǎn)以此達(dá)到控制導(dǎo)軌運(yùn)動的目的。系統(tǒng)的控制界面如圖1所示： 圖1 系統(tǒng)的控制界面控制導(dǎo)軌運(yùn)動，運(yùn)動采取正向運(yùn)動，再返回，即IMU的實(shí)際運(yùn)行位移為零。并保存數(shù)據(jù)控制界面的應(yīng)用程序源程序僅寫出VC中按鈕的響應(yīng)程序：void CAaaDlg:Online() /定長運(yùn)動/ TODO: Add your control notification handler code he

5、reUSB_initial();USB_default_set();USB_set_org_logic(AXS_AX,0);/原點(diǎn)開關(guān)的邏輯,負(fù)邏輯USB_set_el_logic(AXS_AX,0);/硬極限輸入邏輯,低電平使能USB_set_sd_logic(AXS_AX,0);/減速開關(guān)的輸入邏輯,負(fù)邏輯USB_set_alm_logic(AXS_AX, 1);/報(bào)警輸入信號邏輯USB_set_inp_logic(AXS_AX,1);/in的輸入信號邏輯USB_ez_logic(AXS_AX,0);/Z相的輸入邏輯USB_set_pls_outmode(AXS_AX,1);USB_set

6、_out_enable(AXS_AX,1);/脈沖輸出使能/USB_jog_continue(AXS_AX,150,20000,20,20,20,20,1,30000);USB_start_tr_move(AXS_AX, m_dist, 0, m_inspeed, 5000, 5000);/USB_tv_move(AXS_AX, 150, 2000, 3000);/*USB_v_change(AXS_AX, 5000, 5000);while(1)USB_get_speed(AXS_AX, &m_speed);UpdateData(FALSE);MSG msg;while(PeekMe

7、ssage(&msg,0,0,100,PM_REMOVE)TranslateMessage(&msg);DispatchMessage(&msg);Sleep(100);*/void CAaaDlg:OnButton1() /停止運(yùn)動/ TODO: Add your control notification handler code hereUSB_sd_stop(AXS_AX);void CAaaDlg:OnGetSpeed() /獲得速度/ TODO: Add your control notification handler code hereUSB_get_sp

8、eed(AXS_AX, &m_speed);UpdateData(FALSE);void CAaaDlg:OnButton3() /OK 按鈕程序/ TODO: Add your control notification handler code hereUpdateData(true);3，處理數(shù)據(jù)由實(shí)驗(yàn)原理可知，慣性測量單元（IMU）可以通過自身獨(dú)立的測量結(jié)果進(jìn)行積分，計(jì)算出目標(biāo)運(yùn)動的角度和位移等量。本次實(shí)驗(yàn)就是利用IMU的加速度計(jì)的某一敏感軸測量導(dǎo)軌運(yùn)行的加速度，通過加速度兩次積分得到物體的位移，計(jì)算結(jié)果如圖2所示：實(shí)驗(yàn)經(jīng)過往返，從原理上講位移應(yīng)該為零。處理結(jié)果：位移曲線： 速度

9、曲線： 4，源程序：A=load('E:慣性器件綜合實(shí)驗(yàn)我的作業(yè)實(shí)驗(yàn)四X300000_V10000.txt');T=1/200; %單位為秒g=9.78;Ax=A(:,4)*g/1000; %提取加速度計(jì)的值 轉(zhuǎn)化為m/s2Ax=Ax*(1.0009)-0.0036595*g;vx=zeros(12657,1);sx=zeros(12657,1);u=zeros(12657,1);%計(jì)算位移for i=2:12657 vx(i)=vx(i-1)+(Ax(i-1)+Ax(i-1)*T /2; sx(i)=sx(i-1)+(vx(i-1)+ vx(i)/2*T+0.5*A(i-1)

10、*T*T; u(i)=i;endfigureplot(u/100,vx);xlabel('時(shí)間/秒'),ylabel('速度 米/秒');figureplot(u/100,sx);xlabel('時(shí)間/秒'),ylabel('位移 米');5，實(shí)驗(yàn)結(jié)果分析 從原理上講IMU做往返運(yùn)動，位移應(yīng)該出現(xiàn)增大和減小的趨勢，但是由于各種誤差角，而且滑軌也不能保證當(dāng)?shù)厮?，在?jì)算過程中，也未減去有害加速度。所以誤差很大。而且根據(jù)所采集的數(shù)據(jù)可知加速度計(jì)并沒有感知方向，在實(shí)驗(yàn)過程中應(yīng)該根據(jù)計(jì)算脈沖 與時(shí)間，自己計(jì)算方向時(shí)間慣性導(dǎo)航系統(tǒng)半物理仿真

11、實(shí)驗(yàn)一、實(shí)驗(yàn)?zāi)康倪M(jìn)行慣導(dǎo)系統(tǒng)半物理仿真實(shí)驗(yàn)，以驗(yàn)證慣性器件真實(shí)誤差特性情況下慣性導(dǎo)航系統(tǒng)的性能。二、實(shí)驗(yàn)內(nèi)容將采集到陀螺儀與加速度計(jì)的真實(shí)誤差數(shù)據(jù)疊加到軌跡發(fā)生器產(chǎn)生的導(dǎo)航參數(shù)真值上，進(jìn)行慣導(dǎo)解算，并分析誤差特性。三、實(shí)驗(yàn)系統(tǒng)組成真實(shí)的陀螺儀、加速度計(jì)或 IMU，數(shù)據(jù)采集系統(tǒng)和數(shù)據(jù)處理計(jì)算機(jī)。四、實(shí)驗(yàn)步驟（1）采集實(shí)驗(yàn)數(shù)據(jù)（2）處理采集的實(shí)驗(yàn)數(shù)據(jù)，生成半物理的慣性器件誤差數(shù)據(jù)（3）生成半物理的導(dǎo)航數(shù)據(jù)，進(jìn)行導(dǎo)航解算（4）對導(dǎo)航解算結(jié)果進(jìn)行分析（5）完成實(shí)驗(yàn)報(bào)告五、實(shí)驗(yàn)內(nèi)容及結(jié)果（1）半物理仿真數(shù)據(jù)的生成：a) 應(yīng)用前面IMU實(shí)驗(yàn)或慣性導(dǎo)航系統(tǒng)動態(tài)實(shí)驗(yàn)中采集的陀螺儀與加速度計(jì)的靜態(tài)數(shù)據(jù)DATAb

12、) 對以上采集的靜態(tài)數(shù)據(jù)求取均方差X（結(jié)果為X度/小時(shí)或Xg）c) 將DATA中數(shù)據(jù)去掉均值生成新的數(shù)據(jù)DATA1（器件噪聲）d) 自己設(shè)定要仿真的陀螺或加速度計(jì)的精度Y（度/小時(shí)或g）e) 將DATA1中數(shù)據(jù)乘以Y/X生成新的數(shù)據(jù)DATA2（半物理仿真噪聲）f) 從DATA2中讀取數(shù)據(jù)并疊加到軌跡發(fā)生器產(chǎn)生的標(biāo)準(zhǔn)數(shù)據(jù)(不含噪聲)上，進(jìn)行導(dǎo)航解算。（如初始采集的數(shù)據(jù)長度不夠，可以將DATA2中數(shù)據(jù)重復(fù)利用，即將生成一個(gè)幾倍長度于DATA2和數(shù)據(jù)文件DATA3，并從DATA3中讀取半物理數(shù)據(jù)并疊加到軌跡發(fā)生器產(chǎn)生的標(biāo)準(zhǔn)數(shù)據(jù)上）（2）加半物理仿真噪聲數(shù)據(jù)的導(dǎo)航結(jié)果：（3）疊加噪聲的導(dǎo)航結(jié)果：（4）

13、結(jié)果分析：由實(shí)驗(yàn)結(jié)果可見，疊加的仿真噪聲數(shù)據(jù)對姿態(tài)的解算影響很大；但由于所加噪聲較小，所以噪聲數(shù)據(jù)對位移和速度的解算影響不大。六，源程序clear,clcinvout=load('E:慣導(dǎo)實(shí)驗(yàn)數(shù)據(jù)第四次實(shí)驗(yàn)4.3第四部分半物理仿真數(shù)據(jù)生成方法及數(shù)據(jù)格式說明IMU數(shù)據(jù)invout.dat');CaijiShuju=load('E:慣導(dǎo)實(shí)驗(yàn)數(shù)據(jù)第四次實(shí)驗(yàn)4.3第四部分半物理仿真數(shù)據(jù)生成方法及數(shù)據(jù)格式說明IMU數(shù)據(jù)data2.txt');W=CaijiShuju(:,3:5);F=CaijiShuju(:,9:11);W_pingjun=mean(W);F_pingj

14、un=mean(F);%生成噪聲%wx=W(:,1)-W_pingjun(1,1);%器件噪聲wy=W(:,2)-W_pingjun(1,2);wz=W(:,3)-W_pingjun(1,3);fx=F(:,1)-F_pingjun(1,1);fx=F(:,2)-F_pingjun(1,2);fx=F(:,3)-F_pingjun(1,3);%求陀螺均方差%N=size(W);n=N(1,1);%陀螺的精度設(shè)為0.5度/小時(shí)%w_jingdu=0.5/3600*pi/180%0.5度/小時(shí)轉(zhuǎn)成弧度 sx=0;for i=1:n sx=sx+(W(i,1)-W_pingjun(1,1)2endw

15、x_junfangcha=sqrt(sx/n);%x陀螺的均方差 wx1=w_jingdu/wx_junfangcha; Wx=wx*wx1 %半物理仿真噪聲Wx sx=0;for i=1:n sx=sx+(W(i,2)-W_pingjun(1,2)2endwy_junfangcha=sqrt(sx/n);%y陀螺的均方差 wy2=w_jingdu/wy_junfangcha; Wy=wy*wy2 %半物理仿真噪聲Wy sx=0;for i=1:n sx=sx+(W(i,3)-W_pingjun(1,3)2endwz_junfangcha=sqrt(sx/n);%z陀螺的均方差 wz3=w_j

16、ingdu/wz_junfangcha; Wz=wz*wz3 %半物理仿真噪聲Wz%求加計(jì)的均方差% f_jingdu=1/1000*9.8%加計(jì)的精度為1mg sx=0;for i=1:n sx=sx+(F(i,1)-F_pingjun(1,1)2endfx_junfangcha=sqrt(sx/n);%x加計(jì)的均方差fx1= f_jingdu/fx_junfangcha;Fx=fx*fx1; sx=0;for i=1:n sx=sx+(F(i,2)-F_pingjun(1,2)2endfy_junfangcha=sqrt(sx/n);%y加計(jì)的均方差fx2= f_jingdu/fy_jun

17、fangcha;Fy=fx*fx2; sx=0;for i=1:n sx=sx+(F(i,3)-F_pingjun(1,3)2endfz_junfangcha=sqrt(sx/n);%z加計(jì)的均方差fx3= f_jingdu/fz_junfangcha;Fz=fx*fx3; %軌跡發(fā)生器數(shù)據(jù)疊加噪聲%Wx_invout=invout(:,5);Wy_invout=invout(:,6);Wz_invout=invout(:,7);Fx_invout=invout(:,2);Fy_invout=invout(:,3);Fz_invout=invout(:,4);L_invout=invout(:

18、,8); %緯度 Jingdu_invout=invout(:,9);%經(jīng)度Height_invout=invout(:,10);%高度%開始疊加%N1=size(invout); n1=N1(1,1);%應(yīng)為采樣點(diǎn)數(shù)大于軌跡發(fā)生器的個(gè)數(shù)，所以以軌跡發(fā)生器的個(gè)數(shù)為準(zhǔn)Wxx=Wx(1:n1,1)+Wx_invout; Wyy=Wy(1:n1,1)+Wy_invout ; Wzz=Wz(1:n1,1)+Wz_invout; Wibb=Wxx,Wyy,Wzz; Fxx=Fx(1:n1,1)+Fx_invout; Fyy=Fy(1:n1,1)+Fy_invout ; Fzz=Fz(1:n1,1)+Fz

19、_invout; Fibb=Fxx,Fyy,Fzz; q0=zeros(n1,1);q1=zeros(n1,1);q2=zeros(n1,1);q3=zeros(n1,1);Phai=zeros(n1,1);Thita=zeros(n1,1);Gama=zeros(n1,1);Phai(1)=0/180*pi;%偏航初始角Thita(1)=(0)*pi/180;%俯仰初始角Gama(1)=(0)*pi/180;%橫滾初始角 L=zeros(n1,1);nmda=zeros(n1,1);Vxt=zeros(n1+1,1);Vyt=zeros(n1+1,1); q0(1)=cos(-Phai(1)

20、/2)*cos(Thita(1)/2)*cos(Gama(1)/2)+sin(-Phai(1)/2)*sin(Thita(1)/2)*sin(Gama(1)/2);q1(1)=cos(-Phai(1)/2)*sin(Thita(1)/2)*cos(Gama(1)/2)+sin(-Phai(1)/2)*cos(Thita(1)/2)*sin(Gama(1)/2);q2(1)=cos(-Phai(1)/2)*cos(Thita(1)/2)*sin(Gama(1)/2)-sin(-Phai(1)/2)*sin(Thita(1)/2)*cos(Gama(1)/2);q3(1)=cos(-Phai(1)

21、/2)*sin(Thita(1)/2)*sin(Gama(1)/2)-sin(-Phai(1)/2)*cos(Thita(1)/2)*cos(Gama(1)/2);Wie=0.000072921151467;%已經(jīng)是弧度制L(1)=40/180*pi;nmda(1)=116.0/180*pi;T=0.01;%采樣頻率為100HzVxt(1)=0;Vyt(1)=0;Re=6378245+80;%加高度80米e=1/298.3;for k=1:n1c11=q0(k)2+q1(k)2-q2(k)2-q3(k)2;c12=2*(q1(k)*q2(k)+q0(k)*q3(k);c13=2*(q1(k)*

22、q3(k)-q0(k)*q2(k);c21=2*(q1(k)*q2(k)-q0(k)*q3(k);c22=q0(k)2-q1(k)2+q2(k)2-q3(k)2;c23=2*(q2(k)*q3(k)+q0(k)*q1(k);c31=2*(q1(k)*q3(k)+q0(k)*q2(k);c32=2*(q2(k)*q3(k)-q0(k)*q1(k);c33=q0(k)2-q1(k)2-q2(k)2+q3(k)2;Cnb=c11,c12,c13 c21,c22,c23 c31,c32,c33; if abs(c22)>0.0000000000001 Phai(k)=atan(-c21/c22)

23、; end if abs(c22)>0.0000000000001 & c21>0 Phai(k)=pi/2; end if abs(c22)>0.0000000000001 & c21<0 Phai(k)=-pi/2; end if abs(c22)>0.0000000000001 & c22>0 Phai(k)=atan(-c21/c22); endif abs(c22)>0.0000000000001 & c22>0 & c21>0 Phai(k)=atan(c21/c22)+pi;endif

24、abs(c22)>0.0000000000001 & c22>0 & c21<0 Phai(k)=atan(-c21/c22)-pi;end Thita(k)=asin(c23);Gama(k)=-atan(c13/c33); Cbn=inv(Cnb);Aibn=Cbn*Fibb(k,:)'Rxt=Re/(1-e*(sin(L(k)*sin(L(k); axt=Aibn(1,1)+2*Wie*sin(L(k)*Vyt(k)+Vyt(k)*Vxt(k)*tan(L(k)/Rxt;ayt=Aibn(2,1)-2*Wie*sin(L(k)*Vxt(k)-Vx

25、t(k)*Vxt(k)*tan(L(k)/Rxt; Vxt(k+1)=axt*T+Vxt(k);Vyt(k+1)=ayt*T+Vyt(k); Ryt=Re/(1+2*e-3*e*(sin(L(k)*sin(L(k); L(k+1)=0.5*T*(Vyt(k+1)+Vyt(k)/Ryt+L(k);nmda(k+1)=0.5*T*(Vxt(k+1)+Vxt(k)/Rxt*sec(L(k)+nmda(k);Wenn=-Vyt(k)/Ryt;Vxt(k)/Rxt;Vxt(k)/Rxt*tan(L(k);%課本86頁4.2-38式Winn=Wenn+0;Wie*cos(L(k);Wie*sin(L(k)

26、; Winb=Cnb*Winn;Wtbb=Wibb(k,:)'-Winb;dltaTita0_fang=(Wtbb(1,1)*T)2+(Wtbb(2,1)*T)2+(Wtbb(3,1)*T)2; dltaTita=0,-Wtbb(1,1)*T,-Wtbb(2,1)*T,-Wtbb(3,1)*T; Wtbb(1,1)*T,0,Wtbb(3,1)*T,-Wtbb(2,1)*T; Wtbb(2,1)*T,-Wtbb(3,1)*T,0,Wtbb(1,1)*T; Wtbb(3,1)*T,Wtbb(2,1)*T,-Wtbb(1,1)*T,0Q=(1-dltaTita0_fang/8)*eye(4)

27、+0.5*dltaTita)*q0(k);q1(k);q2(k);q3(k);q0(k+1)=Q(1);q1(k+1)=Q(2);q2(k+1)=Q(3);q3(k+1)=Q(4); endfigurehold oni=1:n1;subplot(1,2,1),plot(i,Vxt(i)%速度誤差title('疊加噪聲后的東向速度誤差')subplot(1,2,2),plot(i,Vyt(i)title('疊加噪聲后的的北向速度誤差') figurehold oni=1:n1;subplot(1,2,1),plot(i,L(i)*180/pi)%位置誤差title

28、('疊加噪聲后的的緯度誤差')subplot(1,2,2),plot(i,nmda(i)*180/pi)title('疊加噪聲后的的經(jīng)度誤差') figurehold oni=1:n1;plot(i,Phai(i)*180/pi)%姿態(tài)角誤差subplot(1,3,1),title('疊加噪聲后的的航向角誤差') figurehold oni=1:n1;plot(i,Thita(i)*180/pi)%subplot(1,3,2),title('疊加噪聲后的俯仰角誤差') figurehold oni=1:n1;plot(i,Gam

29、a(i)*180/pi)%subplot(1,3,3), title('疊加噪聲后的橫滾角誤差') %軌跡發(fā)生器的數(shù)據(jù)處理%clear,clcinvout=load('E:慣導(dǎo)實(shí)驗(yàn)數(shù)據(jù)第四次實(shí)驗(yàn)4.3第四部分半物理仿真數(shù)據(jù)生成方法及數(shù)據(jù)格式說明IMU數(shù)據(jù)invout.dat');Wx_invout=invout(:,5);Wy_invout=invout(:,6);Wz_invout=invout(:,7); Wibb=Wx_invout,Wy_invout,Wz_invout;Fx_invout=invout(:,2);Fy_invout=invout(:,3

30、);Fz_invout=invout(:,4);Fibb=Fx_invout,Fy_invout,Fz_invout; L_invout=invout(:,8); %緯度 Jingdu_invout=invout(:,9);%經(jīng)度Height_invout=invout(:,10);%高度N1=size(invout); n1=N1(1,1); q0=zeros(n1,1);q1=zeros(n1,1);q2=zeros(n1,1);q3=zeros(n1,1);Phai=zeros(n1,1);Thita=zeros(n1,1);Gama=zeros(n1,1);Phai(1)=0/180*

31、pi;%偏航初始角Thita(1)=0*pi/180;%俯仰初始角Gama(1)=0*pi/180;%橫滾初始角 L=zeros(n1,1);nmda=zeros(n1,1);Vxt=zeros(n1+1,1);Vyt=zeros(n1+1,1); q0(1)=cos(-Phai(1)/2)*cos(Thita(1)/2)*cos(Gama(1)/2)+sin(-Phai(1)/2)*sin(Thita(1)/2)*sin(Gama(1)/2);q1(1)=cos(-Phai(1)/2)*sin(Thita(1)/2)*cos(Gama(1)/2)+sin(-Phai(1)/2)*cos(Th

32、ita(1)/2)*sin(Gama(1)/2);q2(1)=cos(-Phai(1)/2)*cos(Thita(1)/2)*sin(Gama(1)/2)-sin(-Phai(1)/2)*sin(Thita(1)/2)*cos(Gama(1)/2);q3(1)=cos(-Phai(1)/2)*sin(Thita(1)/2)*sin(Gama(1)/2)-sin(-Phai(1)/2)*cos(Thita(1)/2)*cos(Gama(1)/2);Wie=0.000072921151467;%已經(jīng)是弧度制L(1)=40/180*pi;nmda(1)=116.0/180*pi;T=0.01;%采樣

33、頻率為100HzVxt(1)=0;Vyt(1)=0;Re=6378245+80;%加高度80米e=1/298.3;for k=1:n1c11=q0(k)2+q1(k)2-q2(k)2-q3(k)2;c12=2*(q1(k)*q2(k)+q0(k)*q3(k);c13=2*(q1(k)*q3(k)-q0(k)*q2(k);c21=2*(q1(k)*q2(k)-q0(k)*q3(k);c22=q0(k)2-q1(k)2+q2(k)2-q3(k)2;c23=2*(q2(k)*q3(k)+q0(k)*q1(k);c31=2*(q1(k)*q3(k)+q0(k)*q2(k);c32=2*(q2(k)*q

34、3(k)-q0(k)*q1(k);c33=q0(k)2-q1(k)2-q2(k)2+q3(k)2;Cnb=c11,c12,c13 c21,c22,c23 c31,c32,c33; if abs(c22)>0.0000000000001 Phai(k)=atan(-c21/c22); end if abs(c22)>0.0000000000001 & c21>0 Phai(k)=pi/2; end if abs(c22)>0.0000000000001 & c21<0 Phai(k)=-pi/2; end if abs(c22)>0.00000

35、00000001 & c22>0 Phai(k)=atan(-c21/c22); endif abs(c22)>0.0000000000001 & c22>0 & c21>0 Phai(k)=atan(c21/c22)+pi;endif abs(c22)>0.0000000000001 & c22>0 & c21<0 Phai(k)=atan(-c21/c22)-pi;end Thita(k)=asin(c23);Gama(k)=-atan(c13/c33); Cbn=inv(Cnb);Aibn=Cbn*Fibb(k,:)'Rxt=Re/(1-e*(sin(L(k)*sin(L(k); axt=Aibn(1,1)+2*Wie*sin(L(k)*Vyt(k)+Vyt(k)*Vxt(k)*tan(L(k)/Rxt;ayt=Aibn(2,1)-2*Wie*sin(L(k)*Vxt(k)-Vxt(k)*Vxt(k)*tan(L(k)/Rxt; Vxt(k+1)=axt*T+Vxt(k);Vyt(k+1)=ayt*T+Vyt(