www.gusucode.com > 惯性导航系统仿真程序 SIMULINK环境-SIMULINK源码程序 > code/compass_init.m

    clear all;
format long g;

disp( 'initializing...  ');
tic   
global R2D f re wie g0 gg eps
global Vn Vu Ve  cbn Q CC cnb
global lati0 longi0 h0 Lati Longi
global vepp0 veppa0 
global qa0 q0

global eulera0 

global LINGPIAN  PIAOYI  

%双子样参数  
global CNA Q sampt0

global r0 Lati0 Longi0 Lati Longi eeee Alti

%陀螺常值漂移1度/小时
    PIAOYI=[0  0  0]*(pi/180)/3600;
%加速度零偏1e-4g
    LINGPIAN=[0 0 0]*1e-4*9.8;   
  sampt0=0.01;
    
   

%标准参数 	
    R2D=180/pi;                                
    f=1/298.257;
    re=6378140;
    wie=7.292115e-5;
    g0=9.8;
    gg=5.2884e-3;
    eps=1.0e-10;
    g=9.8;

%初始位置 
%    (度 米)
    lati0=30;
    longi0=110;
    h0=0;
   
%    (弧度 米)
	lati0=lati0/R2D;
    lati0 = 0.49252506110179;
	longi0=longi0/R2D;
    Longi0 =1.9720639894; 
    h0=h0;
    Lati = lati0;
    Longi = longi0;
   
%主惯导初始速度veppa0(米/秒)
    v0=0;
    
    vepp0(1)=0;%v0*cos(yaw0);
    vepp0(2)=0;
    vepp0(3)=0;%-v0*sin(yaw0);
 
    veppa0=vepp0;
    
%主惯导初始姿态角(弧度)
% 	yawa0=10*pi/180;              %%定义初始偏航角
% 	pitcha0=0.1;%%%%%%%%%%%%%%lianzi030304     定义初始俯仰角
%     rolla0=0.2;%%%%%%%%%%%%%%lianzi030304       定义初始滚转角
   yawa0 = 0;
   pitcha0 = 0;
   rolla0 = 0;
    eulera0=[rolla0,yawa0,pitcha0];
    

%初始化主惯导四元数qa0
    pitch=pitcha0;
	yaw=yawa0;
    roll=rolla0;
      
    cnb=[cos(yaw)*cos(pitch),                             sin(pitch),          -sin(yaw)*cos(pitch);
        sin(yaw)*sin(roll)-cos(yaw)*sin(pitch)*cos(roll), cos(pitch)*cos(roll),cos(yaw)*sin(roll)+sin(yaw)*sin(pitch)*cos(roll);
        sin(yaw)*cos(roll)+cos(yaw)*sin(pitch)*sin(roll),-cos(pitch)*sin(roll),cos(yaw)*cos(roll)-sin(yaw)*sin(pitch)*sin(roll)];

    

    q0(1)=sqrt(1+cnb(1,1)+cnb(2,2)+cnb(3,3))/2;
    q0(2)=-(cnb(3,2)-cnb(2,3))/(4*q0(1));
    q0(3)=-(cnb(1,3)-cnb(3,1))/(4*q0(1));
    q0(4)=-(cnb(2,1)-cnb(1,2))/(4*q0(1));
    

    temp=sqrt(q0(1)*q0(1)+q0(2)*q0(2)+q0(3)*q0(3)+q0(4)*q0(4));
	q0(1)=q0(1)/temp;
	q0(2)=q0(2)/temp;
    q0(3)=q0(3)/temp;
    q0(4)=q0(4)/temp;

    %%%%%%%%%%%%%%lianzi030304
    cnb=[1 0 0;0 1 0;0 0 1];
    q0=[1 0 0 0];
    %%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    
    cna=cnb;
    qa0=q0;
    
     %双子样初值   
    CNA=cna*1;
    Q=qa0*1;
    
    
    %%初始对准的参数设置
    k1=0.1;
    k2=50;
    k3=0.06;
    k4=0.3;
    kz=8;
    
    
    %%外测偏航角
    yawa00=0*pi/180;
    
    %粗对准参数
    k=0.6;
    
   %粗对准和精对准的转换时间
    t=150;

D2R=180/pi; %%弧度转换为角度函数

Adtx0=0;   %%
Adty0=0;
delta_x0=0;
delta_y0=0;


Alpha0=300/60*R2D;
Beta0=300/60*R2D;
Gamma0=10*R2D;

