alignment.m

用于惯性导航的初始对准

function buffer_10
  % 北东地坐标系，10个状态量，分别为
  % 1.delta Vn      2.delta Ve      3.Phi n       4.Phi e         5.phi d
  % 6.Ax            7.Ay            8.Gx          9.Gy            10.Gz
  fidr(1)=fopen('jielianjiesuan.dat','r');
% fidr(2)=fopen('randomdata.dat','r');
  fidw(1)=fopen('filterresult.dat','w');
% 一些初始值的选取
  dt=0.25;    ns=10;    nm=2;   
% 我试着把这些过程噪声取大一些
  ta=7200.;            tg=7200.;       rv=1.e-3;
  
  qv=0.;    qt=9.4016e-015;   qa=0;  qg=0;
%   p33=3.046e-5;   p44=3.046e-5;    p55=7.615e-5;    p66=4.e-5;    
%   p77=4.e-5;      p88=9.4e-15;     p99=9.4e-15;     p1010=9.4e-15; 
%===================================================================================================================================
% 4.21早上修改
%   p33=3.046e-5;   p44=3.046e-5;    p55=7.615e-5;    p66=0.;    
%   p77=0.;      p88=9.4e-15;     p99=0.;     p1010=0.; 
  
  p33=3.6e-5;   p44=3.6e-5;    p55=3.6e-5;    p66=4.e-8;    
  p77=4.e-8;      p88=9.4016e-11;     p99=9.4016e-11;     p1010=9.4016e-11; 
%===================================================================================================================================
  erad=20925000.; eomega=7.292115e-5; agrav=32.174;
  xh=zeros(10,1);
  k=ns;
  
  svold(1) = 0;
  svold(2) = 0;
  svold(3) = 0;
  % 均方误差阵的初始化
  ph=zeros(10,10);
  ph(3,3) = p33;     ph(4,4) = p44;      ph(5,5) = p55;     ph(6,6) = p66;
  ph(7,7) = p77;     ph(8,8) = p88;      ph(9,9) = p99;     ph(10,10) = p1010; 
  % 噪声矩阵
  gqgt(1,1) = qv;     gqgt(2,2) = qv;     gqgt(3,3) = qt;
  gqgt(4,4) = qt;     gqgt(5,5) = qt;     gqgt(6,6) = qa;
  gqgt(7,7) = qa;     gqgt(8,8) = qg;     gqgt(9,9) = qg;
  gqgt(10,10)=qg;     % gqgt(12,12)=0.0;    
  rmat(1,1) = rv;     rmat(2,2) = rv;
  % 输入量，为3个位置，3个速度，捷联解算出的姿态矩阵
  for number=1:3200
  u=fscanf(fidr(1),'%f',24);
  
  slat = u(1);
  svgeo(1) = u(4);
  svgeo(2) = u(5);
  svgeo(3) = u(6);

  cbn(1,1) = u(7);
  cbn(1,2) = u(8);
  cbn(1,3) = u(9);
  cbn(2,1) = u(10);
  cbn(2,2) = u(11);
  cbn(2,3) = u(12);
  cbn(3,1) = u(13);
  cbn(3,2) = u(14);
  cbn(3,3) = u(15);
%
%   sinlat = sin(slat);
%   coslat = cos(slat);
%   tanlat = sinlat/coslat;

  sinlat = sin(slat);
  coslat = cos(slat);
  tanlat = sinlat/coslat;
%
  srho(1) =  svgeo(2)/erad;
  srho(2) = -svgeo(1)/erad;
  srho(3) = -svgeo(2)*tanlat/erad;
%      
  seomega(1) =  eomega*coslat;
  seomega(2) =  0.0;
  seomega(3) = -eomega*sinlat;
%      
  somega = srho + seomega;
%
  grav(1) = 0.0;
  grav(2) = 0.0;
  grav(3) = agrav;
%  
  sacc = (svgeo-svold)/dt + cross((srho+2.0*seomega),svgeo) - grav;   

%===================================================================================================================================

%   fmat(1,2) = - 2.0*eomega*sinlat;
%   fmat(1,4) =   agrav;
%   fmat(2,1) =   2.0*eomega*sinlat;
%   fmat(2,3) = - agrav;
%   fmat(3,4) = - eomega*sinlat;
%   fmat(4,3) =   eomega*sinlat;
%   fmat(4,5) =   eomega*coslat;
%   fmat(5,4) = - eomega*coslat;
  
  fmat(1,2) = - 2.0*eomega*sinlat;
  fmat(1,4) = - u(21);    
  fmat(1,5) =   u(20);   
  fmat(2,1) =   2.0*eomega*sinlat;
  fmat(2,3) =   u(21);  
  fmat(2,5) = - u(19); 
  fmat(3,2) =   1/erad;
  fmat(3,4) = - eomega*sinlat;
  fmat(4,1) = - 1/erad;  
  fmat(4,3) =   eomega*sinlat;
  fmat(4,5) =   eomega*coslat;
  fmat(5,2) = - 1/erad*tanlat;
  fmat(5,4) = - eomega*coslat;
  
  fmat(1,6) =   cbn(1,1);
  fmat(1,7) =   cbn(1,2);
  fmat(2,6) =   cbn(2,1);
  fmat(2,7) =   cbn(2,2);
 
  fmat(3,8) =  -cbn(1,1);
  fmat(3,9) =  -cbn(1,2);
  fmat(3,10) =  -cbn(1,3);
  fmat(4,8) =  -cbn(2,1);
  fmat(4,9) =  -cbn(2,2);
  fmat(4,10) =  -cbn(2,3);
  fmat(5,8) =  -cbn(3,1);
  fmat(5,9) =  -cbn(3,2);
  fmat(5,10) =  -cbn(3,3);
  
