基于运动规划的惯性导航系统动态实验GAGGAGAGGAFFFFAFAF二零一三年六月十日实验4.1 惯性导航系统运动轨迹规划与设计实验一、实验目的为进行动态下简化惯性导航算法的实验研究，进行路径和运动状态规划，以验证不同运动状态下惯导系统的性能。

通过实验掌握步进电机控制方法，并产生不同运动路径和运动状态。

二、实验内容学习利用6045B 控制板对步进电机进行控制的方法，并控制电机使运动滑轨产生定长运动和不同加速度下的定长运动。

三、实验系统组成USB_PCL6045B 控制板（评估板）、运动滑轨和控制计算机组成。

四、实验原理IMU安装误差系数的计算方法GAGGAGAGGAFFFFAFAFUSB_PCL6045B 控制板采用了USB 串行总线接口通信方式，不必拆卸计算机箱就可以在台式机或笔记本电脑上进行运动控制芯片PCL6045B 的学习和评估。

USB_PCL6045B 评估板采用USB 串行总线方式实现评估板同计算机的数据交换，由评估板的FIFO 控制回路完成步进电机以及伺服电机的高速脉冲控制，任意 2 轴的圆弧插补，2-4 轴的直线插补等运动控制功能。

USB_PCL6045B 评估板上配置了全部PCL6045B 芯片的外部信号接口和增量编码器信号输入接口。

由USB_PCL6045B 评估测试软件可以进行PCL6045B 芯片的主要功能的评估测试。

GAGGAGAGGAFFFFAFAF图4-1-1USB_PCL6045B 评估板原理框图如图4-1-1 所示，CN11 接口主要用于外部电源连接，可以选择DC5V 单一电源或DC5V/24V 电源。

CN12 接口是USB 信号接口，用于USB_PCL6045B 评估板同计算机的数据交换。

USB_PCL6045B 评估板已经完成对PCL6045B 芯片的底层程序开发和硬件资源与端口的驱动，并封装成156 个API 接口函数。

用户可直接在VC 环境下利用API 接口函数进行编程。

五、实验内容GAGGAGAGGAFFFFAFAF1、操作步骤1）检查电机驱动电源（24V）2）检查USB_PCL6045B 控制板与上位机及电机驱动器间的连接电缆3）启动USB_PCL6045B 控制板评估测试系统检查系统是否正常工作。

4）运行编写的定长运动程序，并比较实际位移与设定位移。

5）修改程序设定不同运动长度，并重复执行步骤4）。

6）对记录实验数据，并进行误差分析。

2、实验数据处理GAGGAGAGGAFFFFAFAF基于VC的控制界面：本次实验必须先设计控制系统的上位机，通过上位机的串口向下位机发送控制命令，下位机接收到命令后，产生PWM波，控制电机的正反转以此达到控制导轨运动的目的。

系统的控制界面如图1所示：图 1 系统的控制界面控制导轨运动，运动采取正向运动，再返回，即IMU 的实际运行位移为零。

并保存数据控制界面的应用程序源程序仅写出VC中按钮的响应程序：GAGGAGAGGAFFFFAFAFvoid CAaaDlg::Online() ////定长运动{// TODO: Add your control notification handler code hereUSB_initial();USB_default_set();USB_set_org_logic(AXS_AX,0);//原点开关的逻辑,负逻辑USB_set_el_logic(AXS_AX,0);//硬极限输入逻辑,低电平使能USB_set_sd_logic(AXS_AX,0);//减速开关的输入逻辑,负逻辑USB_set_alm_logic(AXS_AX, 1);//报警输入信号逻辑USB_set_inp_logic(AXS_AX,1);//in的输入信号逻辑USB_ez_logic(AXS_AX,0);//Z相的输入逻辑GAGGAGAGGAFFFFAFAFUSB_set_pls_outmode(AXS_AX,1);USB_set_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(PeekMessage(&msg,0,0,100,PM_REMOVE)){GAGGAGAGGAFFFFAFAFTranslateMessage(&msg);DispatchMessage(&msg);}Sleep(100);}*/}void CAaaDlg::OnButton1() //////停止运动{// TODO: Add your control notification handler code hereUSB_sd_stop(AXS_AX);}void CAaaDlg::OnGetSpeed() //////获得速度{// TODO: Add your control notification handler code hereGAGGAGAGGAFFFFAFAFUSB_get_speed(AXS_AX, &m_speed);UpdateData(FALSE);}void CAaaDlg::OnButton3() ///OK 按钮程序{GAGGAGAGGAFFFFAFAF// TODO: Add your control notification handler code hereUpdateData(true);}3，处理数据由实验原理可知，惯性测量单元（IMU）可以通过自身独立的测量结果进行积分，计算出目标运动的角度和位移等量。

