function [ yaw_offset, time_offset ] = alignGPSandOdomVelocity(...
  gps_time, gps_data, odom_time, odom_data, ...
  yaw_initial, time_initial, yaw_tolerance, time_tolerance)
%alignGPSandOdomVelocity Aligns GPS and Odometry velocity data
%   The alignment only includes yaw and time alignment assuming the roll
%   and pitch of the data are already aligned. With such assumption, it is
%   enough to just provide velocity components in the XY plane.
%
%    gps_time       : 1xN vector, time for GPS data.
%    gps_data       : 2xN matrix, gps velocity in the XY plane.
%    odom_time      : 1xN vector, time for Odom data.
%    odom_data      : 2xM matrix, odometry data
%    yaw_initial    : scalar, initial yaw alignment
%    time_initial   : scalar, initial time alignment
%    yaw_tolerance  : scalar, stopping criteria for yaw
%    time_tolerance : scalar, stopping criteria for time
%    yaw_offset     : takes the odom data to the GPS data
%    time_offset    : takes the odom data to the GPS data
%
%   It is not necessary to have the same amount of GPS and Odom data. The
%   function will use linear interpolation on the Odom data assuming Odom
%   data has higher frequency.
%
%   Author: Ke Sun
%   Date  : 08/08/2017 (MM/DD/YYYY)

alpha = 0.1; % between 0.01 and 0.3
beta = 0.5;  % between 0.1 and 0.8

exit_flag = false;

% Initial guess.
yaw_offset = yaw_initial;
time_offset = time_initial;

% Initial error.
error = normalizedError(gps_time, gps_data, odom_time, odom_data, yaw_offset, time_offset);

% Gradient descent method.
while ~exit_flag
  % Find the descent direction (gradient).
  yaw_grad = yawGradient(gps_time, gps_data, odom_time, odom_data, yaw_offset, time_offset);
  time_grad = timeGradient(gps_time, gps_data, odom_time, odom_data, yaw_offset, time_offset);
  
  % Backtracking line search.
  % The search direction will be -[yaw_grad time_grad].
  t = 1.0;
  grad_norm = -yaw_grad*yaw_grad - time_grad*time_grad;
  while normalizedError(gps_time, gps_data, odom_time, odom_data, ...
      unroll(yaw_offset-t*yaw_grad), time_offset-t*time_grad) > error + alpha * t * grad_norm
    t = beta * t;
  end
  
  % Update the solution.
  yaw_update = - t*yaw_grad;
  time_update = - t*time_grad;
  yaw_offset = unroll(yaw_offset + yaw_update);
  time_offset = time_offset + time_update;
  
  % Update the error.
  error = normalizedError(gps_time, gps_data, odom_time, odom_data, yaw_offset, time_offset);
  
  % Print the info of this iteration.
%   fprintf(1, 'dpsi: %.8f, dt: %.8f, cost: %.8f\n', yaw_update, time_update, error);
  
  % Set exit_flag if the solution converges.
  exit_flag = abs(yaw_update) < yaw_tolerance & ...
    abs(time_update) < time_tolerance;
end

  function [err] = normalizedError(gps_time, gps_data, odom_time, odom_data, yaw, time)
    % Rotate the odometry data.
    R = [cos(yaw) -sin(yaw); sin(yaw) cos(yaw)];
    odom_data = R * odom_data;
    % Align the time stamps.
    odom_time = odom_time + time;
    
    % Find the time intersection between the two data stream,
    % in order to avoid extrapolation.
    start_time = max([gps_time(1) odom_time(1)]);
    end_time = min([gps_time(end) odom_time(end)]);
    
    gps_data(:, gps_time<start_time | gps_time > end_time) = [];
    odom_data(:, odom_time<start_time | odom_time > end_time) = [];
    gps_time(gps_time<start_time | gps_time > end_time) = [];
    odom_time(odom_time<start_time | odom_time > end_time) = [];
    
    % Interpolate the odom data to ensure the data samples are collected
    % at the same time instances.
    intp_odom_data = interp1(odom_time', odom_data', gps_time', 'linear', 'extrap')';
    
    % Compute the normalized error between the two data stream.
    err = sqrt(mean(sum((intp_odom_data - gps_data).^2, 1)));
  end

  function [yaw_grad] = yawGradient(gps_time, gps_data, odom_time, odom_data, yaw, time)
    epsilon = 1e-5;
    % Perturb the yaw angle.
    err_plus = normalizedError(gps_time, gps_data, odom_time, odom_data, unroll(yaw+epsilon), time);
    err_minus = normalizedError(gps_time, gps_data, odom_time, odom_data, unroll(yaw-epsilon), time);
    % Compute the numerical derivative.
    yaw_grad = (err_plus-err_minus) / (2*epsilon);
  end

  function [time_grad] = timeGradient(gps_time, gps_data, odom_time, odom_data, yaw, time)
    epsilon = 1e-5;
    % Perturb the time alignment.
    err_plus = normalizedError(gps_time, gps_data, odom_time, odom_data, yaw, time+epsilon);
    err_minus = normalizedError(gps_time, gps_data, odom_time, odom_data, yaw, time-epsilon);
    % Compute the numerical derivative.
    time_grad = (err_plus-err_minus) / (2*epsilon);
  end

end








