simulation.m

捷联惯导系统传递对准MATLAB程序

clear all;
clc;
g0=9.78049;
rad_deg=0.01745329;
wie=7.27220417e-5;
Re=6378393.0;
e=3.3670e-3;
Hn=0.1;
time=100;
G_Drift=[0.01*rad_deg/3600   
    0.01*rad_deg/3600  
    0.01*rad_deg/3600];
A_Bias=[1e-4*g0     
    1e-4*g0    
    1e-4*g0];
%%%%%%
T_p=3;
T_r=5;
T_y=7;
%%%%%%
pitchm=10*rad_deg;
rollm=10*rad_deg;
yawm=10*rad_deg;
%%%%初始角%%
pitchk=0*rad_deg;
rollk=0*rad_deg;
yawk=0*rad_deg;
%%%%
% p_pitch=30*rad_deg;
% p_roll=30*rad_deg;
% p_yaw=30*rad_deg;
p_pitch=0*rad_deg;
p_roll=0*rad_deg;
p_yaw=30*rad_deg;
%%%初始误差角%%
pitch_error=1*rad_deg;
roll_error=1*rad_deg;
yaw_error=1*rad_deg;
%%% n对应主惯导
lati=45.7796*rad_deg;
longi=126.6705*rad_deg;
%%%p对子惯导
phi=45.7796*rad_deg;
lamda=126.6705*rad_deg;
wiep=[0
    wie*cos(phi)
    wie*sin(phi)];
g1=g0+0.051799*(sin(phi))^2;
%%%%%% 初始姿态角%%%%
pitch0=pitchm*sin(p_pitch)+pitchk;
roll0=rollm*sin(p_roll)+rollk;
yaw0=yawm*sin(p_yaw)+yawk;
%%%%%
sea_mile=1.852e3/3600;
a0=0.1*g0;
aold=[a0*sin(yaw0);a0*cos(yaw0);0];
v0=[10.0*sea_mile*sin(yaw0);10.0*sea_mile*cos(yaw0);0.0];
v=v0;
a=aold;

Rxp=Re*(1+e*(sin(phi))^2);
Ryp=Re*(1-2*e+3*e*(sin(phi))^2);
wepp=[-v(2)/Ryp
    v(1)/Rxp
    v(1)*tan(phi)/Rxp];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
q=zeros(4,1);
%%%%%%建立 初始 捷 联阵
%%%%%%%%%
Tbn=[cos(roll0)*cos(yaw0) + sin(roll0)*sin(pitch0)*sin(yaw0)     cos(pitch0)*sin(yaw0)        sin(roll0)*cos(yaw0) - cos(roll0)*sin(pitch0)*sin(yaw0)
    -cos(roll0)*sin(yaw0) + sin(roll0)*sin(pitch0)*cos(yaw0)    cos(pitch0)*cos(yaw0)        -sin(roll0)*sin(yaw0) - cos(roll0)*sin(pitch0)*cos(yaw0)
    -sin(roll0)*cos(pitch0)                                        sin(pitch0)                    cos(roll0)*cos(pitch0)];

T(1,1) = cos(roll0+roll_error)*cos(yaw0+yaw_error) + sin(roll0+roll_error)*sin(pitch0+pitch_error)*sin(yaw0+yaw_error);
T(1,2) = cos(pitch0+pitch_error)*sin(yaw0+yaw_error);
T(1,3) = sin(roll0+roll_error)*cos(yaw0+yaw_error) - cos(roll0+roll_error)*sin(pitch0+pitch_error)*sin(yaw0+yaw_error);

T(2,1) = -cos(roll0+roll_error)*sin(yaw0+yaw_error) + sin(roll0+roll_error)*sin(pitch0+pitch_error)*cos(yaw0+yaw_error);
T(2,2) = cos(pitch0+pitch_error)*cos(yaw0+yaw_error);
T(2,3) = -sin(roll0+roll_error)*sin(yaw0+yaw_error) - cos(roll0+roll_error)*sin(pitch0+pitch_error)*cos(yaw0+yaw_error);

T(3,1) = -sin(roll0+roll_error)*cos(pitch0+pitch_error);
T(3,2) = sin(pitch0+pitch_error);
T(3,3) = cos(roll0+roll_error)*cos(pitch0+pitch_error);

