gps-sins.m

GPS与惯性导航的组合导航

clear all
minutes=6;
delta_t= 0.01;
samples=1/delta_t*60*minutes;
tau_1=60;
tau_2=60;
tau_3=3600;
w1=randn(1,samples);
w2=randn(1,samples);
w3=randn(1,samples);
w4=randn(1,samples);
w5=600*randn(1,samples);
w6=600*randn(1,samples);
w7=zeros(1,samples);
w8=zeros(1,samples);
w=[w1;w2;w3;w4;w5;w6;w7;w8;];
v1=randn(1,samples);
v2=randn(1,samples);
v3=randn(1,samples);
v4=randn(1,samples);
v_0=[v1;v2];
v_1=[v1;v2;v3;v4];
gps_flag=zeros(samples,1);
for g=1:1/delta_t:samples
    j=rand;
    if j<0.5
        gps_flag(g)=0;
    else
        gps_flag(g)=1;
    end
end
A=[1/tau_1   0       0      0     0      0      0    0;
     0    1/tau_1    0      0     0      0      0    0;
     0       0    1/tau_2   0     0      0      0    0;
     0       0       0 1/tau_2    0       0     0    0;
     0       0       0      0   1/tau_3   0      0    0;
     0       0       0      0      0   1/tau_3   0    0;
    1/tau_1  0    1/tau_2   0      0      0      0    0;
     0    1/tau_1    0    1/tau_2  0      0      0    0];
 B=[w1;w2;w3;w4;w5;w6;w7;w8];
 C=eye(8);
 D=zeros(8,samples);
 phi=[ exp(-delta_t/tau_1)   0   0   0   0   0   0   0;
     0    exp(-delta_t/tau_1)   0   0   0   0   0   0;
     0      0  exp(-delta_t/tau_2)   0   0   0   0   0;
     0       0   0   exp(-delta_t/tau_2) 0   0   0   0;
     0       0   0   0   exp(-delta_t/tau_3)  0   0   0;
     0       0   0   0   0  exp(-delta_t/tau_3)   0   0;
     tau_1*(1-exp(-delta_t/tau_1)) 0 tau_2*(1-exp(-delta_t/tau_2)) 0   0   0   0   0;
      0  tau_1*(1-exp(-delta_t/tau_1)) 0  tau_2*(1-exp(-delta_t/tau_2))   0   0   0   0];
  Q=[((1/(2*tau_1))*(1-exp((-2*delta_t)/tau_1)))  0 0 0 0 0 0 0;
       0 ((1/(2*tau_1))*(1-exp((-2*delta_t)/tau_1))) 0 0 0 0 0 0;
       0 0 ((1/(2*tau_2))*(1-exp((-2*delta_t)/tau_2))) 0 0 0 0 0;
       0 0 0 ((1/(2*tau_2))*(1-exp((-2*delta_t)/tau_2))) 0 0 0 0;
       0 0 0 0 ((1/(2*tau_3))*(1-exp((-2*delta_t)/tau_3))) 0 0 0;
       0 0 0 0 0 ((1/(2*tau_3))*(1-exp((-2*delta_t)/tau_3))) 0 0;
       0 0 0 0 0 0              0                              0;
       0 0 0 0 0 0 0                                           0];
   process_noise=sqrt(Q)*w;
   R_0=diag([0.5 0.5]);
   R_1=diag([0.5 0.5 0 0]);
   sensor_noise_0=sqrt(R_0)*v_0;
   sensor_noise_1=sqrt(R_1)*v_1;
   x_hat_minus(8,samples)=zeros;
   x_hat_plus(8,samples)=zeros;
   x(8,samples)=zeros;
   P_minus=[0.5  0 0 0 0 0 0 0 ;
             0 0.5 0 0 0 0 0 0 ;
             0   0 1 0 0 0 0 0 ;
             0  0  0 1 0 0 0 0;
             0  0  0 0 3 0 0 0;
             0  0  0 0 0 3 0 0;
             0  0  0 0 0 0 5 0;
             0  0  0 0 0 0 0 5];
   time_index_a=1;
   time_index_b=1;
   for k=2:samples
       x(:,k)=phi*x(:,k-1)+process_noise(:,k-1);
       if gps_flag(k)==0
           H=[1 0 0 0 0 0 0 0;
              0  1 0 0 0 0 0 0];
          R=diag([0.5 0.5]);
          sensor_noise_0(:,k)=sqrt(R)*v_0(:,k);
          z_vel1(time_index_a)=H(1,:)*x(:,k)+sensor_noise_0(1,k);
          z_vel2(time_index_a)=H(2,:)*x(:,k)+sensor_noise_0(2,k);
          z_vel_time(time_index_a)=k*delta_t;
          z_vel=[z_vel1;z_vel2];
          K=P_minus*H'*inv(H*P_minus*H'+R);
          x_hat_plus(:,k)=x_hat_minus(:,k-1)+K*(z_vel(:,time_index_a)-H*x_hat_minus(:,k-1));
          P_plus=(eye(8)-K*H)*P_minus;
          P_plus=(P_plus+P_plus')/2;
          time_index_a=time_index_a+1;
       else 
           H=[1  0 0 0 0 0 0 0 ;
             0 1 0 0 0 0 0 0 ;
             0   0 0 0 1 0 1 0 ;
             0  0  0 1 0 1 0 1];
         R=diag([0.5 0.5 0 0]);
         z_gps1(time_index_b)=H(1,:)*x(:,k)+sensor_noise_1(1,k);
         z_gps2(time_index_b)=H(2,:)*x(:,k)+sensor_noise_1(2,k);
         z_gps3(time_index_b)=H(3,:)*x(:,k)+sensor_noise_1(3,k);
         z_gps4(time_index_b)=H(4,:)*x(:,k)+sensor_noise_1(4,k);
         z_gps=[z_gps1;z_gps2;z_gps3;z_gps4];
         z_gps_time(time_index_b)=k*delta_t;
         K=P_minus*H'*inv(H*P_minus*H'+R);
         x_hat_plus(:,k)=x_hat_minus(:,k-1)+K*(z_gps(:,time_index_b)-H*x_hat_minus(:,k-1));
         P_plus=(eye(8)-K*H)*P_minus;
         P_plus=(P_plus+P_plus')/2;
         time_index_b=time_index_b+1;   
       end
       pmin(:,:,k)=P_plus;
       x_hat_minus(:,k)=phi*x_hat_plus(:,k);
       P_minus=phi*P_plus*phi'+Q;
       P_minus=(P_minus+P_minus')/2;
      
     time(k)=k*delta_t;  %记住时标;
     
   end
   figure(1)
   subplot(2,1,1)
   plot(time,x(7,:),'b',time,x_hat_minus(7,:),'r-.',z_gps_time,z_gps3,'g-')
   xlabel('time(seconds)')
   ylabel('position(meters)')
   title('North Position vs Time')
   axis([0 max(time) -1.5*max(abs(z_gps3)) 1.5*max(abs(z_gps3))])
   
   subplot(2,1,2)
   plot(time,x(8,:),'b-',time,x_hat_minus(8,:),'r-.',z_gps_time,z_gps4,'g-')
   xlabel('time(seconds)')
   ylabel('position(meters)')
   title('East Position vs Time')
   axis([0 max(time) -1.5*max(abs(z_gps4)) 1.5*max(abs(z_gps4))])
   
    subplot(3,1,3)
   plot(x_hat_minus(7,:),x_hat_minus(8,:),'b-')
   xlabel('North Position')
   ylabel('East Position')
   title('North-East Position plot')
   axis([-1.5*max(abs(x_hat_minus(7,:))) 1.5*max(abs(x_hat_minus(7,:)))  -1.5*max(abs(x_hat_minus(8,:))) 1.5*max(abs(x_hat_minus(8,:)))])
   
   orient tall