%   fmat(1,6) =   1;
%   fmat(1,7) =   0;
%   fmat(2,6) =   0;
%   fmat(2,7) =   1;
%  
%   fmat(3,8) =  1;
%   fmat(3,9) =  0;
%   fmat(3,10) =  0;
%   fmat(4,8) =  0;
%   fmat(4,9) =  1;
%   fmat(4,10) =  0;
%   fmat(5,8) =  0;
%   fmat(5,9) =  0;
%   fmat(5,10) =  1;
% 因为我加的是常值漂移
  fmat(6,6) = - 0.0/ta;
  fmat(7,7) = - 0.0/ta;
%
  fmat(8,8)= -0.0/tg;
  fmat(9,9)= -0.0/tg;
  fmat(10,10)= -0.0/tg;
  
  svold(1) = svgeo(1);
  svold(2) = svgeo(2);
  svold(3) = svgeo(3);
%===================================================================================================================================
% form the state transition matrix
     imat = eye(ns,ns);
     phim = imat + fmat*dt;%+0.5*fmat*dt*fmat*dt;
%      
% propagate covariance matrix
      pt = phim*ph*phim'+gqgt*dt;
%      
% propagate state vector
      xt = phim*xh;      
%
% measurement matrix
      hmat(1,1)=1.0;hmat(1,2)=0.0;hmat(1,3)=0.0;hmat(1,4)=0.0;hmat(1,5)=0.0;hmat(1,6)=0.0;hmat(1,7)=0.0;hmat(1,8)=0.0;hmat(1,9)=0.0;hmat(1,10)=0.0;
      hmat(2,1)=0.0;hmat(2,2)=1.0;hmat(2,3)=0.0;hmat(2,4)=0.0;hmat(2,5)=0.0;hmat(2,6)=0.0;hmat(2,7)=0.0;hmat(2,8)=0.0;hmat(2,9)=0.0;hmat(2,10)=0.0;    
%
% compute residual covariance matrix
      alpha = hmat*pt*hmat' + rmat;
%
% compute observation vector
      zvec(1,1) = svgeo(1) - u(16);
      zvec(2,1) = svgeo(2) - u(17);
%
% compute residual vector
      resid = zvec - hmat*xt;
%
% compute gain matrix
      kmat = pt*hmat'*inv(alpha);
%
% update error covariance matrix - Joseph's form
%      ph = (imat-kmat*hmat)*pt*(imat-kmat*hmat)'+kmat*rmat*kmat';
% update error covariance matrix - Standard Kalman form      
      ph = (imat-kmat*hmat)*pt;
%
% update state vector
      xh = xt + kmat*resid;
%
  x(1) = xh(1,1);
  x(2) = xh(2,1);
  x(3) = xh(3,1);
  x(4) = xh(4,1);
  x(5) = xh(5,1);
  x(6) = xh(6,1);
  x(7) = xh(7,1);
  x(8) = xh(8,1);
  x(9) = xh(9,1);
  x(10) = xh(10,1);

%   kk=mod(number,10);
%   if(kk==0)
  fprintf(fidw(1),'%20.8f  ',number*dt); 
  fprintf(fidw(1),'%20.8f  ',sqrt(ph(1,1)));
  fprintf(fidw(1),'%20.8f  ',sqrt(ph(2,2)));
  fprintf(fidw(1),'%20.8f  ',x(3)*57.296);
  fprintf(fidw(1),'%20.8f  ',x(4)*57.296);
  fprintf(fidw(1),'%20.8f  ',x(5)*57.296);

  fprintf(fidw(1),'%20.8f  ',x(3));
  fprintf(fidw(1),'%20.8f  ',u(22));
  fprintf(fidw(1),'%20.8f  ',x(3)+sqrt(ph(3,3)));
  fprintf(fidw(1),'%20.8f  ',x(3)-sqrt(ph(3,3)));
  fprintf(fidw(1),'%20.8f  ',x(4));
  fprintf(fidw(1),'%20.8f  ',u(24));
  fprintf(fidw(1),'%20.8f  ',x(4)+sqrt(ph(4,4)));
  fprintf(fidw(1),'%20.8f  ',x(4)-sqrt(ph(4,4)));
  fprintf(fidw(1),'%20.8f  ',x(5));
  fprintf(fidw(1),'%20.8f  ',u(23));
  fprintf(fidw(1),'%20.8f  ',x(5)+sqrt(ph(5,5)));
  fprintf(fidw(1),'%20.8f  ',x(5)-sqrt(ph(5,5)));
  
  fprintf(fidw(1),'%20.8f  ',sqrt(ph(3,3))*57.296);
  fprintf(fidw(1),'%20.8f  ',sqrt(ph(4,4))*57.296);
  fprintf(fidw(1),'%20.8f  ',sqrt(ph(5,5))*57.296);
  fprintf(fidw(1),'%20.8f  ',sqrt(ph(6,6)));
  fprintf(fidw(1),'%20.8f  ',sqrt(ph(7,7)));
  fprintf(fidw(1),'%20.8f  ',sqrt(ph(8,8)));
  fprintf(fidw(1),'%20.8f  ',sqrt(ph(9,9)));
  fprintf(fidw(1),'%20.8f  ',sqrt(ph(10,10)));
  fprintf(fidw(1),'%20.10f  ',x(8));
  fprintf(fidw(1),'%20.10f  ',x(9));
  fprintf(fidw(1),'%20.14f  ',x(10));
  fprintf(fidw(1),'\n');
end
fclose(fidr(1));
fclose(fidw(1));