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

. .

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

clc;

clear;

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

Data=importdata('temp.txt'); % 导入实验所采集的数据，以矩阵形式赋给 Data 变量，

temp.txt 必须与该 M 文件在同一个文件夹中

Px=Data(:,3); % Px,Py,Pz 为陀螺仪的输出值

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 ;

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 表示，经度的初始值划为弧度形式

- .jz*