本次实验就是利用IMU的加速度计的某一敏感轴测量导轨运行的加速度，通过加速度两次积分得到物体的位移，计算结果如图2所示：实验经过往返，从原理上讲位移应该为零。

处理结果：位移曲线：GAGGAGAGGAFFFFAFAF速度曲线：GAGGAGAGGAFFFFAFAF4，源程序：A=load('E:\惯性器件综合实验\我的作业\实验四\X300000_V10000.txt');T=1/200; %%%%单位为秒g=9.78;Ax=A(:,4)*g/1000; %%%提取加速度计的值转化为m/s^2 Ax=Ax*(1.0009)-0.0036595*g;vx=zeros(12657,1);sx=zeros(12657,1);u=zeros(12657,1);%%%%%计算位移for i=2:12657GAGGAGAGGAFFFFAFAFvx(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)*T*T;u(i)=i;endfigureplot(u/100,vx);xlabel('时间/秒'),ylabel('速度米/秒');figureplot(u/100,sx);xlabel('时间/秒'),ylabel('位移米');5，实验结果分析从原理上讲IMU做往返运动，位移应该出现增大和减小的趋势，但是由于各种误差角，而且滑轨也不能保证当地水平，在计算过程中，也未减去有害加速度。

所以误差很大。

而且根据所采集的数据可知加速度计并没有感知方向，在实验过程中应该根据计算脉冲与时间，自己计算方向时间GAGGAGAGGAFFFFAFAF惯性导航系统半物理仿真实验一、实验目的进行惯导系统半物理仿真实验，以验证惯性器件真实误差特性情况下惯性导航系统的性能。

二、实验内容将采集到陀螺仪与加速度计的真实误差数据叠加到轨迹发生器产生的导航参数真值上，进行惯导解算，并分析误差特性。

三、实验系统组成真实的陀螺仪、加速度计或 IMU，数据采集系统和数据处理计算机。

四、实验步骤（1）采集实验数据（2）处理采集的实验数据，生成半物理的惯性器件误差数据（3）生成半物理的导航数据，进行导航解算（4）对导航解算结果进行分析（5）完成实验报告GAGGAGAGGAFFFFAFAF五、实验内容及结果（1）半物理仿真数据的生成：a)应用前面IMU实验或惯性导航系统动态实验中采集的陀螺仪与加速度计的静态数据DATAb)对以上采集的静态数据求取均方差X（结果为X度/小时或Xg）c)将DATA中数据去掉均值生成新的数据DATA1（器件噪声）d)自己设定要仿真的陀螺或加速度计的精度Y（度/小时或g）e)将DATA1中数据乘以Y/X生成新的数据DATA2（半物理仿真噪声）f)从DATA2中读取数据并叠加到轨迹发生器产生的标准数据(不含噪声)上，进行导航解算。

（如初始采集的数据长度不够，可以将DATA2中数据重复利用，即将生成一个几倍长度于DATA2和数据文件DATA3，并从DATA3中读取半物理数据并叠加到轨迹发生器产生的标准数据上）（2）加半物理仿真噪声数据的导航结果：GAGGAGAGGAFFFFAFAFGAGGAGAGGAFFFFAFAF（3）叠加噪声的导航结果：GAGGAGAGGAFFFFAFAFGAGGAGAGGAFFFFAFAFGAGGAGAGGAFFFFAFAF（4）结果分析：由实验结果可见，叠加的仿真噪声数据对姿态的解算影响很大；但由于所加噪声较小，所以噪声数据对位移和速度的解算影响不大。

