sculling.m

组合导航的程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 Re = 6378393.0;
 pi = 3.1415926;
 Rad_Degree = 0.01745329; 
 wie = 7.2921158e-5;
 latitude = 45.7796*Rad_Degree;
 longtitude = 126.6705* Rad_Degree;
 
 h1 = 0.001;%standard sample time
 h2 = 0.01;
 v_r = [0;0;0]; %Real 
 v_s = [0;0;0]; %sculling
 v_g = [0;0;0]; %general 
 q = [0.8;0.1;0.5;0.435];
  
 for k= 1:5000%simulation time =1000*h1
   t=h1*(k-1);
   
   cita1 = [pi*sin(100*pi*h1/4)*cos(100*pi*(t+h1/4))/900;  0;  0];%two sample 
   cita2 = [pi*sin(100*pi*h1/4)*cos(100*pi*(t+3*h1/4))/900;  0;  0];
   dv1 = [0;  sin(100*pi*h1/4)*sin(100*pi*(t+h1/4))/(5*pi);  0];
   dv2 = [0;  sin(100*pi*h1/4)*sin(100*pi*(t+3*h1/4))/(5*pi);  0];
   
   
   phi = cita1+cita2; % rotation vector
   
   phi0 = sqrt(phi(1)*phi(1)+phi(2)*phi(2)+phi(3)*phi(3));
   c = cos(phi0/2);
   s = sin(phi0/2)/phi0;
   delta_qb = [c;s*phi(1);s*phi(2);s*phi(3)];
        q = quaterion(q,delta_qb);   
             
  T(1,1) = q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4);
  T(1,2) = 2*(q(2)*q(3)-q(1)*q(4));
  T(1,3) = 2*(q(2)*q(4)+q(1)*q(3));

  T(2,1) =  2*(q(2)*q(3)+q(1)*q(4));
  T(2,2) =  q(1)*q(1)-q(2)*q(2)+q(3)*q(3)-q(4)*q(4);
  T(2,3) =  2*(q(3)*q(4)-q(1)*q(2));

  T(3,1) = 2*(q(2)*q(4)-q(1)*q(3)); 
  T(3,2) = 2*(q(3)*q(4)+q(1)*q(2));
  T(3,3) = q(1)*q(1)-q(2)*q(2)-q(3)*q(3)+q(4)*q(4);
  
  tmp1 = chacheng((cita1+cita2),(dv1+dv2));
  
  
  tmp2 = chacheng(cita1,dv2);
  tmp3 = chacheng(dv1,cita2);
    
   v_r = v_r+T*(dv1+dv2+0.5*tmp1');
  %v_r = v_r+T*(dv1+dv2);
  
 if mod(k,10)==0
     ve_r(round(k/10)) = v_r(1);
    vn_r(round(k/10)) = v_r(2);
    vz_r(round(k/10)) = v_r(3);


   end
end



q = [0.8;0.1;0.5;0.435];
for k= 1:500 %simulation time =1000*h
    t=h2*(k-1);
    
    cita1 = [pi*sin(100*pi*h2/4)*cos(100*pi*(t+h2/4))/900;  0;  0];%two sample 
    cita2 = [pi*sin(100*pi*h2/4)*cos(100*pi*(t+3*h2/4))/900;  0;  0];
 
   dv1 = [0;  sin(100*pi*h2/4)*sin(100*pi*(t+h2/4))/(5*pi);  0];
   dv2 = [0;  sin(100*pi*h2/4)*sin(100*pi*(t+3*h2/4))/(5*pi);  0];
    
   phi = cita1+cita2; % rotation vector
   
   phi0 = sqrt(phi(1)*phi(1)+phi(2)*phi(2)+phi(3)*phi(3));
   c = cos(phi0/2);
   s = sin(phi0/2)/phi0;
   delta_qb = [c;s*phi(1);s*phi(2);s*phi(3)];
        q = quaterion(q,delta_qb);   
             
  T(1,1) = q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4);
  T(1,2) = 2*(q(2)*q(3)-q(1)*q(4));
  T(1,3) = 2*(q(2)*q(4)+q(1)*q(3));

  T(2,1) =  2*(q(2)*q(3)+q(1)*q(4));
  T(2,2) =  q(1)*q(1)-q(2)*q(2)+q(3)*q(3)-q(4)*q(4);
  T(2,3) =  2*(q(3)*q(4)-q(1)*q(2));

  T(3,1) = 2*(q(2)*q(4)-q(1)*q(3)); 
  T(3,2) = 2*(q(3)*q(4)+q(1)*q(2));
  T(3,3) = q(1)*q(1)-q(2)*q(2)-q(3)*q(3)+q(4)*q(4);
  
 tmp1 = chacheng((cita1+cita2),(dv1+dv2));
 tmp2 = chacheng(cita1,dv2);
 tmp11 = chacheng(cita1,dv1);
  tmp12 = chacheng(cita2,dv2);

  
  tmp3= chacheng(cita2,dv1);
  %tmp4= chacheng(cita1,dv2)-chacheng(dv1,cita2);
  %tmp5 = chacheng(cita1,dv2);
  
  tmp6 = chacheng(dv1,cita2);
  tmp7 =chacheng(cita1,dv2);

    

  
  v_s = v_s+T*(dv1+dv2+0.5*tmp1'+2*(tmp2'-tmp3')/3);  
  %v_s = v_s+T*(dv1+dv2+0.5*tmp1');
   v_g = v_g+T*(dv1+dv2);
    
  ve_s(k) = v_s(1);
  vn_s(k) = v_s(2);
  vz_s(k) = v_s(3);

  ve_g(k) = v_g(1);
  vn_g(k) = v_g(2);
  vz_g(k) = v_g(3);

end



i=1:1:500;

 %plot(i*h2,vn_g(i)-vn_r(i),'r',i*h2,vn_s(i)-vn_r(i),'--')
 plot(i*h2,ve_g(i)-ve_r(i),'r',i*h2,ve_s(i)-ve_r(i),'--')


 %plot(i*h2,vn_g(i)-vn_r(i))
 %xlabel('t(s)'),ylabel('北向速度误差   (m/s)')
 %plot(i*h2,vn_s(i)-vn_r(i))
 %xlabel('t(s)'),ylabel('北向速度误差   (m/s)')

  %plot(i,vz_s(i)-vz_r(i))
  %plot(i,vz_g(i)-vz_r(i))
  
  %plot(i*h2,ve_g(i)-ve_r(i))
  %xlabel('t(s)'),ylabel('东向速度误差    (m/s)')
  %plot(i*h2,ve_s(i)-ve_r(i)) 
   %xlabel('t(s)'),ylabel('东向速度误差   (m/s)')