analytic_coarse_alignment_methods.m

这是一个本人编写的一个接联惯性导航系统解析粗对准程序可观测性分析的一个程序。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Analytic Coarse Alignment Methods
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all;
clear all;
%initialization of ideal angle
yaw_angle=pi*0/180;       %偏航角
pitching_angle=pi*0/180;  %俯仰角
turn_angle=pi*0/180;      %滚动角
latitude=pi*45/180;       %当地纬度
rot_earth=pi*15/180/3600;      %地球自转
g=9.806293866767001;
T_simu=5e-3;    %采样时间
t_simu=5;       %对准时间
data_num=t_simu/T_simu;
%calculate of ideal strapdown_matrix from chenzhe
Cbt_idea11=cos(turn_angle)*cos(yaw_angle)-sin(turn_angle)*sin(pitching_angle)*sin(yaw_angle);
Cbt_idea12=cos(turn_angle)*sin(yaw_angle)+sin(turn_angle)*sin(pitching_angle)*cos(yaw_angle);
Cbt_idea13=-sin(turn_angle)*cos(pitching_angle);
Cbt_idea21=-cos(pitching_angle)*sin(yaw_angle);
Cbt_idea22=cos(pitching_angle)*cos(yaw_angle);
Cbt_idea23=sin(pitching_angle);
Cbt_idea31=sin(turn_angle)*cos(yaw_angle)+cos(turn_angle)*sin(pitching_angle)*sin(yaw_angle);
Cbt_idea32=sin(turn_angle)*sin(yaw_angle)-cos(turn_angle)*sin(pitching_angle)*cos(yaw_angle);
Cbt_idea33=cos(turn_angle)*cos(pitching_angle);
Cbt_idea=[Cbt_idea11 Cbt_idea12 Cbt_idea13
          Cbt_idea21 Cbt_idea22 Cbt_idea23
          Cbt_idea31 Cbt_idea32 Cbt_idea33];
Ctb_idea=Cbt_idea'      
%output of n(t)
g_tx=0;
g_ty=0;
g_tz=-g;
g_t=[g_tx g_ty g_tz]';
w_tiex=0;
w_tiey=rot_earth*cos(latitude);
w_tiez=rot_earth*sin(latitude);
w_tie=[w_tiex w_tiey w_tiez]';
r_t=[g*w_tiey 0 0]';
rr_t=[0 -r_t(3) r_t(2);r_t(3) 0 -r_t(1);-r_t(2) r_t(1) 0]*g_t;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%output of b 理想情况下。
w_bie=Cbt_idea*w_tie;
g_b=Cbt_idea*g_t;
%陀螺的误差设置：陀螺的常值漂移delta_w=0.02度每小时,随机干扰w_randomerror=0.01度每小时
delta_w=pi*0.02/180/3600;
w_randomerror=pi*0.01/180/3600*randn(3000,1);
%加速度计的误差设置：加速度计的偏置量delta_a=1e-4*g,随机干扰a_randomerror=(5e-5*randn(3000,1))*g
delta_a=1e-4*g;
a_randomerror=(5e-5*randn(3000,1))*g;
%陀螺和加速度计的实际测量值 data_num=1000
w_bie_measum=[0 0 0]';
g_b_measum=[0 0 0]';
for i=1:data_num 
    w_bie_measx(i)=w_bie(1)+delta_w+w_randomerror(i);
    w_bie_measy(i)=w_bie(2)+delta_w+w_randomerror(i+1000);
    w_bie_measz(i)=w_bie(3)+delta_w+w_randomerror(i+2000);
    w_bie_measum(1)=w_bie_measum(1)+w_bie_measx(i);
    w_bie_measum(2)=w_bie_measum(2)+w_bie_measy(i);
    w_bie_measum(3)=w_bie_measum(3)+w_bie_measz(i);
    
    g_b_measx(i)=g_b(1)+delta_a+a_randomerror(i);
    g_b_measy(i)=g_b(2)+delta_a+a_randomerror(i+1000);
    g_b_measz(i)=g_b(3)+delta_a+a_randomerror(i+2000);
    g_b_measum(1)=g_b_measum(1)+g_b_measx(i);
    g_b_measum(2)=g_b_measum(2)+g_b_measy(i);
    g_b_measum(3)=g_b_measum(3)+g_b_measz(i);
end
w_bie_meas(1)=w_bie_measum(1)/data_num;
w_bie_meas(2)=w_bie_measum(2)/data_num;
w_bie_meas(3)=w_bie_measum(3)/data_num;
w_bie_meas=[w_bie_meas(1) w_bie_meas(2) w_bie_meas(3)]';
g_b_meas(1)=g_b_measum(1)/data_num;
g_b_meas(2)=g_b_measum(2)/data_num;
g_b_meas(3)=g_b_measum(3)/data_num;
g_b_meas=[g_b_meas(1) g_b_meas(2) g_b_meas(3)]';
r_b(1)=w_bie_meas(3)*g_b_meas(2)-w_bie_meas(2)*g_b_meas(3);
r_b(2)=w_bie_meas(1)*g_b_meas(3)-w_bie_meas(3)*g_b_meas(1);
r_b(3)=w_bie_meas(2)*g_b_meas(1)-w_bie_meas(1)*g_b_meas(2);
r_b=[r_b(1) r_b(2) r_b(3)]';
rr_b=[0 -r_b(3) r_b(2);r_b(3) 0 -r_b(1);-r_b(2) r_b(1) 0]*g_b_meas;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%the establish of initial strapdown_matrix1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%初始捷联矩阵的建立方案一(经典方法 from《捷联惯导系统原理》 陈哲）
%Ctb_get_align1=inv([g_t';w_tie';r_t'])*[g_b_meas';w_bie_meas';r_b'];
%Cbt_get_align1=Ctb_get_align1'
%初始捷联矩阵的建立方案二（from 捷联惯导系统粗对准方法比较 魏春岭 张洪越）
Ctb_get_align2=inv([g_t';r_t';rr_t'])*[g_b_meas';r_b';rr_b']
Cbt_get_align2=Ctb_get_align2';
%进行正交化处理，除去刻度系数误差和歪斜误差
Ctb_get_align20=Ctb_get_align2*inv(sqrtm(Ctb_get_align2'*Ctb_get_align2))
Cbt_get_align20=Ctb_get_align20';
%误差分析
Ctb_get_align_error=eye(3)-Ctb_get_align20*Cbt_idea