%
% Assume the origin of the inertially-fixed frame is at the dock.

  [ northings eastings ] = deg2utm( c.latitude.value*180/pi, c.longitude.value*180/pi );
%
% Navigation relative to the dock:
  dn = northings - northings(end);
  de = eastings  - eastings(end);
%
% Ground velocity from Dvl:
  vel = c.platform_velocity_wrt_ground.value;
%
% Time.  This is the main computation cycle of the MVC, about .4 seconds:
  time = c.time.value;
%
% Now relate everything to Mvc time.  Mvc time has the fastest sampling rate, so
% there cannot be two dvl or dat measurements in one Mvc interval.
%
% Transpose them so that the vectors are tall and skinny.
  npts = length(time);
  dvl = NaN(npts,3);
  dat = NaN(npts,2);
  for( i = 2:npts)
    if( c.platform_velocity_wrt_ground.time(i) <= time(i) && ...
          c.platform_velocity_wrt_ground.time(i) > time(i-1) )
      dvl(i,:) =  c.platform_velocity_wrt_ground.value(:,i)';
    end
    if( c.TrackAcousticContact.heading_to_contact.time(i) <= time(i) && ...
          c.TrackAcousticContact.heading_to_contact.time(i) > time(i-1) )
      dat(i,1) = c.TrackAcousticContact.heading_to_contact.value(i);
      dat(i,2) = c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.value(i);
    end
  end
  lastDvlTime = time(1);
  lastDatTime = time(1);
%
% Run the loop
  for( i=2:length(time) )
    % If a Dvl measurement came in, do a process update:
    if( ~sum(isnan(dvl(i,:))))
      dt = time(i) - lastDvlTime;
      xbar = xhat + [dt 0;0 dt]*dvl(i,:)';
      lastDvlTime = time(i);
    end
    % If a Dat measurement came in, do a measurement update:
    if( ~sum(isnan(dat(i,:))))
      xhat = xbar
    end
  end
  
