xppvaj.m

GPS TOOLBOX包含以下内容： 1、GPS相关常量和转换因子； 2、角度变换； 3、坐标系转换： &#61656 点变换； &#61656 矩阵变换； &#61656 向量变换

%                                xppvaj.m
%  Scope:   This MATLAB program determines acceleration and jerk from the 
%           input data of time, position and velocity, and plots the relevant 
%           trajectory information.
%  Usage:   xppvaj
%  Inputs:  - name of the ASCII input data file,
%             Each record contains the following data:
%             - time, in seconds,
%             - latitude, in radians,
%             - longitude, in radians,
%             - altitude, in meters,
%             - East velocity component, in meters/second,
%             - North velocity component, in meters/second,
%             - Up velocity component, in meters/second,
%           - name of the scenario/trajectory, e.g. Low/Medium/High 
%             Dynamics Race Track 
%  Outputs: - the following plots are executed
%             - plot 1 - trajectory latitude/longitude/altitude versus time
%             - plot 2 - trajectory latitude versus longitude
%             - plot 3 - trajectory East/North/Up velocity versus time
%             - plot 4 - trajectory East/North/Up acceleration versus time
%             - plot 5 - trajectory East/North/Up jerk versus time
%             - plot 6 - trajectory velocity/acceleration magnitude versus time
%  Last update:  12/23/00
%  Copyright (C) 2000 by LL Consulting. All Rights Reserved.

clear
close all

disp('  ');
fname = input('Enter the name of the input data file -- > ','s');
inpdata = load(fname);

disp('  ');
disp('Enter Scenario name, e.g. Low/Medium/High Dynamics Scenario ');
aa = input('    --> ','s');
disp('  ');

time = inpdata(:,1);
lat = inpdata(:,2);
lon = inpdata(:,3);
alt = inpdata(:,4);
velE = inpdata(:,5);
velN = inpdata(:,6);
velU = inpdata(:,7);
dt = time(2) - time(1);    %  time step
dt2 = dt + dt;
dt4 = dt*dt;

[nrow,ncol] = size(inpdata);
n = max(nrow,ncol);

%  Plot the trajectory position

figure(1)
rad2deg = 180 / pi;
subplot(311)
plot(time,lat*rad2deg); grid;
aa1 = ['Figure 1. ' aa '  -  Trajectory Latitude/Longitude/Altitude  versus  Time'];
title(aa1);
ylabel('Latitude, in degrees');

subplot(312)
plot(time,lon*rad2deg); grid;
ylabel('Longitude, in degrees');

subplot(313)
plot(time,alt); grid;
ylabel('Altitude, in meters');
xlabel('Time, in seconds');

figure(2)
plot(lon*rad2deg,lat*rad2deg); grid;
aa2 = ['Figure 2. ' aa  '  -  Trajectory Latitude versus Longitude'];
title(aa2);
xlabel('Longitude, in degrees');
ylabel('Latitude, in degrees')

%  Plot the trajectory velocity

figure(3)
subplot(311)
plot(time,velE); grid;
aa3 = ['Figure 3. ' aa  '  -  Trajectory East/North/Up Velocity  versus  Time'];
title(aa3);
ylabel('East Velocity, in m/s');

subplot(312)
plot(time,velN); grid;
ylabel('North Velocity, in m/s');

subplot(313)
plot(time,velU); grid;
ylabel('Up Velocity, in m/s');
xlabel('Time, in seconds');

%  Computation of the East (acceleration and jerk) component 

accE  = velE(3:n) - velE(1:n-2);
accE  = accE / dt2;
accE(2:n-1) = accE;
accE(1) = accE(2);
accE(n) = accE(n-1);
jerkE = velE(3:n) - 2*velE(2:n-1) + velE(1:n-2);
jerkE = jerkE / dt4;
jerkE(2:n-1) = jerkE;
jerkE(1) = jerkE(2);
jerkE(n) = jerkE(n-1);

%  Computation of the North (acceleration and jerk) component 

accN  = velN(3:n) - velN(1:n-2);
accN  = accN/dt2;
accN(2:n-1) = accN;
accN(1) = accN(2);
accN(n) = accN(n-1);

jerkN = velN(3:n) - 2*velN(2:n-1) + velN(1:n-2);
jerkN = jerkN / dt4;
jerkN(2:n-1) = jerkN;
jerkN(1) = jerkN(2);
jerkN(n) = jerkN(n-1);

%  Computation of the Up (acceleration and jerk) component 

accU  = velU(3:n) - velU(1:n-2);
accU  = accU / dt2;
accU(2:n-1) = accU;
accU(1) = accU(2);
accU(n) = accU(n-1);

jerkU = velU(3:n) - 2*velU(2:n-1) + velU(1:n-2);
jerkU = jerkU / dt4;
jerkU(2:n-1) = jerkU;
jerkU(1) = jerkU(2);
jerkU(n) = jerkU(n-1);

%  Plot trajectory acceleration

figure(4)
subplot(311)
plot(time,accE); grid;
aa4 = ['Figure 4. ' aa  '  -  Trajectory East/North/Up Acceleration  versus  Time'];
title(aa4);
ylabel('East Acceleration, in m/s^2');

subplot(312)
plot(time,accN); grid;
ylabel('North Acceleration, in m/s^2');

subplot(313)
plot(time,accU); grid;
ylabel('Up Acceleration, in m/s^2');
xlabel('Time, in seconds');

%  Plot trajectory jerk

figure(5)
subplot(311)
plot(time,jerkE); grid;
aa5 = ['Figure 5. ' aa '  -  Trajectory East/North/Up Jerk  versus  Time'];
title(aa5);
ylabel('East Jerk, in m/s^3');

subplot(312)
plot(time,jerkN); grid;
ylabel('North Jerk, in m/s^3');

subplot(313)
plot(time,jerkU);grid;
ylabel('Up Jerk, in m/s^3');
xlabel('Time, in seconds');

%  Plot velocity and acceleration magnitude

figure(6)
subplot(211)
vel = sqrt(velE.^2 + velN.^2 + velU.^2);
plot(time,vel); grid;
aa61 = ['Figure 6.1. ' aa '  - Trajectory Velocity Magnitude  versus  Time'];
title(aa61);
xlabel('Time, in seconds');
ylabel('Velocity Magnitude, in m/s');

subplot(212)
acc = sqrt(accE.^2 + accN.^2 + accU.^2);
plot(time,acc); grid;
aa62 = ['Figure 6.2. ' aa '  - Trajectory Acceleration Magnitude  versus  Time'];
title(aa62);
xlabel('Time, in seconds');
ylabel('Acceleration Magnitude, in m/s^2');

disp ('  ');
disp ('End of the program  XPPAVJ ');
disp ('  ');