% Tsm=[1          yaw_error       -roll_error;
%     -yaw_error       1             pitch_error;
%     roll_error     -pitch_error    1];
% Tms=inv(Tsm);
% T=Tbn;

Tsm=inv(Tbn)*T;
Tms=inv(Tsm);
dp=Tms(2,3)/rad_deg*60
dr=Tms(3,1)/rad_deg*60
dy=Tms(1,2)/rad_deg*60
T=Tbn;
q(1) = sqrt(1+T(1,1)+T(2,2)+T(3,3))/2.0;
q(2) = (T(3,2)-T(2,3))/(4*q(1));
q(3) = (T(1,3)-T(3,1))/(4*q(1));
q(4) = (T(2,1)-T(1,2))/(4*q(1));
Tn=time/Hn;

X=[0;
    0;
    0;
    0; 
    0;
    0;
    0;
    0
    0];

P = zeros(9);
P(1,1) = 0.1^2;
P(2,2) = 0.1^2; 
P(3,3) = 0.1^2;
P(4,4) = 0.1^2;
P(5,5) = 0.2^2;
P(6,6) = 0.1^2;
P(7,7) = 0.1^2;
P(8,8) = 0.1^2;
P(9,9) = 0.05^2;
 
% % P = zeros(9);
% % P(1,1) = 0.1^2;
% % P(2,2) = 0.1^2; 
% % P(3,3) = (1*rad_deg)^2;
% % P(4,4) = (1*rad_deg)^2;
% % P(5,5) = (1*rad_deg)^2;
% % P(6,6) = (1*rad_deg)^2;
% % P(7,7) = (1*rad_deg)^2;
% % P(8,8) = (1*rad_deg)^2;
% % P(9,9) = 0.005^2;

