惯导姿态解算

惯导姿态解算，根据加速度计和陀螺的测量数值，求解惯导系统的姿态，速度和位置的代码

%捷联惯导程序，依据加表和陀螺仪的输出数据来求解飞行器的姿态

clc;
clear;
format long;  %设置数据精度为15位小数

Data=load（‘G:\翟颖课题\惯导解算\惯性器件输出信息2.txt‘）; % 导入实验所采集的数据，以矩阵形式赋给Data变量，temp.txt必须与该M文件在同一个文件夹中
Px=Data（:1）;  % PxPyPz为陀螺仪的输出值
Py=Data（:2）;
Pz=Data（:3）;

Nx=Data（:4）;  % NxNy，Nz为加速度计的输出值
Ny=Data（:5）;
Nz=Data（:6）;
% 陀螺仪模型参数标定如下：
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 ; -Myx 1 -Myz ; -Mzx -Mzy 1 ];
% 加速度计模型参数标定如下：
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 ; -Iyx 1 -Iyz ; -Izx -Izy 1 ];

Delta_t = 0.035; %采样时间为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（112476）;
L（1）= 39.9928*pi/180 ;    %纬度用L表示，纬度的初始值划为弧度形式，因为后面计算位置矩阵更新
L（2）= 39.9928*pi/180 ;    %时需要用到前两次的L值来计算当前L值，所以在此定义2个初始L值
Lamda = 116.3563*pi/180 ; %经度用Lamda表示，经度的初始值划为弧度形式
h = 42;                 %高度用h表示
V = [ 0 ; 0 ; 0 ];  %导航坐标系中的东北天初始速度都为0
Vx = 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（112476）;    %定义姿态角矩阵，供画图用
Gama_Matrix  = zeros（112476）;
Fai_Matrix   = zeros（112476）;
L_Matrix     = zeros（112476）;    %定义经纬度矩阵，供画图用，L的特殊性决定了其数据个数为12001
L_Matrix（1）  =39.9928;           
Lamda_Matrix = zeros（112476）;
Ve_Matrix    = zeros（112476）;    %定义速度矩阵，供画图用
Vn_Matrix    = zeros（112476）;
Vu_Matrix    = zeros（112476）;

%以下计算捷联矩阵的初始值，捷联矩阵的初始值仅仅由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（32）-T（23）>0） 
        Q1 = 0.5*sqrt（1+T（11）-T（22）-T（33））;
    else if （T（32）-T（23）==0）
        Q1 = 0;
     else Q1 = -0.5*sqrt（1+T（11）-T（22）-T（33））;
        end            %求解Q1
    end

    if（T（13）-T（31）>0） 
        Q2 = 0.5*sqrt（1-T（11）+T（22）-T（33））;
    else if （T（13）-T（31）==0）
       Q2 = 0;
     else Q2 = -0.5*sqrt（1-T（11）+T（22）-T（33））;
        end   

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件       10951  2014-04-01 15:59  INS5.m