%                                trajs.m
%  Scope:   This MATLAB macro computes the vehicle trajectory with the 
%           following characteristics: straight segment with constant
%           speed in ENU frame; the trajectory is specified in ECEF and
%           ENU frames. WGS-84 constants are used.
%  Usage:   [trajecef,trajenu] = trajs(user0xyz,v0enu,deltat,nsteps)
%  Description of parameters:
%           user0xyz - input, user's initial ECEF position, the components 
%                      are in meters
%           v0enu    - input, ENU velocity vector, the components are in 
%                      meters/second
%           deltat   - input, time step, in seconds
%           nsteps   - input, number of step desired
%           trajecef - output, ECEF position, the components are in meters;
%                      trajecef(k+1,:) is the k-th time step value
%           trajenu  - output, ENU position, the components are in meters;
%                      trajenu(k+1,:) is the k-th time step value; the 
%                      origin is the initial user's position
%  External Matlab macros used:  tecefgd, venuecef, wgs84con
%  Last update:  05/29/00
%  Copyright (C) 1999-00 by LL Consulting. All Rights Reserved.

function  [trajecef,trajenu] = trajs(user0xyz,v0enu,deltat,nsteps)

%  Initialization

[lat,lon,alt] = tecefgd(user0xyz);

trajenu = zeros(nsteps+1,3);
trajecef = zeros(nsteps+1,3);
trajecef(1,:) = user0xyz;

%  Compute ECEF and ENU position at each time step

for k = 1:nsteps
   temp = trajenu(k,:) + v0enu * deltat;
   trajenu(k+1,:) = temp;
   trajecef(k+1,:) = user0xyz + (venuecef(temp,lat,lon))';
end