function [east,nrth] = drcalc( time, yaw, pitch, speed, origin );
%DRCALC      Calculates dead reckoning trajectory of vehicle
%   [east,north] = DRCALC( TIME, YAW, PITCH, SPEED, ORIGIN )
%   integrates the horizontal component of the vehicle velocity and
%   returns vectors east and north.  YAW and PITCH are in radians.
%   ORIGIN is an optional argument [E N] to specify the starting
%   point.
%

%----------------------------------------------------------------------
%                         Copyright (C) 1995
%               Massachussetts Institute of Technology
%              Sea Grant Underwater Vehicles Laboratory
%----------------------------------------------------------------------
%     $Revision: 1.1 $                  $State: Exp $
%     $Author: rob $            $Date: 2000/01/10 19:07:54 $
%----------------------------------------------------------------------


if nargin < 4, error('too few arguments'); end
if nargin < 5
    origin = [0 0];
end
                                % efficiency considerations
cp = cos(pitch);
sp = sin(pitch);
cy = cos(yaw);
sy = sin(yaw);
                                % dimensions must match
m = size(time,1);
if length(speed) > m-1, speed = speed(2:m); end
                                % horizontal components
ds = speed.*diff(time);
deast = 0.5*ds.*(cp(1:m-1).*sy(1:m-1) + cp(2:m).*sy(2:m));
dnrth = 0.5*ds.*(cp(1:m-1).*cy(1:m-1) + cp(2:m).*cy(2:m));
                                % allocate
east = zeros(size(time));
nrth = east;
                                % initialize
east(1) = origin(1);
nrth(1) = origin(2);

for i = 2:m                     % main loop for integrating
    east(i) = east(i-1) + deast(i-1);
    nrth(i) = nrth(i-1) + dnrth(i-1);
end
