matlab GPS与捷联惯导的组合导航程序，可以运行.rar

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