搜索
捷联惯导程序，依据加表和陀螺仪的输出数据来求解飞行器的姿态clc;clear;formatlong;%设置数据精度为15位小数Data=importdata('temp.txt');%导入实验所采集的数据，以矩阵形式赋给Data变量，temp.txt必须与该M文件在同一个文件夹中Px=Data(:,3);%Px,Py,Pz为陀螺仪的输出值Py=Data(:,4);Pz=Data(:,5);Nx=Data(:,6);%Nx,Ny，Nz为加速度计的输出值Ny=Data(:,7);Nz=Data(:,8);%陀螺仪模型参数标定如下：Sx=-4.085903e-006;Sy=-4.085647e-006;Sz=-4.085170e-006;Mxy=5.059527e-003;Mxz=-1.031103e-003;Myx=-3.355451e-003;Myz=3.508468e-003;Mzx=-1.266671e-003;Mzy=-2.318244e-004;Dx=-2.009710e-006;Dy=8.156346e-007;Dz=-5.749059e-007;GyroCali_A=[1-Mxy-Mxz;-Myx1-Myz;-Mzx-Mzy1];%加速度计模型参数标定如下：Kx=9.272930e-004;Ky=9.065544e-004;Kz=9.443748e-004;Ixy=6.533872e-003;Ixz=9.565992e-004;Iyx=-6.319376e-003;Iyz=-6.902339e-004;Izx=-1.144549e-003;Izy=-3.857963e-004;Bx=-3.400847e-002;By=-8.916341e-003;Bz=-9.947414e-003;AccCali_A=[1-Ixy-Ixz;-Iyx1-Iyz;-Izx-Izy1];Delta_t=0.05;%采样时间为0.05秒Delta_Theta_x=0;Delta_Theta_y=0;Delta_Theta_z=0;%定义陀螺仪输出的角度增量Delta_Vx=0;Delta_Vy=0;Delta_Vz=0;%定义加速度计输出的速度增量L=zeros(1,12001);L(1)=45.7328*pi/180;%纬度用L表示，纬度的初始值划为弧度形式，因为后面计算位置矩阵更新L(2)=45.7328*pi/180;%时需要用到前两次的L值来计算当前L值，所以在此定义2个初始L值Lamda=126.6287*pi/180;%经度用Lamda表示，经度的初始值划为弧度形式h=136;%高度用h表示V=[0;0;0];%导航坐标系中的东北天初始速度都为0Vx=0;%方便后面的速度计算与速度更新Vy=0;Vz=0;Theta=0;Gama=0;Fai=0;%初始姿态角（俯仰角/倾斜角/航向角）都为0,此处均为弧度Re=6378254;Rp=6356803;%定义地球的半长轴与半短轴e=(Re-Rp)/Re;%定义旋转椭球扁率（椭球度）Wie=15.04107/180*pi;%定义地球自转角速度，地球坐标系相对于惯性坐标系的角速度Theta_Matrix=zeros(1,12000);%定义姿态角矩阵，供画图用Gama_Matrix=zeros(1,12000);Fai_Matrix=zeros(1,12000);L_Matrix=zeros(1,12001);%定义经纬度矩阵，供画图用，L的特殊性决定了其数据个数为12001L_Matrix(1)=45.7328;Lamda_Matrix=zeros(1,12000);Ve_Matrix=zeros(1,12000);%定义速度矩阵，供画图用Vn_Matrix=zeros(1,12000);Vu_Matrix=zeros(1,12000);%以下计算捷联矩阵的初始值，捷联矩阵的初始值仅仅由Theta，Gama，Fai的初始值决定T=[cos(Gama)*cos(Fai)-sin(Gama)*sin(Theta)*sin(Fai)-cos(Theta)*sin(Fai)sin(Gama)*cos(Fai)+cos(Gama)*sin(Theta)*sin(Fai);cos(Gama)*sin(Fai)+sin(Gama)*sin(Theta)*cos(Fai)cos(Theta)*cos(Fai)sin(Gama)*sin(Fai)-cos(Gama)*sin(Theta)*cos(Fai);-sin(Gama)*cos(Theta)sin(Theta)cos(Gama)*cos(Theta)];%由捷联矩阵的初始值计算初始四元数值，为捷联矩阵的实时更新做准备if(T(3,2)-T(2,3)0)Q1=0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3));elseif(T(3,2)-T(2,3)==0)Q1=0;elseQ1=-0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3));end%求解Q1endif(T(1,3)-T(3,1)0)Q2=0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3));elseif(T(1,3)-T(3,1)==0)Q2=0;elseQ2=-0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3));end%求解Q2endif(T(2,1)-T(1,2)0)Q3=0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3));elseif(T(2,1)-T(1,2)==0)Q3=0;elseQ3=-0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3));end%求解Q3endQ0=0.5*sqrt(1-Q1*Q1-Q2*Q2-Q3*Q3);%求解Q0Q=[Q0;Q1;Q2;Q3];%四元数初始值Q=Q/sqrt(Q0*Q0+Q1*Q1+Q2*Q2+Q3*Q3);%四元数的初始归一化，为得到最小漂移误差%以下求位置矩阵的初始值，通过位置矩阵更新后，反过来算运载体所在的经纬度%位置矩阵仅仅与经纬度有关系，Ce2n表示把地球坐标系转换为导航坐标系的转换矩阵Ce2n=[-sin(Lamda)cos(Lamda)0;-sin(L(1))*cos(Lamda)-sin(L(1))*sin(Lamda)cos(L(1));cos(L(1))*cos(Lamda)cos(L(1))*sin(Lamda)sin(L(1))];%大循环，共执行12000次，实时更新捷联矩阵，速度矩阵，位置矩阵，保存作图所需数据fork=1:12000;GyroCali_B=[Sx*Px(k)-Dx*Delta_t;Sy*Py(k)-Dy*Delta_t;Sz*Pz(k)-Dz*Delta_t];Delta_Theta=GyroCali_A*GyroCali_B;%计算陀螺仪输出的角度增量Delta_Theta_x=Delta_Theta(1);Delta_Theta_y=Delta_Theta(2);Delta_Theta_z=Delta_Theta(3);Delta_Theta_Module=sqrt(Delta_Theta_x*Delta_Theta_x+Delta_Theta_y*Delta_Theta_y+Delta_Theta_z*Delta_Theta_z);AccCali_B=[Kx*Nx(k)-Bx*Delta_t;Ky*Ny(k)-By*Delta_t;Kz*Nz(k)-Bz*Delta_t];Delta_V=AccCali_A*AccCali_B;%计算加速度计输出的速度增量Delta_Vx=Delta_V(1);Delta_Vy=Delta_V(2);Delta_Vz=Delta_V(3);Delta_V_Module=sqrt(Delta_Vx*Delta_Vx+Delta_Vy*Delta_Vy+Delta_Vz*Delta_Vz);%使用毕卡法求解四元数更新矩阵，即捷联矩阵Bika=zeros(4);Bika(1,1)=cos(0.5*Delta_Theta_Module);Bika(1,2)=-Delta_Theta_x/Delta_Theta_Module*sin(0.5*Delta_Theta_Module);Bika(1,3)=-Delta_Theta_y/Delta_Theta_Module*sin(0.5*Delta_Theta_Module);Bika(1,4)=-Delta_Theta_z/Delta_Theta_Module*sin(0.5*Delta_Theta_Module);Bika(2,1)=-Bika(1,2);Bika(2,2)=Bika(1,1);Bika(2,3)=-Bika(1,4);Bika(2,4)=Bika(1,3);Bika(3,1)=-Bika(1,3);Bika(3,2)=-Bika(2,3);Bika(3,3)=Bika(1,1);Bika(3,4)=-Bika(1,2);Bika(4,1)=-Bika(1,4);Bika(4,2)=-Bika(2,4);Bika(4,3)=-Bika(3,4);Bika(4,4)=Bika(1,1);Q=Bika*Q;%每循环一次，更新一次四元素Q值，为求捷联矩阵Q=Q/sqrt(Q0*Q0+Q1*Q1+Q2*Q2+Q3*Q3);%四元数的归一化，为得到最小漂移误差Q0=Q(1);Q1=Q(2);Q2=Q(3);Q3=Q(4);%捷联矩阵的四元数表达式T=[Q0*Q0+Q1*Q1-Q2*Q2-Q3*Q32*(Q1*Q2-Q0*Q3)2*(Q1*Q3+Q0*Q2)2*(Q1*Q2+Q0*Q3)Q0*Q0-Q1*Q1+Q2*Q2-Q3*Q32*(Q2*Q3-Q0*Q1)2*(Q1*Q3-Q0*Q2)2*(Q2*Q3+Q0*Q1)Q0*Q0-Q1*Q1-Q2*Q2+Q3*Q3];%*********************************************************************%********************求三个姿态角Theta，Gama和Fai********************%*********************************************************************Theta_Main=asin(T(3,2));Gama_Main=atan(-T(3,1)/T(3,3));Fai_Main=atan(-T(1,2)/T(2,2));Theta=Theta_Main;if(T(3,3)0)Gama=Gama_Main;elseif(T(3,3)0&&Gama_Main0)Gama=Gama_Main+pi;elseGama=Gama_Main-pi;%此处用else实为不妥，不过为了程序的完善性，只能这样了endendif(T(2,2)0)Fai=Fai_Main+pi;elseif(T(2,2)==0)Fai=pi/2;elseif(Fai_Main0)Fai=Fai_Main;elseFai=Fai_Main+2*pi;endendend%以下存储姿态角到三个矩阵里面，为画图做准备Theta_Matrix(k)=Theta*180/pi;%作图用矩阵,以角度表示Gama_Matrix(k)=Gama*180/pi;%作图用矩阵,以角度表示if(Fai2*pi)Fai_Matrix(k)=Fai*180/pi;elseFai_Matrix(k)=Fai*180/pi-360;%作图用矩阵,以角度表示end%到此为止，姿态角的求解完毕，以下先求速度%*********************************************************************%*******