DR.rar_DR_PDOP_gps dr kalman matlab_kalman 导航_navigation

clear; clc; format long % parameters about earth Re=6378137.0;f=1/298.257; LatConst=5.5/60; LonConst=8.8/60; % WGS84<-->HK80 %%%%%%%%%%%%%% time %%%%%%%%%%%%%% k=0; % k=0 initialization; k=1 navigation t=0; % navigation time (s) t_stop=30; % over time (s) T=1/20; % emulation time (s) Kalman_T=1; % kalman filter interval (s) %%%%%%%%% Steps counting %%%%%%%%% zacc_old=0; zacc=0; dz=0; dz_old=0; t_old=0; fs=1/T; step_num=0; %%%%%%%%%%% step count %%%%%%%%%%% n=0; check=[]; step_num=[0 0]; old_g=[1 1;1 1]; old_z=[1 1;1 1]; %%%%%%%%%%%%%%% DR %%%%%%%%%%%%%%% t_DR=0; % step_size=0.45; P=step_size; step_fs=2; step_T=0; step_num_old=[0 0]; w=[]; P_DR_old=[0;0]; DR=[0;0;0] % x,y,heading %%%%%%%%%%%%%% GPS %%%%%%%%%%%%%%% GPS_flag1=0; GPS_flag2=0; GPS_flag=0; %%%%%%%%%%%%%% KALMAN %%%%%%%%%%%% Kal_flag=0; % =GPS_flag1*GPS_flag2*DR_flag k=1; Xc=zeros(7,1); PK=zeros(7,7); Yc=zeros(3,1); gyro_modi=0; Pe=0; % step_size error Be=0.02; % heading bias error bias_compass=0; P_kal_old=[0 0]'; posi_kal_old=[0 0]'; %%%%%%%%%% Tmp data recorder %%%%% gyro=[]; accx=[]; accy=[]; accz=[]; comp=[]; gyro_f=[]; accz_f=[];Head_gps=0; %%%%%%%%%% Data recorder %%%%%%%%% Data_DR=[]; Data_Kal=[]; Data_GPS=[]; FidDual=fopen('100steps04.txt'); t=0; navi_flag=0 while 1%t<=t_stop ReadLine1=fgetl(FidDual); if ~ischar(ReadLine1) break end; if size(ReadLine1)<10 continue; end [t,Data]=getdata(t,ReadLine1); if Data(1,1)==1 navi_flag=1; end if navi_flag==0 continue; end if Data(1,1)==2 gyro=[gyro,Data(1,2)]; accx=[accx,Data(1,3)]; accy=[accy,Data(1,4)]; accz=[accz,Data(1,5)]; comp=[comp,mod(Data(1,6),360)]; elseif Data(1,1)==1 Long_gps=Data(1,2); Lati_gps=Data(1,3); posi_gps=[Long_gps;Lati_gps]; HDOP_gps=Data(1,4); %Data(1,1)=0; elseif Data(1,1)==3&(length(Data)>1) Head_gps=Data(1,2); end if Data(1,1)==2% DR % fir filter if length(gyro)>20 gyro=gyro(1,2:21); accx=accx(1,2:21); accz=accz(1,2:21); end [gyro_fir,accz_fir,accx_fir]=fir(gyro,accz,accx); gyro_f=[gyro_f,gyro_fir]; accz_f=[accz_f,accz_fir]; % counting steps n=n+1; [step_fs,step_num,check,old_g,old_z]=step_counting(n,gyro_fir,accz_fir,accx_fir,check,step_num,old_g,old_z); % DR step_d=step_num-step_num_old; if step_d>0 t_DR=0; DR(3)=comp(length(comp))-Be; [DR]=DR_step(DR,P,step_d); step_num_old=step_num; end elseif Data(1,1)==1 % kalman Data(1,1)=0; %head_comp=mean(comp);comp=[]; if k==1 x_gps=0;y_gps=0; P_gps=DR(1:2,1); PK=10*eye(4); P=step_size; k=k+1; step_kal=0; posi_gps_old=posi_gps; posi_DR=[Long_gps;Lati_gps]; posi_kal=[Long_gps;Lati_gps]; else Xc=zeros(4,1); P_gps=deg2xy(posi_gps,posi_gps_old,P_gps); posi_gps_old=posi_gps; Yc_DR=DR; Yc_sen=[P_gps]; [t,Xc,PK]=kalman_cloop(t,Xc,PK,Yc_DR,Yc_sen,HDOP_gps,P,step_num-step_kal); step_kal=step_num(1,1); [DR,Pe]=kal_modi_cloop(Yc_DR,Xc); P=P-Pe; if P<0.1 P=0; elseif P>1.8 P=P/2; end P=P end [posi_kal]=xy2deg(DR(1:2),P_kal_old,posi_kal); P_kal_old=DR(1:2); Data_GPS=[Data_GPS;t,P_gps',Head_gps,posi_gps']; Data_Kal=[Data_Kal;t,DR',posi_kal(1:2)']; elseif Data(1,1)==0 [posi_kal]=xy2deg(DR(1:2),P_kal_old,posi_kal); P_kal_old=DR(1:2); Data_Kal=[Data_Kal;t,DR',posi_kal(1:2)']; end [posi_DR]=xy2deg(DR(1:2),P_DR_old,posi_DR); P_DR_old=DR(1:2); Data_DR=[Data_DR;t,DR',posi_DR(1:2)']; end fig_num=0 fig_num=fig_num+1; figure(fig_num); plot(Data_DR(:,2),Data_DR(:,3),'g');hold on; plot(Data_GPS(:,2),Data_GPS(:,3),'r .');hold on; plot(Data_Kal(:,2),Data_Kal(:,3),'b');hold on; xlabel('trace(X/m Y/m)');legend('DR','GPS','Kalman') fig_num=fig_num+1; figure(fig_num); %subplot(3,1,1);plot(Data_DR(:,2),Data_DR(:,3),'g');ylabel('DR') subplot(2,1,1);plot(Data_GPS(:,1),Data_GPS(:,4),'r');ylabel('GPS') subplot(2,1,2);plot(Data_DR(:,1),Data_DR(:,4),'g') %subplot(3,1,3);plot(Data_Kal(:,2),Data_Kal(:,3),'b');ylabel('kalman') fig_num=fig_num+1; figure(fig_num); plot(Data_DR(:,5),Data_DR(:,6),'g');hold on; plot(Data_GPS(:,5),Data_GPS(:,6),'r .');hold on; plot(Data_Kal(:,5),Data_Kal(:,6),'b');hold on; xlabel('trace(X:Longitude/deg Y:Latitude/deg)');legend('DR','GPS','Kalman');