gpstrackingdemo.m

GPS导航系统的matlab程序4（精度很高哦） GPS导航系统的matlab程序4（精度很高哦）

        k = k + 1;
        Time(k)  = t - StartTime;
        xtrue    = [xtrue,[PosN;VelN;PosE;VelE;PosD;VelD]];
        sd1      = [sd1,[sqrt(P1(1,1));sqrt(P1(2,2));sqrt(P1(3,3));sqrt(P1(4,4));sqrt(P1(5,5));sqrt(P1(6,6))]];
        x1hat    = [x1hat,[x1(1);x1(2);x1(3);x1(4);x1(5);x1(6)]];
        x2hat    = [x2hat,[x2(1);x2(2);x2(3);x2(4);x2(5);x2(6)]];
        x3hat    = [x3hat,[x3(1);x3(2);x3(4);x3(5);x3(7);x3(8)]];
        phihat   = x4(1);
        phidothat = x4(2);
        Phi      = [Phi,phi];
        PhiHat   = [PhiHat,phihat];
        PhiDot   = [PhiDot,phidot];
        PhiDotHat= [PhiDotHat,phidothat];
        [VehState4,dPosdphi] = Fig8Mod1D(t,phihat,TrackLength,Speed,CrossOverHeight);
        x4hat    = [x4hat,[VehState4(1);VehState4(4);VehState4(2);VehState4(5);VehState4(3);VehState4(6)]];
        for j=1:NoSats,
            sdPR(j) = sqrt(P1(j+6,j+6));
        end;
        %
        % Satellite pseudorange error estimation data, including
        %   true psorange error [m]
        %   estimated pseudorange error [m]
        %   standard deviation of estimation uncertainty [m]
        %
        % They are plotted to show that:
		%	1. Satellites not in view have RMS uncertainties of around 10 m
        %      estimated delays of zero m, and RMS tru values around 10 m.
        %   2. Satellites in view have RMS uncertainties of around 6 m,
        %      and estimated delays tracking true values within 6 m RMS.
        %
        XPR           = [XPR,[xPR;x1(7:35);sdPR';x2(7:35);x3(10:38)]];
		SatsInView(k) = NoSatsAvail;
    end;
    %
    % TYPE2 filter: temporal update
    %
    x1 = Phi1*x1;
    P1 = Phi1*P1*Phi1' + Q1;
    P1 = (P1+P1')/2;
    %
    % DAMP2 filter: temporal update
    %
    x2 = Phi2*x2;
    P2 = Phi2*P2*Phi2' + Q2;
    P2 = (P2+P2')/2;
    % DAMP3 filter: temporal update
    %
    x3 = Phi3*x3;
    P3 = Phi3*P3*Phi3' + Q3;
    P3 = (P3+P3')/2;
    %
    % 1D Figure-8 tracker filter: temporal update
    %
    x4    = Phi4*x4;
    P4 = Phi4*P4*Phi4' + Q4;
    P4 = (P4+P4')/2;
    %
    % Along-track vehicle phase update
    %
    State = PhiState*State + randn(1)*[0;1];
    phi   = State(1);
    %
    % Simulated pseudorange delay error: temporal update
    %
    xPR  = PhiPR*xPR + sqrtqPR*randn(NoSats,1);
end;
%
% Display TYPE2 tracker performance statistics
%
T2RMSPosErrN = std(x1hat(1,:)-xtrue(1,:));
T2RMSPosErrE = std(x1hat(3,:)-xtrue(3,:));
T2RMSPosErrD = std(x1hat(5,:)-xtrue(5,:));
T2RMSVelErrN = std(x1hat(2,:)-xtrue(2,:));
T2RMSVelErrE = std(x1hat(4,:)-xtrue(4,:));
T2RMSVelErrD = std(x1hat(6,:)-xtrue(6,:));
disp('TYPE2 Tracker:');
disp(['   RMS N Pos Err = ',num2str(T2RMSPosErrN),' [m]']);
disp(['   RMS E Pos Err = ',num2str(T2RMSPosErrE),' [m]']);
disp(['   RMS D Pos Err = ',num2str(T2RMSPosErrD),' [m]']);
disp(['   RMS N Vel Err = ',num2str(T2RMSVelErrN),' [m]']);
disp(['   RMS E Vel Err = ',num2str(T2RMSVelErrE),' [m]']);
disp(['   RMS D Vel Err = ',num2str(T2RMSVelErrD),' [m]']);
%
% Display DAMP2 tracker performance statistics
%
D2RMSPosErrN = std(x2hat(1,:)-xtrue(1,:));
D2RMSPosErrE = std(x2hat(3,:)-xtrue(3,:));
D2RMSPosErrD = std(x2hat(5,:)-xtrue(5,:));
D2RMSVelErrN = std(x2hat(2,:)-xtrue(2,:));
D2RMSVelErrE = std(x2hat(4,:)-xtrue(4,:));
D2RMSVelErrD = std(x2hat(6,:)-xtrue(6,:));
disp('DAMP2 Tracker:');
disp(['   RMS N Pos Err = ',num2str(D2RMSPosErrN),' [m]']);
disp(['   RMS E Pos Err = ',num2str(D2RMSPosErrE),' [m]']);
disp(['   RMS D Pos Err = ',num2str(D2RMSPosErrD),' [m]']);
disp(['   RMS N Vel Err = ',num2str(D2RMSVelErrN),' [m]']);
disp(['   RMS E Vel Err = ',num2str(D2RMSVelErrE),' [m]']);
disp(['   RMS D Vel Err = ',num2str(D2RMSVelErrD),' [m]']);
%
% Display DAMP3 tracker performance statistics
%
D3RMSPosErrN = std(x3hat(1,:)-xtrue(1,:));
D3RMSPosErrE = std(x3hat(3,:)-xtrue(3,:));
D3RMSPosErrD = std(x3hat(5,:)-xtrue(5,:));
D3RMSVelErrN = std(x3hat(2,:)-xtrue(2,:));
D3RMSVelErrE = std(x3hat(4,:)-xtrue(4,:));
D3RMSVelErrD = std(x3hat(6,:)-xtrue(6,:));
disp('DAMP3 Tracker:');
disp(['   RMS N Pos Err = ',num2str(D3RMSPosErrN),' [m]']);
disp(['   RMS E Pos Err = ',num2str(D3RMSPosErrE),' [m]']);
disp(['   RMS D Pos Err = ',num2str(D3RMSPosErrD),' [m]']);
disp(['   RMS N Vel Err = ',num2str(D3RMSVelErrN),' [m]']);
disp(['   RMS E Vel Err = ',num2str(D3RMSVelErrE),' [m]']);
disp(['   RMS D Vel Err = ',num2str(D3RMSVelErrD),' [m]']);
%
% Display FIG8 tracker performance statistics
%
T4RMSPosErrN = std(x4hat(1,:)-xtrue(1,:));
T4RMSPosErrE = std(x4hat(3,:)-xtrue(3,:));
T4RMSPosErrD = std(x4hat(5,:)-xtrue(5,:));
T4RMSVelErrN = std(x4hat(2,:)-xtrue(2,:));
T4RMSVelErrE = std(x4hat(4,:)-xtrue(4,:));
T4RMSVelErrD = std(x4hat(6,:)-xtrue(6,:));
disp('Fig8Mod1D Tracker:');
disp(['   RMS N Pos Err = ',num2str(T4RMSPosErrN),' [m]']);
disp(['   RMS E Pos Err = ',num2str(T4RMSPosErrE),' [m]']);
disp(['   RMS D Pos Err = ',num2str(T4RMSPosErrD),' [m]']);
disp(['   RMS N Vel Err = ',num2str(T4RMSVelErrN),' [m]']);
disp(['   RMS E Vel Err = ',num2str(T4RMSVelErrE),' [m]']);
disp(['   RMS D Vel Err = ',num2str(T4RMSVelErrD),' [m]']);
%
% Plot 2D plan view of true trajectory and estimated trajectories
%
figure;
plot(x1hat(3,:),x1hat(1,:),'g-',x2hat(3,:),x2hat(1,:),'b-',x3hat(4,:),x3hat(1,:),'y-',x4hat(3,:),x4hat(1,:),'r-',xtrue(3,:),xtrue(1,:),'k-');axis equal;
title('Plan View of Estimated Locations and True Locations');
legend('TYPE2','DAMP2','DAMP3','FIG8','TRUE');
%
% Plot TYPE2 tracker navigation errors
%
figure;
subplot(3,1,1),plot(Time/3600,sd1(1,:),'k--',Time/3600,-sd1(1,:),'k--',Time/3600,x1hat(1,:)-xtrue(1,:),'k-');
title('TYPE2 Simulation: Position Error');
ylabel('North');
subplot(3,1,2),plot(Time/3600,sd1(3,:),'k--',Time/3600,-sd1(3,:),'k--',Time/3600,x1hat(3,:)-xtrue(3,:),'k-');
ylabel('East');
subplot(3,1,3);plot(Time/3600,sd1(5,:),'k--',Time/3600,-sd1(5,:),'k--',Time/3600,x1hat(5,:)-xtrue(5,:),'k-');
ylabel('Down');
xlabel('Time [hr]');
%
figure;
subplot(3,1,1),plot(Time/3600,sd1(2,:),'k--',Time/3600,-sd1(2,:),'k--',Time/3600,x1hat(2,:)-xtrue(2,:),'k-');
title('TYPE2 Simulation: Velocity Error');
ylabel('North');
subplot(3,1,2),plot(Time/3600,sd1(4,:),'k--',Time/3600,-sd1(4,:),'k--',Time/3600,x1hat(4,:)-xtrue(4,:),'k-');
ylabel('East');
subplot(3,1,3),plot(Time/3600,sd1(6,:),'k--',Time/3600,-sd1(6,:),'k--',Time/3600,x1hat(6,:)-xtrue(6,:),'k-');
ylabel('Down');
xlabel('Time [hr]');
%
% For each satellite (whether in view or not), plot calculated +/1 RMS
% signal delay estimation uncertainty, true delay and estimated delay.
%
for j=0:6
    figure;
    subplot(4,1,1),plot(Time/3600,XPR(4*j+1,:),'b-',Time/3600,XPR(4*j+1+NoSats,:),'k-',Time/3600,XPR(4*j+1+2*NoSats,:),'k--',Time/3600,-XPR(4*j+1+2*NoSats,:),'k--');
    ylabel(['SatNo. ',num2str(4*j+1)]);
    title('TYPE2 Tracker: User Equivalent Range Errors [m] from Propagation Delays');
    subplot(4,1,2),plot(Time/3600,XPR(4*j+2,:),'b-',Time/3600,XPR(4*j+2+NoSats,:),'k-',Time/3600,XPR(4*j+2+2*NoSats,:),'k--',Time/3600,-XPR(4*j+2+2*NoSats,:),'k--');
    ylabel(['SatNo. ',num2str(4*j+2)]);
    subplot(4,1,3),plot(Time/3600,XPR(4*j+3,:),'b-',Time/3600,XPR(4*j+3+NoSats,:),'k-',Time/3600,XPR(4*j+3+2*NoSats,:),'k--',Time/3600,-XPR(4*j+3+2*NoSats,:),'k--');
    ylabel(['SatNo. ',num2str(4*j+3)]);
    subplot(4,1,4),plot(Time/3600,XPR(4*j+4,:),'b-',Time/3600,XPR(4*j+4+NoSats,:),'k-',Time/3600,XPR(4*j+4+2*NoSats,:),'k--',Time/3600,-XPR(4*j+4+2*NoSats,:),'k--');
    ylabel(['SatNo. ',num2str(4*j+4)]);
    xlabel('Time [hr]');
end;
figure;
subplot(4,1,1),plot(Time/3600,XPR(NoSats,:),'b-',Time/3600,XPR(NoSats+NoSats,:),'k-',Time/3600,XPR(NoSats+2*NoSats,:),'k--',Time/3600,-XPR(NoSats+2*NoSats,:),'k--');
ylabel(['SatNo. ',num2str(NoSats)]);
title('TYPE2 Tracker: User Equivalent Range Errors [m] from Propagation Delays');
subplot(4,1,2),plot(Time/3600,SatsInView,'k-');
ylabel('# in View');
subplot(4,1,3),plot(Time/3600,Phi,'b-',Time/3600,PhiHat,'r-');
legend('true','est.');
ylabel('Track Phase [rad]');
subplot(4,1,4),plot(Time/3600,PhiDot,'b-',Time/3600,PhiDotHat,'r-');
legend('true','est.');
ylabel('Phase Rate [rad/s]');
xlabel('Time [hr]');
%
% Plot Power Spectral Densities of Position Errors
%
freq = (0:2048)/4096/dt;
figure;
subplot(3,1,1),
Y   = fft((x1hat(1,:)-xtrue(1,:)),4096);
Pyy = Y.*conj(Y)/4096;
loglog(freq,Pyy(1:2049),'k-');
title('Relative PSD of TYPE2 Tracker Position Errors');
ylabel('North');
subplot(3,1,2),
Y   = fft((x1hat(3,:)-xtrue(3,:)),4096);
Pyy = Y.*conj(Y)/4096;
loglog(freq,Pyy(1:2049),'k-');
ylabel('East');
subplot(3,1,3),
Y   = fft((x1hat(5,:)-xtrue(5,:)),4096);
Pyy = Y.*conj(Y)/4096;
loglog(freq,Pyy(1:2049),'k-');
ylabel('Down');
xlabel('Frequency [Hz]');
%
figure;
subplot(3,1,1),
Y   = fft((x2hat(1,:)-xtrue(1,:)),4096);
Pyy = Y.*conj(Y)/4096;
loglog(freq,Pyy(1:2049),'k-');
title('Relative PSD of DAMP2 Tracker Position Errors');
ylabel('North');
subplot(3,1,2),
Y   = fft((x2hat(3,:)-xtrue(3,:)),4096);
Pyy = Y.*conj(Y)/4096;
loglog(freq,Pyy(1:2049),'k-');
ylabel('East');
subplot(3,1,3),
Y   = fft((x2hat(5,:)-xtrue(5,:)),4096);
Pyy = Y.*conj(Y)/4096;
loglog(freq,Pyy(1:2049),'k-');
ylabel('Down');
xlabel('Frequency [Hz]');
%
figure;
subplot(3,1,1),
Y   = fft((x3hat(1,:)-xtrue(1,:)),4096);
Pyy = Y.*conj(Y)/4096;
loglog(freq,Pyy(1:2049),'k-');
title('Relative PSD of DAMP3 Tracker Position Errors');
ylabel('North');
subplot(3,1,2),
Y   = fft((x3hat(3,:)-xtrue(3,:)),4096);
Pyy = Y.*conj(Y)/4096;
loglog(freq,Pyy(1:2049),'k-');
ylabel('East');
subplot(3,1,3),
Y   = fft((x3hat(5,:)-xtrue(5,:)),4096);
Pyy = Y.*conj(Y)/4096;
loglog(freq,Pyy(1:2049),'k-');
ylabel('Down');
xlabel('Frequency [Hz]');
%
figure;
subplot(3,1,1),
Y   = fft((x4hat(1,:)-xtrue(1,:)),4096);
Pyy = Y.*conj(Y)/4096;
loglog(freq,Pyy(1:2049),'k-');
title('Relative PSD of FIG8 Tracker Position Errors');
ylabel('North');
subplot(3,1,2),
Y   = fft((x4hat(3,:)-xtrue(3,:)),4096);
Pyy = Y.*conj(Y)/4096;
loglog(freq,Pyy(1:2049),'k-');
ylabel('East');
subplot(3,1,3),
Y   = fft((x4hat(5,:)-xtrue(5,:)),4096);
Pyy = Y.*conj(Y)/4096;
loglog(freq,Pyy(1:2049),'k-');
ylabel('Down');
xlabel('Frequency [Hz]');