捷联惯性导航初始对准+双子样补偿导航算法

自主导航课程捷联惯性导航跑车试验，粗对准 kalman滤波精对准 + 双子样捷联算法

clear all;clc

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%% 初 始 化 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp（‘初始化‘）;
%%%%%%%地球数据
WIE  = 7.292115e-5 ;%（地球自转角速度，弧度制commented by LiYou）
e   = 0.0818191908426;
R   = 6378137.0;
Tm = 0.01;
%%%%%%%速度初值
Vn = 0;
Vt = 0;
Ve = 0;
V_last = [VnVtVe]‘;
%%%位置初值
Lati  = 28.2202*pi/180;
Longti = 112.9916*pi/180;
Alti      = 0;
g0=9.7803267711905*（1+0.00193185138639*sin（Lati）^2）/（（1-0.00669437999013*sin（Lati）^2）^0.5*（1.0+Alti/R）^2）;
Pos = zeros（31）;
disp（‘初始化完成‘）;
%%%%%%%%%%%%%%%%%%%%%%%%%%%% 初 始 化 完 成 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%载 入 数 据 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp（‘载入数据‘）;
load d_GyroAcc2012.dat
[row columu] = size（d_GyroAcc2012）;
disp（‘载入数据完成‘）
%%%%%%%%%%%%%%%%%%%%%%载 入 数 据 完 成 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%   解 析 初 对 准 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp（‘解析粗对准开始‘）;

Data_1   = zeros（120006）;
Data_1（:1） = d_GyroAcc2012（1:120001）;
Data_1（:2） = d_GyroAcc2012（1:120002）;
Data_1（:3） =d_GyroAcc2012（1:120003）; 
Data_1（:4） = d_GyroAcc2012（1:120004）;
Data_1（:5） = d_GyroAcc2012（1:120005）;
Data_1（:6） = d_GyroAcc2012（1:120006）;
%陀螺数据
W_1 = Data_1（:1:3）‘;
%加表
A_1 = Data_1（:4:6）‘;
%载体系
W    = mean（W_12）*100;
G    = mean（A_12）*100;%（实验数据为 1北 2天 3东，时间间隔为0.01s ；commented by liyou）
W_G  = cross（WG）;
A    = [W  -G -W_G];
%地理系
Wien = [WIE*cos（Lati） WIE*sin（Lati） 0]‘;
gn   =g0;
Gn   = [0 -gn 0]‘;
Wn_Gn= cross（WienGn）;
B    = [Wien Gn Wn_Gn];
%姿态矩阵Cnb表示从N到B
Cnb_1= A/B;
Cnb_1 = Cnb_1 * （Cnb_1‘ * Cnb_1）^（-1/2）;
Cbn_1 = Cnb_1‘;
%%%%姿态角，按照先产生YAW，再产生PITCH，再产生ROLL的顺序
Yaw   = atan2（-Cnb_1（13）Cnb_1（11））*180/pi;
Pitch = asin（Cnb_1（12））*180/pi;
Roll  = atan2（-Cnb_1（32）Cnb_1（22））*180/pi;

disp（‘解析粗对准结果：‘）;
disp（‘偏航角 = ‘）disp（Yaw）;
disp（‘俯仰角 = ‘）disp（Pitch）;
disp（‘滚动角 = ‘）disp（Roll）;
disp（‘解析粗对准结束‘）;
%%%%%%%%%%%%%%%%%%%%%%%% 初 对 准 结 束 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%    精  对  准  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp（‘精对准开始‘）;
Data_2   = zeros（480006）;
Data_2（:1） = d_GyroAcc2012（12001:600001）;
Data_2（:2） = d_GyroAcc2012（12001:600002）;
Data_2（:3） = d_GyroAcc2012（12001:600003）;
Data_2（:4） = d_GyroAcc2012（12001:600004）;
Data_2（:5） = d_GyroAcc2012（12001:600005）;
Data_2（:6） = d_GyroAcc2012（12001:600006）;
%陀螺
W_2 = Data_2（:

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件   44535696  2011-05-22 16:30  NavigationProgram\d_GyroAcc2012.dat

     文件      12232  2013-12-26 16:05  NavigationProgram\INS_program_Ver2.m

     目录          0  2014-06-13 09:28  NavigationProgram

----------- ---------  ---------- -----  ----

             44547928                    3