捷联惯导Matlab程序求解飞行器的姿态

1、-. z.捷联惯导程序，依据加表和陀螺仪的输出数据来求解飞行器的姿态clc;clear;format long; %设置数据精度为15位小数Data=importdata(temp.t*t); % 导入实验所采集的数据，以矩阵形式赋给Data变量，temp.t*t必须与该M文件在同一个文件夹中P*=Data(:,3); % P*,Py,Pz为陀螺仪的输出值Py=Data(:,4);Pz=Data(:,5);N*=Data(:,6); % N*,Ny，Nz为加速度计的输出值Ny=Data(:,7);Nz=Data(:,8);% 陀螺仪模型参数标定如下：S* = -4.085903e-006 ;

2、Sy = -4.085647e-006 ; Sz = -4.085170e-006 ;M*y = 5.059527e-003 ; M*z = -1.031103e-003 ; My* = -3.355451e-003 ;Myz = 3.508468e-003 ; Mz* = -1.266671e-003 ; Mzy = -2.318244e-004 ;D* = -2.009710e-006 ; Dy = 8.156346e-007 ; Dz = -5.749059e-007 ;GyroCali_A = 1 -M*y -M*z ; -My* 1 -Myz ; -Mz* -Mzy 1 ;% 加速度

3、计模型参数标定如下：K* = 9.272930e-004 ; Ky = 9.065544e-004 ; Kz = 9.443748e-004 ;I*y = 6.533872e-003 ; I*z = 9.565992e-004 ; Iy* = -6.319376e-003 ;Iyz = -6.902339e-004 ; Iz* = -1.144549e-003 ; Izy = -3.857963e-004 ;B* = -3.400847e-002 ; By = -8.916341e-003 ; Bz = -9.947414e-003 ;AccCali_A = 1 -I*y -I*z ; -Iy

4、* 1 -Iyz ; -Iz* -Izy 1 ;Delta_t = 0.05; %采样时间为0.05秒Delta_Theta_* = 0;Delta_Theta_y = 0;Delta_Theta_z = 0; %定义陀螺仪输出的角度增量Delta_V* = 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个

5、初始L值Lamda = 126.6287*pi/180 ; %经度用Lamda表示，经度的初始值划为弧度形式h = 136 ; %高度用h表示V = 0 ; 0 ; 0 ; %导航坐标系中的东北天初始速度都为0V* = 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 ; %定义地球

6、自转角速度，地球坐标系相对于惯性坐标系的角速度Theta_Matri* = zeros(1,12000); %定义姿态角矩阵，供画图用Gama_Matri* = zeros(1,12000);Fai_Matri* = zeros(1,12000);L_Matri* = zeros(1,12001); %定义经纬度矩阵，供画图用，L的特殊性决定了其数据个数为12001L_Matri*(1) = 45.7328; Lamda_Matri* = zeros(1,12000);Ve_Matri* = zeros(1,12000); %定义速度矩阵，供画图用Vn_Matri* = zeros(1,120

7、00);Vu_Matri* = 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)*

8、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); else if (T(3,2)-T(2,3)=0) Q1 = 0; else Q1 = -0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3); end %求解Q1 end if(T(1,3)-T(3,1)0) Q2 = 0.5*sqrt(1-T(1,1

9、)+T(2,2)-T(3,3); else if (T(1,3)-T(3,1)=0) Q2 = 0; else Q2 = -0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3); end %求解Q2 end if(T(2,1)-T(1,2)0) Q3 = 0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3); else if (T(2,1)-T(1,2)=0) Q3 = 0; else Q3 = -0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3); end %求解Q3 end Q0 = 0.5*sqrt(1-Q1*Q1-Q2*Q2-Q3*Q3); %求解Q0

10、Q = 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(

11、Lamda) sin( L(1) ) ;%大循环，共执行12000次，实时更新捷联矩阵，速度矩阵，位置矩阵，保存作图所需数据for k = 1:12000; GyroCali_B = S*P*(k)-D*Delta_t ; Sy*Py(k)-Dy*Delta_t ; Sz*Pz(k)-Dz*Delta_t ; Delta_Theta = GyroCali_A * GyroCali_B ; %计算陀螺仪输出的角度增量 Delta_Theta_* = Delta_Theta(1); Delta_Theta_y = Delta_Theta(2); Delta_Theta_z = Delta_Thet

12、a(3); Delta_Theta_Module = sqrt( Delta_Theta_* * Delta_Theta_* + Delta_Theta_y * Delta_Theta_y + Delta_Theta_z * Delta_Theta_z ); AccCali_B = K*N*(k)-B*Delta_t ; Ky*Ny(k)-By*Delta_t ; Kz*Nz(k)-Bz*Delta_t ; Delta_V = AccCali_A * AccCali_B ; %计算加速度计输出的速度增量 Delta_V* = Delta_V(1); Delta_Vy = Delta_V(2);

13、 Delta_Vz = Delta_V(3); Delta_V_Module = sqrt( Delta_V* * Delta_V* + 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_* / Delta_Theta_Module * sin(0.5 * Delta_Theta_Module); Bika(1,3) = -Delta_Thet

14、a_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,

15、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-Q

16、3*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_Mai

17、n; if (T(3,3)0) Gama = Gama_Main ; else if (T(3,3) 0) Gama = Gama_Main + pi; else Gama = Gama_Main - pi; %此处用else实为不妥，不过为了程序的完善性，只能这样了 end end if ( T(2,2)0) Fai = Fai_Main; else Fai = Fai_Main + 2*pi ; end end end %以下存储姿态角到三个矩阵里面，为画图做准备 Theta_Matri*(k) = Theta*180/pi; %作图用矩阵,以角度表示 Gama_Matri*(k) = G

18、ama*180/pi; %作图用矩阵,以角度表示 if (Fai0) Lamda = Lamda_Main; else if (Lamda_Main0) Lamda = Lamda_Main + pi; else Lamda = Lamda_Main - pi; end end Lamda_Matri*(k) = Lamda*180/pi; %作图用矩阵，以角度表示end%*%*以下是画图程序*%*k=1:1:12000; %绘制三轴姿态变化图线-绿色figure(1);plot(k/20,Theta_Matri*(k),g);*label(Time(second);ylabel(Angle(degree);title(Theta(俯仰角);grid on;figure(2);plot(k/20,Gama_Matri*(k),g);*label(Time(second);ylabel(Angle(degree);title(Gama滚转角);grid on;figure(3);plot(k/20,Fai_