global Sg dCbg bg Ng
global Sa dCba ba da Na
global I t Ka Kg
global C   % gama-表示绕x轴旋转，phi-表示绕y轴旋转，theta-表示绕z轴旋转
gama = 0; phi = 0 ; theta = 90 ;
phi = phi*pi/180;theta = theta * pi/180; gama = gama*pi/180;
C11 = cos(phi)*cos(theta); C12 = sin(theta); C13 = -sin(phi)*cos(theta);
C21 = -cos(phi)*sin(theta)*cos(gama)+sin(phi)*sin(gama);
C22 = cos(theta)*cos(gama);
C23 = sin(phi)*sin(theta)*cos(gama)+cos(phi)*sin(gama);
C31 = cos(phi)*sin(theta)*sin(gama)+sin(phi)*cos(gama);
C32 = -sin(gama)*cos(theta);
C33 = -sin(phi)*sin(theta)*sin(gama)+cos(phi)*cos(gama);
C = [C11 C12 C13;C21 C22 C23;C31 C32 C33];

I = eye(3);
t = 0;
% ba = [100 100 100]'; % ug
% ba = ba.*1e-6*g;
% Na = [5 5 5]'; % white noise, ug
% Na = Na.*1e-6*g;
% 
% bg = [1 1 1]'; % deg/h
% bg = bg.*pi/180/3600;
% Ng = [0.1 0.1 0.1]'; % white noise deg/h
% Ng = Ng.*pi/180/3600;
Ka = 10000;
ba = [50 50 50]'*1; % ug
Sa = diag([50 50 50]')*1; % ppm
dCba = [0 5 -5;-5 0 5;5 -5 0]*1; % 加表安装误差 角分
da = diag([5 5 5]); % 加表二次项系数 e-5*s2/m
Na = [5 5 5]'*1; % ug;

Kg = 10000;
bg = [0.1 0.1 0.1]'*10; % 陀螺零偏 deg/h
Sg = diag([50 50 50]')*1 ; % 陀螺刻度因子 ppm
dCbg = [0 5 -5;-5 0 5;5 -5 0]*1; %陀螺安装误差 1/60deg(角分)
Ng = [0.02 0.02 0.02]'*10; % 陀螺白噪声 deg/h

ba = ba.*1e-6*g; % m/s2
Sa = Sa.*1e-6;
dCba = dCba.*pi/180/60;
da = da.*1e-5;
Na = Na.*1e-6*g;

bg = bg.*pi/180/3600;
Sg = Sg.*1e-6;
dCbg = dCbg.*pi/180/60;
Ng = Ng.*pi/180/3600; % rad/s

disp( 'OK!  ');
%初始化完成
    disp( 'initialize OK!...  ');
    clc;

    
global frame wie frame sampt0
% global r0 Lati0 Longi0 Lati Longi eeee Alti
% global Vn Vu Ve  cbn Q CC cnb
global X A B K P Fn d_e d_v  ang_1 ang_2 vel_1 vel_2
global d_roll d_yaw d_pitch navi_resul

d_roll = 0;  d_yaw = 0;  d_pitch = 0;
navi_resul = zeros(1,8);
CC = zeros(2,10);
frame = 0.0;   eeee =  0.00669438000426;
sampt0 = 0.01;  r0 = 6378137.0;
Vn =  0; Vu =  0;Ve =  0;
Alti = 0; d_e=zeros(3,1);  d_v=zeros(2,1);
ang_1 = zeros(3,1);
ang_2 = zeros(3,1);
vel_1 = zeros(3,1);
vel_2 = zeros(3,1);
Lati0 = 0.49252506110179;
Longi0 =1.9720639894;
Lati = Lati0;
Longi = Longi0;
    
roll = 0*pi/180;
yaw = 0*pi/180;
pitch = 0*pi/180;

cnb=[cos(yaw)*cos(pitch),                             sin(pitch),          -sin(yaw)*cos(pitch);
        sin(yaw)*sin(roll)-cos(yaw)*sin(pitch)*cos(roll), cos(pitch)*cos(roll),cos(yaw)*sin(roll)+sin(yaw)*sin(pitch)*cos(roll);
        sin(yaw)*cos(roll)+cos(yaw)*sin(pitch)*sin(roll),-cos(pitch)*sin(roll),cos(yaw)*cos(roll)-sin(yaw)*sin(pitch)*sin(roll)];
    cbn = cnb';
    q0(1)=sqrt(1+cnb(1,1)+cnb(2,2)+cnb(3,3))/2;
    q0(2)=-(cnb(3,2)-cnb(2,3))/(4*q0(1));
    q0(3)=-(cnb(1,3)-cnb(3,1))/(4*q0(1));
    q0(4)=-(cnb(2,1)-cnb(1,2))/(4*q0(1));
    temp=sqrt(q0(1)*q0(1)+q0(2)*q0(2)+q0(3)*q0(3)+q0(4)*q0(4));
	q0(1)=q0(1)/temp;
	q0(2)=q0(2)/temp;
    q0(3)=q0(3)/temp;
    q0(4)=q0(4)/temp;
    Q = q0';