nav_data_s1017.m

自己编写的kalman滤波算法的应用

function nav_data_s1017(psg,psa)
%
%%%
%% 三通道的系统仿真


%％％％％坐标系为 东北天 坐标系％％％％％％％


%%navigation data generation
clc;
%clear;

% let's assume the simulation step number:
load para_m;

%maxcount=5*60*20;

%the sample rate:(s) 

%Ts=0.05;

%define In=eye(n);
I3=eye(3);

% the earth radius
 R=6378137.0;
 
 %the g
 g=9.80;
 
 % the initial height
 h0=0.0;
 
 % the earth angle rotation rate
 o_miga=360.0/24*pi/180/3600;
 
 % the initial latitude
 fai0=45.0*pi/180;
 
 % the initial longitude
 l0=100.0*pi/180;
 
 % the initial speed of the ship
 ve=pve;
 vn=pvn;
 vu=0.0;
 
 v0=[ve;vn;vu];
 
 %the initial speed increment
 dve=0.0;
 dvn=0.0;
 dvu=0.0;
 
 dv_l0=0;
 %the initial angle rate
 
 we=0;
 wn=o_miga*cos(fai0);
 wu=o_miga*sin(fai0);
  
 % the initial angle increment
 dseite=0;
 dseitn=0;
 dseitu=0;
   
 % the initial acceleration 
%  ae=0;
%  an=0;
%  au=0;
 
 %the initial bad acceleration
 bad_a=[0;0;0];
 
 %the initial matrix of p-r-a
 %C_l_b=[    1    , -1*pi/180, 1*pi/180;
 %            1*pi/180  ,    1,  -1*pi/180;
 %            -1*pi/180,    1*pi/180,   1];
 
 
 C_l_b=eye(3);%+delta;
 C_b_l=C_l_b';
 
 % acording to the above matrix, we can get the inintial value of Q0
 
 q10=0.5*sqrt(1+C_b_l(1,1)+C_b_l(2,2)+C_b_l(3,3));
 q20=0.25*(C_b_l(3,2)-C_b_l(2,3))/q10;
 q30=0.25*(C_b_l(1,3)-C_b_l(3,1))/q10;
 q40=0.25*(C_b_l(2,1)-C_b_l(1,2))/q10;
 
 q0=[q20;q30;q40;q10];
 
 %C_l_b=eye(3);
 %the initial matrix of position 初始位置矩阵
 
 C_e_l=[      -sin(l0)         ,          cos(l0)            ,        0      ; 
         -sin(fai0)*cos(l0)    ,    -sin(fai0)*sin(l0)       ,    cos(fai0)  ;
           cos(fai0)*cos(l0)   ,     cos(fai0)*sin(l0)       ,     sin(fai0)      ];
 
 %define In-eye(n)
 I4=eye(4);
 I3=eye(3);
 
 %gnerate the matrix that raw data of navigation save,  it contains: delta_V, delta_seit; 
 
 dv1=linspace(0,1,maxcount);      %用于保存采样时间内的速度增量
 dv2=linspace(0,1,maxcount);
 dv3=linspace(0,1,maxcount);
 
 dseit1=linspace(0,1,maxcount);   %用于保存采样时间内的角度增量
 dseit2=linspace(0,1,maxcount);
 dseit3=linspace(0,1,maxcount);
 
 
 w1=linspace(0,1,maxcount);       % 采样保存采样时间内的角速度；
 w2=linspace(0,1,maxcount);
 w3=linspace(0,1,maxcount);
 
 c_matrix=linspace(0,1,maxcount);
 
 for i=1:maxcount
     dv1(i)=0;
     dv2(i)=0;
     dv3(i)=0;
     
     dseit1(i)=0;
     dseit2(i)=0;
     dseit3(i)=0;
     
     w1(i)=0;
     w2(i)=0;
     w3(i)=0;
     
     c_matrix(i)=0;
 end
 
 
 dv1(1)=dve;                     %初始化
 dv2(1)=dvn;
 dv3(1)=dvu;
 
 dseit1(1)=dseite;
 dseit2(1)=dseitn;
 dseit3(1)=dseitu;
 
 
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %保存输出姿态角矩阵；
 roll=linspace(0,1,maxcount);
 pitch=linspace(0,1,maxcount);
 yaw=linspace(0,1,maxcount);
 
 %保存输出速度矩阵：
%  vout_e=linspace(0,1,maxcount);
%  vout_n=linspace(0,1,maxcount);
%  vout_u=linspace(0,1,maxcount);
 
 %保存输出的位置矩阵
 sout_e=linspace(0,1,maxcount);
 sout_n=linspace(0,1,maxcount);
 sout_e0=0;
 sout_n0=0;
 %
 
 
 %%%%%%%%%以下的部分是为了补偿划船误差
%  temp_seit_huachuan=[0;0;0];
%  temp_v_huachuan=[0;0;0];
 %%%%%%%%%
 
 % 注意，此处产生的原始数据都是在导航坐标系里面的数据，即东北天坐标系里面的数据
 % 也就是原始的惯性元件的测量数据在导航坐标系里面的投影
 
 %注意此处假设在采用时间间隔内加速度和角速度都为常数
 
 %m_C_l_b=[    1    , -1*pi/180, 2*pi/180;
 %            1*pi/180  ,    1,  -2*pi/180;
 %            -2*pi/180,    2*pi/180,   1];
         
 %m_C_b_l=m_C_l_b';
 
 %%%%%%%%%%%% The following code was inerted on 2003,9,26 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %原始量测数据数据产生
 
 %摇摆模型的参数
 
 delta=[0, -2*pi/180, 1*pi/180;
     2*pi/180,  0   ,  -1*pi/180;
     -1*pi/180,  1*pi/180,  0];


