资源描述：
《捷联惯导Matlab程序求解飞行器的姿态.doc》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。
1、捷联惯导程序，依据加表和陀螺仪的输出数据来求解飞行器的姿态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)
2、;%陀螺仪模型参数标定如下：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-M
3、xz;-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-0
4、03;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)=
5、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=(
6、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_Mat
7、rix=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)*s
8、in(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)