六，源程序GAGGAGAGGAFFFFAFAFclear,clcinvout=load('E:\惯导实验数据\第四次\实验4.3\第四部分半物理仿真数据生成方法及数据格式说明\IMU数据\invout.dat');CaijiShuju=load('E:\惯导实验数据\第四次\实验4.3\第四部分半物理仿真数据生成方法及数据格式说明\IMU数据\data2.txt');W=CaijiShuju(:,3:5);F=CaijiShuju(:,9:11);W_pingjun=mean(W);GAGGAGAGGAFFFFAFAFF_pingjun=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);%%%%%%%%%%%%%陀螺的精度设为0.5度/小时%%%%%%%%%%%%%%%%%%%w_jingdu=0.5/3600*pi/180%0.5度/小时转成弧度sx=0;for i=1:nsx=sx+(W(i,1)-W_pingjun(1,1))^2GAGGAGAGGAFFFFAFAFendwx_junfangcha=sqrt(sx/n);%x陀螺的均方差 wx1=w_jingdu/wx_junfangcha;Wx=wx*wx1 %半物理仿真噪声Wxsx=0;for i=1:nsx=sx+(W(i,2)-W_pingjun(1,2))^2endwy_junfangcha=sqrt(sx/n);%y陀螺的均方差 wy2=w_jingdu/wy_junfangcha;Wy=wy*wy2 %半物理仿真噪声Wysx=0;for i=1:nsx=sx+(W(i,3)-W_pingjun(1,3))^2endwz_junfangcha=sqrt(sx/n);%z陀螺的均方差 wz3=w_jingdu/wz_junfangcha;GAGGAGAGGAFFFFAFAFWz=wz*wz3 %半物理仿真噪声Wz %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%求加计的均方差%%%%%%%%%%%%%%%%%%%%%%%f_jingdu=1/1000*9.8%加计的精度为1mgsx=0;for i=1:nsx=sx+(F(i,1)-F_pingjun(1,1))^2endfx_junfangcha=sqrt(sx/n);%x加计的均方差GAGGAGAGGAFFFFAFAFfx1= f_jingdu/fx_junfangcha;Fx=fx*fx1;sx=0;for i=1:nsx=sx+(F(i,2)-F_pingjun(1,2))^2endfy_junfangcha=sqrt(sx/n);%y加计的均方差fx2= f_jingdu/fy_junfangcha;Fy=fx*fx2;sx=0;for i=1:nsx=sx+(F(i,3)-F_pingjun(1,3))^2endfz_junfangcha=sqrt(sx/n);%z加计的均方差fx3= f_jingdu/fz_junfangcha;Fz=fx*fx3; %%%%%%%%%%%%%%%%%%%%%%%%%%%%轨迹发生器数据叠加噪GAGGAGAGGAFFFFAFAF声%%%%%%%%%%%%%%%%%%%%%%%%%%%%%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(:,8); %纬度Jingdu_invout=invout(:,9);%经度Height_invout=invout(:,10);%高度%%%%%%%%%%%%%%%开始叠加%%%%%%%%%%%%%%%%%%%%N1=size(invout);n1=N1(1,1);%%%%应为采样点数大于轨迹发生器的个数，所以以轨迹发生器的个数为准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];GAGGAGAGGAFFFFAFAFFxx=Fx(1:n1,1)+Fx_invout;Fyy=Fy(1:n1,1)+Fy_invout ;Fzz=Fz(1:n1,1)+Fz_invout;Fibb=[Fxx,Fyy,Fzz];q0=zeros(n1,1);GAGGAGAGGAFFFFAFAFq1=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)/2)*cos(Thita(1)/2)*cos(Gama(1)/2)+sin(-Phai(1)/2)*sin(Thita(1)/2)*sin(Gama(1)/2);q1(1)=cos(-GAGGAGAGGAFFFFAFAFPhai(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)/2)*sin(Thita(1)/2)*sin(Gama(1)/2)-sin(-Phai(1)/2)*cos(Thita(1)/2)*cos(Gama(1)/2);Wie=0.000072921151467;%已经是弧度制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;GAGGAGAGGAFFFFAFAFc12=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)*q3(k)-q0(k)*q1(k));c33=q0(k)^2-q1(k)^2-q2(k)^2+q3(k)^2; Cnb=[c11,c12,c13c21,c22,c23c31,c32,c33];GAGGAGAGGAFFFFAFAFif abs(c22)>0.0000000000001Phai(k)=atan(-c21/c22);endif abs(c22)>0.0000000000001 & c21>0 Phai(k)=pi/2;endif abs(c22)>0.0000000000001 & c21<0Phai(k)=-pi/2;endif 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 abs(c22)>0.0000000000001 & c22>0 & c21<0 Phai(k)=atan(-c21/c22)-pi;endGAGGAGAGGAFFFFAFAFThita(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(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))+nmd a(k);Wenn=[-GAGGAGAGGAFFFFAFAFVyt(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))];Winb=Cnb*Winn;Wtbb=Wibb(k,:)'-Winb;dltaTita0_fang=(Wtbb(1,1)*T)^2+(Wtbb(2,1)*T)^2+(Wtb b(3,1)*T)^2;dltaTita=[0,-Wtbb(1,1)*T,-Wtbb(2,1)*T,-Wtbb(3,1)*T;GAGGAGAGGAFFFFAFAFWtbb(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,0]Q=((1-dltaTita0_fang/8)*eye(4)+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;GAGGAGAGGAFFFFAFAFsubplot(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('叠加噪声后的的纬度误差')subplot(1,2,2),plot(i,nmda(i)*180/pi)title('叠加噪声后的的经度误差')figurehold oni=1:n1;plot(i,Phai(i)*180/pi)%姿态角误差subplot(1,3,1), title('叠加噪声后的的航向角误差')GAGGAGAGGAFFFFAFAFfigurehold oni=1:n1;plot(i,Thita(i)*180/pi)%subplot(1,3,2), title('叠加噪声后的俯仰角误差')figurehold oni=1:n1;plot(i,Gama(i)*180/pi)%subplot(1,3,3),GAGGAGAGGAFFFFAFAFtitle('叠加噪声后的横滚角误差')%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%轨迹发生器的数据处理%%%%%%%%%%%%%%%clear,clcinvout=load('E:\惯导实验数据\第四次\实验4.3\第四部分半物理仿真数据生成方法及数据格式说明\IMU数据\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);Fz_invout=invout(:,4);Fibb=[Fx_invout,Fy_invout,Fz_invout];L_invout=invout(:,8); %纬度GAGGAGAGGAFFFFAFAFJingdu_invout=invout(:,9);%经度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*pi;%偏航初始角Thita(1)=0*pi/180;%俯仰初始角Gama(1)=0*pi/180;%横滚初始角L=zeros(n1,1);nmda=zeros(n1,1);Vxt=zeros(n1+1,1);GAGGAGAGGAFFFFAFAFVyt=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(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);GAGGAGAGGAFFFFAFAFq3(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;%已经是弧度制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)*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));GAGGAGAGGAFFFFAFAFc31=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,c13c21,c22,c23c31,c32,c33];if abs(c22)>0.0000000000001Phai(k)=atan(-c21/c22);endif abs(c22)>0.0000000000001 & c21>0 Phai(k)=pi/2;endif abs(c22)>0.0000000000001 & c21<0Phai(k)=-pi/2;endif abs(c22)>0.0000000000001 & c22>0 Phai(k)=atan(-c21/c22);endGAGGAGAGGAFFFFAFAFif 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;endThita(k)=asin(c23);GAGGAGAGGAFFFFAFAFGama(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(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))+nmd a(k);Wenn=[-Vyt(k)/Ryt;Vxt(k)/Rxt;Vxt(k)/Rxt*tan(L(k))];%课本86GAGGAGAGGAFFFFAFAF页4.2-38式Winn=Wenn+[0;Wie*cos(L(k));Wie*sin(L(k))];Winb=Cnb*Winn;Wtbb=Wibb(k,:)'-Winb;dltaTita0_fang=(Wtbb(1,1)*T)^2+(Wtbb(2,1)*T)^2+(Wtb b(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,0]Q=((1-dltaTita0_fang/8)*eye(4)+0.5*dltaTita)*[q0(k);q1(k) ;q2(k);q3(k)];q0(k+1)=Q(1);GAGGAGAGGAFFFFAFAFq1(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('轨迹发生器的北向速度误差')GAGGAGAGGAFFFFAFAFfigurehold oni=1:n1;subplot(1,2,1),plot(i,L(i)*180/pi)%位置误差title('轨迹发生器的纬度误差')subplot(1,2,2),plot(i,nmda(i)*180/pi)title('轨迹发生器的经度误差')figurehold oni=1:n1;plot(i,Phai(i)*180/pi)%姿态角误差subplot(1,3,1), title('轨迹发生器的航向角误差')figurehold oni=1:n1;plot(i,Thita(i)*180/pi)%subplot(1,3,2),title('轨迹发生器的俯仰角误差')GAGGAGAGGAFFFFAFAFfigurehold oni=1:n1;plot(i,Gama(i)*180/pi)%subplot(1,3,3), title('轨迹发生器的横滚角误差')如有侵权请联系告知删除，感谢你们的配合！29942 74F6 瓶\€%21012 5214 刔31813 7C45 籅31366 7A86 窆27348 6AD4 櫔?29496 7338 猸35662 8B4E 譎m39403 99EB 駫834407 8667 虧GAGGAGAGGAFFFFAFAF。