%捷聯慣導程序，依據加表和陀螺儀的輸出數據來求解飛行器的姿態(tài)

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;??????????%初始姿態(tài)角（俯仰角/傾斜角/航向角）都為0此處均為弧度

Re?=?6378254?;??Rp?=?6356803?;%定義地球的半長軸與半短軸
e??=?（Re?-?Rp）/Re?;???????????%定義旋轉橢球扁率（橢球度）
Wie?=?15.04107/180*pi?;???????%定義地球自轉角速度，地球坐標系相對于慣性坐標系的角速度

Theta_Matrix?=?zeros（112476）;????%定義姿態(tài)角矩陣，供畫圖用
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