Q=zeros(9);
Q(1,1) = power(A_Bias(1),2);
Q(2,2) = power(A_Bias(2),2);
Q(3,3) = power(G_Drift(1),2);
Q(4,4) = power(G_Drift(2),2);
Q(5,5) = power(G_Drift(3),2);
Q(6,6) = power(G_Drift(1),2);
Q(7,7) = power(G_Drift(2),2);
Q(8,8) = power(G_Drift(3),2);
Q(9,9) = power(0.0002,2);
R=zeros(5);
R(1,1) = power(0.0001,2);
R(2,2) = power(0.0001,2);
R(3,3) = power(0.0001*rad_deg,2);
R(4,4) = power(0.0001*rad_deg,2);
R(5,5) = power(0.0001*rad_deg,2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=1:Tn
    for i=1:3
        pitchT(i) = pitchm*sin(2*pi*(k-(3-i)/2)*Hn/T_p+p_pitch)+pitchk;
        rollT(i)  = rollm*sin(2*pi*(k-(3-i)/2)*Hn/T_r+p_roll)+rollk;
        yawT(i)   = yawm*sin(2*pi*(k-(3-i)/2)*Hn/T_y+p_yaw)+yawk;
        
        wpT(i) = 2*pi/T_p*pitchm*cos(2*pi*(k-(3-i)/2)*Hn/T_p+p_pitch);
        wrT(i) = 2*pi/T_r*rollm*cos(2*pi*(k-(3-i)/2)*Hn/T_r+p_roll);
        wyT(i) = 2*pi/T_y*yawm*cos(2*pi*(k-(3-i)/2)*Hn/T_y+p_yaw);
%         
%         wpT2(i)=((2*pi/T_p)^2)*pitchm*cos(2*pi*(k-(3-i)/2)*Hn/T_p+p_pitch);
%         wrT2(i)=((2*pi/T_r)^2)*rollm*cos(2*pi*(k-(3-i)/2)*Hn/T_r+p_roll);
%         wyT2(i)=((2*pi/T_y)^2)*yawm*cos(2*pi*(k-(3-i)/2)*Hn/T_y+p_yaw);
    end
    delay_time(k)=rand/10;
    delay_pitchT = pitchm*sin(2*pi/T_p*(k*Hn-delay_time(k))+p_pitch)+pitchk;
    delay_rollT  = rollm*sin(2*pi/T_r*(k*Hn-delay_time(k))+p_roll)+rollk;
    delay_yawT   = yawm*sin(2*pi/T_y*(k*Hn-delay_time(k))+p_yaw)+yawk;
    
    %     Tbn=[cos(rollT(3))*cos(yawT(3)) + sin(rollT(3))*sin(pitchT(3))*sin(yawT(3))     cos(pitchT(3))*sin(yawT(3))   sin(rollT(3))*cos(yawT(3)) - cos(rollT(3))*sin(pitchT(3))*sin(yawT(3))
    %         -cos(rollT(3))*sin(yawT(3)) + sin(rollT(3))*sin(pitchT(3))*cos(yawT(3))    cos(pitchT(3))*cos(yawT(3))     -sin(rollT(3))*sin(yawT(3)) - cos(rollT(3))*sin(pitchT(3))*cos(yawT(3))
    %         -sin(rollT(3))*cos(pitchT(3))                                              sin(pitchT(3))                  cos(rollT(3))*cos(pitchT(3))];
    
    Tbn=[cos(delay_rollT)*cos(delay_yawT) + sin(delay_rollT)*sin(delay_pitchT)*sin(delay_yawT)     cos(delay_pitchT)*sin(delay_yawT)   sin(delay_rollT)*cos(delay_yawT) - cos(delay_rollT)*sin(delay_pitchT)*sin(delay_yawT)
        -cos(delay_rollT)*sin(delay_yawT) + sin(delay_rollT)*sin(delay_pitchT)*cos(delay_yawT)    cos(delay_pitchT)*cos(delay_yawT)     -sin(delay_rollT)*sin(delay_yawT) - cos(delay_rollT)*sin(delay_pitchT)*cos(delay_yawT)
        -sin(delay_rollT)*cos(delay_pitchT)                                              sin(delay_pitchT)                  cos(delay_rollT)*cos(delay_pitchT)];
    
    tempp1(k)=pitchT(3);
    tempp2(k)=delay_pitchT;
    
    for i=1:3
        wnbb(1,i) =  sin(rollT(i))*cos(pitchT(i))*wyT(i) + cos(rollT(i))*wpT(i);
        wnbb(2,i) = -sin(pitchT(i))*wyT(i) + wrT(i);
        wnbb(3,i) = -cos(rollT(i))*cos(pitchT(i))*wyT(i) + sin(rollT(i))*wpT(i);
    end    
    wnbb1=[wnbb(1,1)
        wnbb(2,1)
        wnbb(3,1)];
    wnbb2=[wnbb(1,2)
        wnbb(2,2)
        wnbb(3,2)];
    wnbb3=[wnbb(1,3)
        wnbb(2,3)
        wnbb(3,3)];
    %%%%%%%%%%%%%%%%%%%%%     
    Rxt = Re*(1+e*sin(lati)*sin(lati));
    Ryt = Re*(1-2*e+3*e*sin(lati)*sin(lati));
    %%%%%%%%%%%%%%%%%%%  
    v00=v0;
    v0(1) = v0(1)+a(1)*Hn;
    v0(2) = v0(2)+a(2)*Hn;
    
    wenn(1,1) = -v0(2)/Ryt;
    wenn(2,1) = v0(1)/Rxt;
    wenn(3,1) = v0(1)*tan(lati)/Rxt;
    
    %%%%%%%%%%%%
    longi= longi+(v00(1)+v0(1))/2*Hn/(Rxt*cos(lati));
    lati = lati+(v00(2)+v0(2))/2*Hn/Ryt;
    wien=[0;
        wie*cos(lati);
        wie*sin(lati)];
    
    winn=wien+wenn;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%555  
    
    for i=1:3
        Tnb11(i) = cos(rollT(i))*cos(yawT(i)) + sin(rollT(i))*sin(pitchT(i))*sin(yawT(i));
        Tnb12(i) = -cos(rollT(i))*sin(yawT(i)) + sin(rollT(i))*sin(pitchT(i))*cos(yawT(i));
        Tnb13(i) = -sin(rollT(i))*cos(pitchT(i));
        
        Tnb21(i) = cos(pitchT(i))*sin(yawT(i));
        Tnb22(i) = cos(pitchT(i))*cos(yawT(i));
        Tnb23(i) = sin(pitchT(i));
        
        Tnb31(i) = sin(rollT(i))*cos(yawT(i)) - cos(rollT(i))*sin(pitchT(i))*sin(yawT(i));
        Tnb32(i) = -sin(rollT(i))*sin(yawT(i)) - cos(rollT(i))*sin(pitchT(i))*cos(yawT(i));
        Tnb33(i) = cos(rollT(i))*cos(pitchT(i));
    end