sins_alignment.m

这是本人编写的一个接联惯性导航系统解析粗对准的仿真程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%the classical method of SINS alignment from 陈哲《截联惯导系统原理》
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all;
clear all;
clc;
%initialization of ideal angle
yaw_angle=pi*45/180;           %偏航角
pitching_angle=pi*30/180;      %俯仰角
turn_angle=pi*5/180;           %滚动角
latitude=pi*45/180;            %当地纬度
rot_earth=pi*15/180/3600;      %地球自转  单位：弧度每秒
g=9.806293866767001; 
T_simu=5e-3;     %采样时间
T_align=5;       %粗对准  对准时间
data_num=T_align/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_align1_error=Ctb_get_align1-Ctb_idea
Ctb_get_align2_error=Ctb_get_align2-Ctb_idea

%进行正交化处理，除去刻度系数误差和歪斜误差
%Ctb_get_align20=Ctb_get_align2*inv(sqrtm(Ctb_get_align2'*Ctb_get_align2))
%Cbt_get_align20=Ctb_get_align20';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%下面用粗对准对通过解析计算得到的捷联矩阵进行第一次修正
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fie_txsum=0;
fie_tysum=0;
fie_tzsum=0;
for i=1:data_num
    fcap_tcap=Ctb_get_align2*[g_b_measx(i);g_b_measy(i);g_b_measz(i)];
    wcap_tcap=Ctb_get_align2*[w_bie_measx(i);w_bie_measy(i);w_bie_measz(i)];
    fie_tx(i)=fcap_tcap(2)/g;
    fie_ty(i)=-fcap_tcap(1)/g;
    fie_tz(i)=wcap_tcap(1)/w_tiey+tan(latitude)*fie_ty(i);
    fie_txsum=fie_txsum+fie_tx(i);
    fie_tysum=fie_tysum+fie_ty(i);
    fie_tzsum=fie_tzsum+fie_tz(i);
end
%%%取平均值来减小误差
fie_txx=fie_txsum/data_num;
fie_tyy=fie_tysum/data_num;
fie_tzz=fie_tzsum/data_num;
%%%修正公式
Cttcap=[1 -fie_tzz fie_tyy;fie_tzz 1 -fie_txx;-fie_tyy fie_txx 1];

Ctb_first_modify1=Cttcap*Ctb_get_align1
Cbt_first_modify1=Ctb_first_modify1';

Ctb_first_modify2=Cttcap*Ctb_get_align2
Cbt_first_modify2=Ctb_first_modify2';

Ctb_first_error1=Ctb_first_modify1-Ctb_idea
Ctb_first_error2=Ctb_first_modify2-Ctb_idea

%正交化处理
%Ctb_first_modify0=Ctb_first_modify*inv(sqrtm(Ctb_first_modify'*Ctb_first_modify))
%Ctb_get_align2_error=Ctb_get_align2-Ctb_idea
%Ctb_first_error=Ctb_first_modify-Ctb_idea
%Ctb_first0_error=Ctb_first_modify0-Ctb_idea
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%利用一阶数字滤波器进行精对准
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%误差设置：加速度计的偏置量delta_a=1e-4*g
%随机干扰设为常值误差 a_randomerror=5e-5*g
%陀螺的常值漂移delta_w=0.02度每小时,随机干扰w_randomerror=0.01度每小时
%精对准动态特性：失准角误差带2％，取阻尼系数0.707，调节时间60s
%a1=0.750  w1=0.750 于是K_dx=0.15 K_x=0.0077
%a2=0.750  w2=0.750 于是K_dy=K_dz=0.15 K_y=0.0154 K_z=0.0031
%150s后fei_txxx=-1.5e-4;fei_tyyy=1.5e-4;fei_zzz=-2.84e-3
fie_txxx=-1.5e-4;
fie_tyyy=1.5e-4;
fie_tzzz=-2.84e-3;