捷联惯导Matlab程序

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

clc;

clear;

format long; %设置数据精度为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 ; -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.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 ]; %导航坐标系中的东北天初始速度都为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(1,12000); %定义姿态角矩阵，供画图用

Gama_Matrix = zeros(1,12000);

Fai_Matrix = zeros(1,12000);

L_Matrix = zeros(1,12001); %定义经纬度矩阵，供画图用，L的特殊性决定了其数据个数为12001

L_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));

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)+T(2,2)-T(3,3));