自主导航课程捷联惯性导航跑车试验,粗对准 kalman滤波精对准 + 双子样捷联算法
代码片段和文件信息
属性 大小 日期 时间 名称
----------- --------- ---------- ----- ----
文件 44535696 2011-05-22 16:30 NavigationProgramd_GyroAcc2012.dat
文件 12232 2013-12-26 16:05 NavigationProgramINS_program_Ver2.m
目录 0 2014-06-13 09:28 NavigationProgram
----------- --------- ---------- ----- ----
44547928 3
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 NavigationProgramd_GyroAcc2012.dat
文件 12232 2013-12-26 16:05 NavigationProgramINS_program_Ver2.m
目录 0 2014-06-13 09:28 NavigationProgram
----------- --------- ---------- ----- ----
44547928 3
版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容, 请发送邮件举报,一经查实,本站将立刻删除。
评论列表(条)