捷联惯导程序，依据加表和陀螺仪的输出数据来求解飞行器的姿态clc;clear;format long; %设置数据精度为15位小数Data=importdata(''); % 导入实验所采集的数据，以矩阵形式赋给Data变量，必须与该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 = ; Sy = ; Sz = ;Mxy = ; Mxz = ; Myx = ;Myz = ; Mzx = ; Mzy = ;Dx = ; Dy = ; Dz = ;GyroCali_A = [ 1 -Mxy -Mxz ; -Myx 1 -Myz ; -Mzx -Mzy 1 ];% 加速度计模型参数标定如下：Kx = ; Ky = ; Kz = ;Ixy = ; Ixz = ; Iyx = ;Iyz = ; Izx = ; Izy = ;Bx = ; By = ; Bz = ;AccCali_A = [1 -Ixy -Ixz ; -Iyx 1 -Iyz ; -Izx -Izy 1 ];Delta_t = ; %采样时间为秒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)= *pi/180 ; %纬度用L表示，纬度的初始值划为弧度形式，因为后面计算位置矩阵更新L(2)= *pi/180 ; %时需要用到前两次的L值来计算当前L值，所以在此定义2个初始L值Lamda = *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 = 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) = ;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 = *sqrt(1+T(1,1)-T(2,2)-T(3,3));else if (T(3,2)-T(2,3)==0)Q1 = 0;else Q1 = *sqrt(1+T(1,1)-T(2,2)-T(3,3));end %求解Q1endif(T(1,3)-T(3,1)>0)Q2 = *sqrt(1-T(1,1)+T(2,2)-T(3,3));else if (T(1,3)-T(3,1)==0)Q2 = 0;else Q2 = *sqrt(1-T(1,1)+T(2,2)-T(3,3));end %求解Q2endif(T(2,1)-T(1,2)>0)Q3 = *sqrt(1-T(1,1)-T(2,2)+T(3,3));else if (T(2,1)-T(1,2)==0)Q3 = 0;else Q3 = *sqrt(1-T(1,1)-T(2,2)+T(3,3));end %求解Q3endQ0 = *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次，实时更新捷联矩阵，速度矩阵，位置矩阵，保存作图所需数据for k = 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 * Delta_Theta_Module);Bika(1,2) = -Delta_Theta_x / Delta_Theta_Module * sin * Delta_Theta_Module);Bika(1,3) = -Delta_Theta_y / Delta_Theta_Module * sin * Delta_Theta_Module);Bika(1,4) = -Delta_Theta_z / Delta_Theta_Module * sin * 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*Q3 2*(Q1*Q2-Q0*Q3) 2*(Q1*Q3+Q0*Q2) 2*(Q1*Q2+Q0*Q3) Q0*Q0-Q1*Q1+Q2*Q2-Q3*Q3 2*(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 ;else if (T(3,3)<0 && Gama_Main > 0)Gama = Gama_Main + pi;else Gama = Gama_Main - pi; %此处用else实为不妥，不过为了程序的完善性，只能这样了endendif ( T(2,2)<0 )Fai = Fai_Main + pi ;else if (T(2,2)==0)Fai = pi/2;else if ( Fai_Main>0)Fai = Fai_Main;else Fai = Fai_Main + 2*pi ;endendend%以下存储姿态角到三个矩阵里面，为画图做准备Theta_Matrix(k) = Theta*180/pi; %作图用矩阵,以角度表示Gama_Matrix(k) = Gama*180/pi; %作图用矩阵,以角度表示if (Fai<2*pi)Fai_Matrix(k) = Fai*180/pi;else Fai_Matrix(k) = Fai*180/pi-360; %作图用矩阵,以角度表示end%到此为止，姿态角的求解完毕，以下先求速度%*********************************************************************%********************求飞行器相对于东北天的速度************************* %*********************************************************************Rm = Re*( 1-2*e+3*e*sin( L(k+1) )^2 );Rn = Re*( 1+e*sin(L(k+1))^2 );LL = 3/2*L(k+1) - 1/2*L(k) ;F = [ 0 -1/(Rm + h) 0 ; 1/(Rn + h) 0 0 ; tan( LL )/(Rn + h) 0 0];g = +*sin(L(k+1))^*h;G = [0;0;-g];Wen2n = F*V;Wie2n = [0 ; Wie*cos(L(k+1)); Wie*sin(L(k+1))];W = 2*Wie2n + Wen2n;W_X = [ 0 -W(3) W(2) ; W(3) 0 -W(1) ; -W(2) W(1) 0 ]; %此式中W_X为2*Wie+Wen2n 的反对称矩阵V = V + T*( Delta_V+ [ 0 -Delta_Theta_z Delta_Theta_y ; Delta_Theta_z 0 -Delta_Theta_x ; -Delta_Theta_y Delta_Theta_x 0 ]*Delta_V ) + Delta_t*(G-W_X*V);Vx = V(1);Vy = V(2);Vz = V(3);Ve_Matrix(k) = V(1);Vn_Matrix(k) = V(2);Vu_Matrix(k) = V(3);%*********************************************************************%********************求飞行器所在的经纬度******************************%*********************************************************************Epsilon = F*V*Delta_t;Ce2n =( eye(3) - [ 0 -Epsilon(3) Epsilon(2) ; Epsilon(3) 0 -Epsilon(1) ; -Epsilon(2) Epsilon(1) 0 ] )*Ce2n ; %位置矩阵实时更新%下面通过位置矩阵来实时更新经纬度L(k+2) = asin( Ce2n(3,3)); %由于L本来就是以矩阵形式定义的，下面定义一个把L用角度表示的矩阵L_Matrix(k+1) = L(k+2)*180/pi;Lamda_Main = atan( Ce2n(3,2)/Ce2n(3,1) ); %计算出来的L和Lamda都是弧度制的if (Ce2n(3,1)>0)Lamda = Lamda_Main;else if (Lamda_Main<0)Lamda = Lamda_Main + pi;else Lamda = Lamda_Main - pi;endendLamda_Matrix(k) = Lamda*180/pi; %作图用矩阵，以角度表示end%**************************************************************************%********************以下是画图程序*****************************************%**************************************************************************k=1:1:12000; %绘制三轴姿态变化图线-绿色figure(1);plot(k/20,Theta_Matrix(k),'g');xlabel('Time(second)');ylabel('Angle(degree)');title('Theta(俯仰角)');grid on;figure(2);plot(k/20,Gama_Matrix(k),'g');xlabel('Time(second)');ylabel('Angle(degree)');title('Gama（滚转角）');grid on;figure(3);plot(k/20,Fai_Matrix(k),'m');xlabel('Time(second)');ylabel('Angle(degree)');title('Fai（偏航角）');grid on;%绘制东北天个方向的速度变化曲线-红色figure(4);plot(k/20,Ve_Matrix(k),'r');xlabel('Time(second)');ylabel('Velocity(m/s)');title('Ve（东向速度）');grid on;figure(5);plot(k/20,Vn_Matrix(k),'r');xlabel('Time(second)');ylabel('Velocity(m/s)');title('Vn（北向速度）');grid on;figure(6);plot(k/20,Vu_Matrix(k),'r');xlabel('Time(second)');ylabel('Velocity(m/s)');title('Vu（天向速度）');grid on;%绘制飞行器所在经纬度曲线-蓝色figure(7);plot(k/20,L_Matrix(k),'b');xlabel('Time(second)');ylabel('Degree');title('Latitude L(纬度)');grid on;figure(8);plot(k/20,Lamda_Matrix(k),'b');xlabel('Time(second)');ylabel('Degree');title('Longitude Lamda(经度)');grid on;。