捷联惯导Matlab程序.docxVIP

哈尔滨工业大学《导航原理》课程 捷联惯导程序，依据加表和陀螺仪的输出数据来求解飞行器的姿态 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 Vu_Matrix = zeros(1,12000); = zeros(1,12000); %以下计算捷联矩阵的初始值，捷联矩阵的初始值仅仅由 Theta，Gama， Fai 的初始值决定 T = [