% the initial acceleration 
 ae=pae;
 an=pan;
 au=-g;
 
 fai_y=pfai_y*pi/180;
 fai_p=pfai_p*pi/180;
 fai_r=pfai_r*pi/180;
 
 y_m=py_m*pi/180;
 p_m=pp_m*pi/180;
 r_m=pr_m*pi/180;
 
 omiga_y=2*pi*pomiga_y;
 omiga_p=2*pi*pomiga_p;
 omiga_r=2*pi*pomiga_r;

 %%陀螺漂移和加速度计零偏
  Ex=psg*pi/180/3600;
  Ey=psg*pi/180/3600;
  Ez=psg*pi/180/3600;
 
  Ax=psa*0.0001*g;
  Ay=psa*0.0001*g;
  Az=psa*0.0001*g;
 %%%%%%
 for i=1:maxcount
     
     %有害加速度的计算；
     
     %v0=[ve;vn;vu];
     
     bad=[                0,                               -(2.0*o_miga+ve/((R+h0)*cos(fai0)))*sin(fai0),   (2.0*o_miga+ve/((R+h0)*cos(fai0)))*cos(fai0);
          (2.0*o_miga+ve/((R+h0)*cos(fai0)))*sin(fai0),                             0,                                 -vn/(R+h0)                  ;
          -(2.0*o_miga+ve/((R+h0)*cos(fai0)))*cos(fai0),                             vn/(R+h0),                              0                      ];
 
     bad_a=bad*[ve;vn;vu] ; %有害加速度
     
     f=C_l_b*([ae;an;au]+bad_a);
     
     deltaVe(i)=(f(1)+Ax+0.0005*g*rand(1,1))*Ts;
     deltaVn(i)=(f(2)+Ay+0.0005*g*rand(1,1))*Ts;
     deltaVu(i)=(f(3)+Az+0.0005*g*rand(1,1))*Ts;
     
     %%%% 保存载体坐标系内的加速度计输出的原始加速度信息2003。10。8  %%%%
     aout_e(i)=deltaVe(i)/Ts;
     aout_n(i)=deltaVn(i)/Ts;
     aout_u(i)=deltaVu(i)/Ts;
     %%%%%%%%
     
     v=v0+[ae;an;au+g]*Ts;
     %s=s0+v*Ts;
     fai=fai0+v(2)/(R+h0)*Ts;
         
         
     y(i)=y_m*sin(omiga_y*i*Ts+fai_y);
     p(i)=p_m*sin(omiga_p*i*Ts+fai_p);
     r(i)=r_m*sin(omiga_r*i*Ts+fai_r);
     
     dy(i)=y_m*omiga_y*cos(omiga_y*i*Ts+fai_y);
     dp(i)=p_m*omiga_p*cos(omiga_p*i*Ts+fai_p);
     dr(i)=r_m*omiga_r*cos(omiga_r*i*Ts+fai_r);
     
%      C_b_l=[cos(r(i))*sin(y(i))+sin(r(i))*sin(p(i))*cos(y(i)), cos(p(i))*cos(y(i)),      sin(r(i))*sin(y(i))-cos(r(i))*sin(p(i))*cos(y(i));
%            cos(r(i))*cos(y(i))-sin(r(i))*sin(p(i))*sin(y(i)),    -cos(p(i))*sin(y(i)),    sin(r(i))*cos(y(i))+cos(r(i))*sin(p(i))*sin(y(i));
%            -sin(r(i))*cos(p(i)),                                   sin(p(i)),                        cos(r(i))*cos(p(i)) ];
    
    C_l_b=[cos(r(i))*cos(y(i))-sin(r(i))*sin(p(i))*sin(y(i)),   cos(r(i))*sin(y(i))+sin(r(i))*sin(p(i))*cos(y(i)),       -sin(r(i))*cos(p(i));
             -cos(p(i))*sin(y(i)),                                           cos(p(i))*cos(y(i)),                       sin(p(i));             
             sin(r(i))*cos(y(i))+cos(r(i))*sin(p(i))*sin(y(i)),            sin(r(i))*sin(y(i))-cos(r(i))*sin(p(i))*cos(y(i)), cos(r(i))*cos(p(i))]; 
    C_l_b=C_l_b+delta;
   
    C_b_l=C_l_b';
    
    %%%%%%%%2003.12.3
%     Cb=[cos(r(i)), 0,  sin(r(i))*cos(p(i));
%           0,       1,        -sin(p(i));
%        sin(r(i)),  0,  cos(p(i))*cos(r(i))]+delta;
%    
%     omiga_b=Cb*[dp(i);dr(i);dy(i)];
%    
    
    omiga_b=C_l_b*[dp(i);dr(i);dy(i)];
    
    %%%%%%%%
    
    ve=v(1);
    vn=v(2);
    vu=v(3);
    
    faint(i)=v(1);
    
    omiga_bb=omiga_b+C_l_b*([0;o_miga*cos(fai);o_miga*sin(fai)]+[-vn/(R+h0);ve/(R+h0);ve/(R+h0)*tan(fai)]);
    
    deltaSe(i)=(omiga_bb(1)+Ex+0.05*pi/180/3600*rand(1,1))*Ts;
    deltaSn(i)=(omiga_bb(2)+Ey+0.05*pi/180/3600*rand(1,1))*Ts;
    deltaSu(i)=(omiga_bb(3)+Ez+0.05*pi/180/3600*rand(1,1))*Ts;
    
    origratee(i)=deltaSe(i)/Ts;
    origraten(i)=deltaSn(i)/Ts;
    origrateu(i)=deltaSu(i)/Ts;
      
    v0=v;
    fai0=fai;
     
 end
 %figure;
 
 %plot(faint);