ImRunningOutOfNames.m

function [ Truth_Data, dt, theta, n_vec, offset_vec ] = ...
    ImRunningOutOfNames( VANTAGE_Data, Truth_Data, should_debug )
%%% Inputs:
%%%     - VANTAGE_Data: a struct with the following fields as they pertain
%%%         to the VANTAGE measurements
%%%         > t: Time vector
%%%         > x: X position vector
%%%         > y: Y position vector
%%%         > z: Z position vector
%%%         > d: A length 2 vector, first element: minimum distance on
%%%             which ToF is trusted, second element: maximum distance on
%%%             which ToF is trusted
%%%     - Truth_Data: a struct with the following fields as they pertain
%%%         to the Truth measurements
%%%         > t: Time vector
%%%         > x: X position vector
%%%         > y: Y position vector
%%%         > z: Z position vector
%%%
%%% Outputs:
%%%     - dt: Truth data time shift

if nargin < 3
    should_debug = 0;
end

%{
First we have to extract the VANTAGE measurement values we trust
%}
VANTAGE_Data_Distance = sqrt( VANTAGE_Data.x.^2 + VANTAGE_Data.y.^2 + ...
    VANTAGE_Data.z.^2 );

VANTAGE_Index_Start = find( VANTAGE_Data_Distance >= VANTAGE_Data.d(1), ...
    1, 'first' );
VANTAGE_Index_End = find( VANTAGE_Data_Distance <= VANTAGE_Data.d(2), ...
    1, 'last' );
VANTAGE_Index = VANTAGE_Index_Start : VANTAGE_Index_End;

%{
Now we will generate a 3D linear least squares fit (f) of the trusted
VANTAGE 3D data ([x,y,z]):
f_x = m_x * t + b_x
[ m_x; b_x ] = inv( A' * A ) * ( A' * x )
A = [ t, 1 ];
same process for y and z
f = [ f_x; f_y; f_z ]
%}
VANTAGE_Fit.t = VANTAGE_Data.t( VANTAGE_Index );
A = [ VANTAGE_Fit.t, ones( size( VANTAGE_Fit.t ) ) ];
intermediary = ( A' * A ) \ ( A' * VANTAGE_Data.x( VANTAGE_Index ) );
m = intermediary(1);
b = intermediary(2);
VANTAGE_Fit.f_x = @(t) m .* t + b;

intermediary = ( A' * A ) \ ( A' * VANTAGE_Data.y( VANTAGE_Index ) );
m = intermediary(1);
b = intermediary(2);
VANTAGE_Fit.f_y = @(t) m .* t + b;

intermediary = ( A' * A ) \ ( A' * VANTAGE_Data.z( VANTAGE_Index ) );
m = intermediary(1);
b = intermediary(2);
VANTAGE_Fit.f_z = @(t) m .* t + b;

if should_debug
    
    figure;
    scatter3( VANTAGE_Data.x( VANTAGE_Index ), ...
        VANTAGE_Data.y( VANTAGE_Index ), ...
        VANTAGE_Data.z( VANTAGE_Index ), 2, ...
        VANTAGE_Data.t( VANTAGE_Index ) )
    hold on
    plot3( VANTAGE_Fit.f_x( VANTAGE_Data.t( VANTAGE_Index ) ), ...
        VANTAGE_Fit.f_y( VANTAGE_Data.t( VANTAGE_Index ) ), ...
        VANTAGE_Fit.f_z( VANTAGE_Data.t( VANTAGE_Index ) ), ...
        '-r', 'LineWidth', 2 )
    title( 'VANTAGE Data & Fit' )
    
end

%{
Now we need to do the same for the truth data, but only over the range
which corresponds to the positions where the truth data is accurate
%}
Truth_Data_Distance = sqrt( Truth_Data.x.^2 + Truth_Data.y.^2 + ...
    Truth_Data.z.^2 );

Truth_Index_Start = find( Truth_Data_Distance >= VANTAGE_Data.d(1), 1, 'first' );
Truth_Index_End = find( Truth_Data_Distance <= VANTAGE_Data.d(2), 1, 'last' );
Truth_Index = Truth_Index_Start:Truth_Index_End;

%{
Now that we know what values of the Truth data line up with the trusred
VANTAGE data, we can fit them
%}
Truth_Fit.t = Truth_Data.t( Truth_Index );
A = [ Truth_Fit.t, ones( size( Truth_Fit.t ) ) ];
intermediary = ( A' * A ) \ ( A' * Truth_Data.x( Truth_Index ) );
m = intermediary(1);
b = intermediary(2);
Truth_Fit.f_x = @(t) m .* t + b;

intermediary = ( A' * A ) \ ( A' * Truth_Data.y( Truth_Index ) );
m = intermediary(1);
b = intermediary(2);
Truth_Fit.f_y = @(t) m .* t + b;

intermediary = ( A' * A ) \ ( A' * Truth_Data.z( Truth_Index ) );
m = intermediary(1);
b = intermediary(2);
Truth_Fit.f_z = @(t) m .* t + b;

if should_debug
    
    figure;
    scatter3( Truth_Data.x( Truth_Index ), ...
        Truth_Data.y( Truth_Index ), ...
        Truth_Data.z( Truth_Index ), 2, ...
        Truth_Data.t( Truth_Index ) )
    hold on
    plot3( Truth_Fit.f_x( Truth_Data.t( Truth_Index ) ), ...
        Truth_Fit.f_y( Truth_Data.t( Truth_Index ) ), ...
        Truth_Fit.f_z( Truth_Data.t( Truth_Index ) ), ...
        '-r', 'LineWidth', 2 )
    title( 'Truth Data & Fit' )
    
end

%{
Now we can determine the best fit lines to the distance line generated by
X_3D_Fit.vec, but this time we will be fitting function for time
%}
Truth_Fit.f_dist_3D = @(t) sqrt( Truth_Fit.f_x(t).^2 + ...
    Truth_Fit.f_y(t).^2 + Truth_Fit.f_z(t).^2 );

A = [ Truth_Fit.f_dist_3D( Truth_Fit.t ), ones( size( Truth_Fit.t ) ) ];
intermediary = ( A' * A ) \ ( A' * Truth_Fit.t );
m = intermediary(1);
b = intermediary(2);
Truth_Fit.f_t = @(d) m .* d + b;

VANTAGE_Fit.f_dist_3D = @(t) sqrt( VANTAGE_Fit.f_x(t).^2 + ...
    VANTAGE_Fit.f_y(t).^2 + VANTAGE_Fit.f_z(t).^2 );

A = [ VANTAGE_Fit.f_dist_3D( VANTAGE_Fit.t ), ones( size( VANTAGE_Fit.t ) ) ];
intermediary = ( A' * A ) \ ( A' * VANTAGE_Fit.t );
m = intermediary(1);
b = intermediary(2);
VANTAGE_Fit.f_t = @(d) m .* d + b;

if should_debug
    
    figure;
    plot( VANTAGE_Data.t( VANTAGE_Index ), ...
        VANTAGE_Data_Distance( VANTAGE_Index ), 'ok' )
    hold on
    plot( VANTAGE_Data.t( VANTAGE_Index ), ...
        VANTAGE_Fit.f_dist_3D( VANTAGE_Data.t( VANTAGE_Index ) ), ...
        '-r', 'LineWidth', 2 )
    plot( VANTAGE_Fit.f_t( VANTAGE_Fit.f_dist_3D( VANTAGE_Data.t( VANTAGE_Index ) ) ), ...
        VANTAGE_Fit.f_dist_3D( VANTAGE_Data.t( VANTAGE_Index ) ), ...
        '-b', 'LineWidth', 2 )
    title( 'VANTAGE Distance & Dist Fit & Time Fit' )
    
    figure;
    plot( Truth_Data.t( Truth_Index ), ...
        Truth_Data_Distance( Truth_Index ), 'ok' )
    hold on
    plot( Truth_Data.t( Truth_Index ), ...
        Truth_Fit.f_dist_3D( Truth_Data.t( Truth_Index ) ), ...
        '-r', 'LineWidth', 2 )
    plot( Truth_Fit.f_t( Truth_Fit.f_dist_3D( Truth_Data.t( Truth_Index ) ) ), ...
        Truth_Fit.f_dist_3D( Truth_Data.t( Truth_Index ) ), ...
        '-b', 'LineWidth', 2 )
    title( 'Truth Distance & Dist Fit & Time Fit' )
    
end

%{
Now we will numerically minimize the function:
( VANTAGE_Fit.f_t( VANTAGE_Fit.t ) - Truth_Fit.f_t( Truth_Fit.t + dt ) ).^2
by varying dt to adequate precision
%}
VANTAGE_t = linspace( min( VANTAGE_Fit.t ), max( VANTAGE_Fit.t ), 1000 )';

% note we need to make the distance traveled the same for both time vectors
Truth_Start_Distance = min( VANTAGE_Fit.f_dist_3D( VANTAGE_t ) );
Truth_Start_Time = Truth_Fit.f_t( Truth_Start_Distance );
Truth_Stop_Distance = max( VANTAGE_Fit.f_dist_3D( VANTAGE_t ) );
Truth_Stop_Time = Truth_Fit.f_t( Truth_Stop_Distance );
Truth_t = linspace( Truth_Start_Time, Truth_Stop_Time, 1000 )';

% initializing things for the while loop
dt_error_vec = Truth_Fit.f_t( Truth_Fit.f_dist_3D( Truth_t ) ) - ...
    VANTAGE_Fit.f_t( VANTAGE_Fit.f_dist_3D( VANTAGE_t ) );
dt_error_mean = mean( dt_error_vec );
dt_error_squares = sum( dt_error_vec.^2 );
dt_sign = sign( dt_error_mean );
dt_change = dt_sign * 10^( round( log10( abs( dt_error_mean ) ) + 1 ) );

precision_current = abs( dt_change );
precision_required = 1e-12; % something sufficiently small chosen
dt = 0;

% debug stuff
if should_debug
    
    figure;
    plot( VANTAGE_Fit.f_t( VANTAGE_Fit.f_dist_3D( VANTAGE_t ) ), ...
        VANTAGE_Fit.f_dist_3D( VANTAGE_t ), ...
        '-k', 'LineWidth', 2 )
    hold on
    plot( Truth_Fit.f_t( Truth_Fit.f_dist_3D( Truth_t ) ), ...
        Truth_Fit.f_dist_3D( Truth_t ), ...
        '-r', 'LineWidth', 2 )
    title( 'VANTAGE Time Fit & Truth Time Fit' )
    
    prev_squares = dt_error_squares;
    iteration = 1;
    T_t{iteration} = Truth_Fit.f_t( Truth_Fit.f_dist_3D( Truth_t ) );
    T_d{iteration} = Truth_Fit.f_dist_3D( Truth_t );
    
end

while precision_current > precision_required
    
    % test out the next point in the same direction as last time
    dt_error_vec_new = ...
        Truth_Fit.f_t( Truth_Fit.f_dist_3D( Truth_t ) ) - ...
        VANTAGE_Fit.f_t( VANTAGE_Fit.f_dist_3D( VANTAGE_t ) ) + ...
        dt + dt_change;
    dt_error_squares_new = sum( dt_error_vec_new.^2 );
    
    % if it is an improvement, use it
    if dt_error_squares_new < dt_error_squares
        
        dt = dt + dt_change;
        dt_error_squares = dt_error_squares_new;
        
    else % mignt be time to switch directions
        
        dt_error_vec_new = ...
            Truth_Fit.f_t( Truth_Fit.f_dist_3D( Truth_t ) ) - ...
            VANTAGE_Fit.f_t( VANTAGE_Fit.f_dist_3D( VANTAGE_t ) ) + ...
            dt - dt_change;
        dt_error_squares_new = sum( dt_error_vec_new.^2 );
        
        % if it is an improvement, use it
        if dt_error_squares_new < dt_error_squares
            
            dt_change = -dt_change;
            dt = dt + dt_change;
            dt_error_squares = dt_error_squares_new;
            
        else % it is at a local minimum for this stepsize, decrease stepsize
            
            dt_change = dt_change / 10;
            
        end
        
    end
    
    % update the precision
    precision_current = abs( dt_change );
    
    % degub (lol) stuff
    if should_debug
        
        if dt_error_squares < prev_squares
            
            prev_squares = dt_error_squares;
            iteration = iteration + 1;
            T_t{iteration} = Truth_t + dt;
            T_d{iteration} = Truth_Fit.f_dist_3D( Truth_t );
            
        end
        
    end
    
end

if should_debug
    
    colors = [ 0, 0, 0; jet( length( T_t ) - 1 ) ];
    figure;
    plot( VANTAGE_t, VANTAGE_Fit.f_dist_3D( VANTAGE_t ), '-ok', ...
        'LineWidth', 2 )
    hold on
    plot( T_t{1}, T_d{1}, '-o', 'color', colors(1,:), 'LineWidth', 2 )
    
    for i = 2 : length( T_t )
        
        plot( T_t{i}, T_d{i}, '-', 'color', colors(i,:), 'LineWidth', 2 )
        
    end
    
end

% Apply dt correction to Truth timestamp
Truth_Data.t = Truth_Data.t + dt;

% Apply dt correction to Truth fits
Truth_Fit.t = Truth_Fit.t + dt;
Truth_Fit.x = @(t) Truth_Fit.x( t - dt );
Truth_Fit.y = @(t) Truth_Fit.y( t - dt );
Truth_Fit.z = @(t) Truth_Fit.z( t - dt );

%{
Now it is time to rotate the truth data to be aligned with the VANTAGE data
%}
VANTAGE_vector = [ VANTAGE_Fit.f_x( max( VANTAGE_t ) + 1 ), ...
                   VANTAGE_Fit.f_y( max( VANTAGE_t ) + 1 ), ...
                   VANTAGE_Fit.f_z( max( VANTAGE_t ) + 1 ) ] - ...
                 [ VANTAGE_Fit.f_x( min( VANTAGE_t ) ), ...
                   VANTAGE_Fit.f_y( min( VANTAGE_t ) ), ...
                   VANTAGE_Fit.f_z( min( VANTAGE_t ) ) ];
Truth_vector = [ Truth_Fit.f_x( min( VANTAGE_t ) + 1 ), ...
                 Truth_Fit.f_y( min( VANTAGE_t ) + 1 ), ...
                 Truth_Fit.f_z( min( VANTAGE_t ) + 1 ) ] - ...
               [ Truth_Fit.f_x( min( VANTAGE_t ) ), ...
                 Truth_Fit.f_y( min( VANTAGE_t ) ), ...
                 Truth_Fit.f_z( min( VANTAGE_t ) ) ];

% angle between VANTAGE_vector and Truth_vector
theta = acos( dot( VANTAGE_vector, Truth_vector ) / ...
    ( norm( VANTAGE_vector ) * norm( Truth_vector ) ) );

% initialization of the rotation vector in case theta = 0
n_vec = [ 0, 0, 0 ];

if should_debug
    
    figure;
    scatter3( VANTAGE_Data.x, VANTAGE_Data.y, VANTAGE_Data.z, 2, ...
        VANTAGE_Data.t )
    hold on
    plot3( VANTAGE_Fit.f_x( VANTAGE_Data.t ), ...
        VANTAGE_Fit.f_y( VANTAGE_Data.t ), ...
        VANTAGE_Fit.f_z( VANTAGE_Data.t ), ...
        '-r', 'LineWidth', 2 )
    scatter3( Truth_Data.x, Truth_Data.y, Truth_Data.z, 2, ...
        Truth_Data.t )
    plot3( Truth_Fit.f_x( Truth_Data.t-dt ), ...
        Truth_Fit.f_y( Truth_Data.t-dt ), ...
        Truth_Fit.f_z( Truth_Data.t-dt ), ...
        '-r', 'LineWidth', 2 )
    
end

% check to make sure theta is nonzero
if theta
    
    % normal unit vector to VANTAGE_vector and Truth_vector
    n_vec = cross( Truth_vector, VANTAGE_vector );
    n_vec = n_vec ./ norm( n_vec );
    
    %{
    according to Wikipedia
    (https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle)
    the rotation matrix for a rotation of theta about n_vec is:
    %}
    R = [ cos(theta) + n_vec(1)^2 * ( 1 - cos(theta) ), ...
          n_vec(1) * n_vec(2) * ( 1 - cos(theta) ) - n_vec(3) * sin(theta), ...
          n_vec(1) * n_vec(3) * ( 1 - cos(theta) ) + n_vec(2) * sin(theta); ...
          n_vec(2) * n_vec(1) * ( 1 - cos(theta) ) + n_vec(3) * sin(theta), ...
          cos(theta) + n_vec(2)^2 * ( 1 - cos(theta) ), ...
          n_vec(2) * n_vec(3) * ( 1 - cos(theta) ) - n_vec(1) * sin(theta); ...
          n_vec(3) * n_vec(1) * ( 1 - cos(theta) ) - n_vec(2) * sin(theta), ...
          n_vec(3) * n_vec(2) * ( 1 - cos(theta) ) + n_vec(1) * sin(theta), ...
          cos(theta) + n_vec(3)^2 * ( 1 - cos(theta) ) ];
    
    % rotate Truth_Data.[x,y,z]
    for i = 1 : length( Truth_Data.t )
        
        vec = [ Truth_Data.x(i); Truth_Data.y(i); Truth_Data.z(i) ];
        
        intermediate = R * vec;
        
        Truth_Data.x(i) = intermediate(1);
        Truth_Data.y(i) = intermediate(2);
        Truth_Data.z(i) = intermediate(3);
        
    end
    
%     % rotate the best fit line
    b_vec = [ Truth_Fit.f_x( 0 ), Truth_Fit.f_y( 0 ), Truth_Fit.f_z( 0 ) ];
    Truth_vector = R * Truth_vector';
    b_vec = R * b_vec';
    Truth_Fit.f_x = @(t) Truth_vector(1) .* ( t - dt ) + b_vec(1);
    Truth_Fit.f_y = @(t) Truth_vector(2) .* ( t - dt ) + b_vec(2);
    Truth_Fit.f_z = @(t) Truth_vector(3) .* ( t - dt ) + b_vec(3);
    
end

if should_debug
    
    scatter3( Truth_Data.x, Truth_Data.y, Truth_Data.z, 2, ...
        Truth_Data.t )
    plot3( Truth_Fit.f_x( Truth_Data.t ), ...
        Truth_Fit.f_y( Truth_Data.t ), ...
        Truth_Fit.f_z( Truth_Data.t ), ...
        '-b', 'LineWidth', 2 )
    colormap jet
    title( 'Timestamp Corrected Truth Data Rotation: Before & After' )
    
end

%{
Finally, we are going to change the offset in the truth data such that the
areas of data we trust overlap in 3D space by making the mean point during
that span be identical for VANTAGE data and truth data
Note: VANTAGE_t from earlier is a time vector corresponding to this domain
%}
Truth_Offset_x = mean( VANTAGE_Fit.f_x( VANTAGE_t ) ) - ...
    mean( Truth_Fit.f_x( VANTAGE_t ) );
Truth_Offset_y = mean( VANTAGE_Fit.f_y( VANTAGE_t ) ) - ...
    mean( Truth_Fit.f_y( VANTAGE_t ) );
Truth_Offset_z = mean( VANTAGE_Fit.f_z( VANTAGE_t ) ) - ...
    mean( Truth_Fit.f_z( VANTAGE_t ) );
offset_vec = [ Truth_Offset_x, Truth_Offset_y, Truth_Offset_z ];

if should_debug
    
    figure;
    scatter3( VANTAGE_Data.x, VANTAGE_Data.y, VANTAGE_Data.z, 2, ...
        VANTAGE_Data.t )
    hold on
    plot3( VANTAGE_Fit.f_x( VANTAGE_Data.t ), ...
        VANTAGE_Fit.f_y( VANTAGE_Data.t ), ...
        VANTAGE_Fit.f_z( VANTAGE_Data.t ), ...
        '-r', 'LineWidth', 2 )
    scatter3( Truth_Data.x, Truth_Data.y, Truth_Data.z, 2, ...
        Truth_Data.t )
    plot3( Truth_Fit.f_x( Truth_Data.t ), ...
        Truth_Fit.f_y( Truth_Data.t ), ...
        Truth_Fit.f_z( Truth_Data.t ), ...
        '-r', 'LineWidth', 2 )
    
end

% Applying offset
Truth_Data.x = Truth_Data.x + Truth_Offset_x;
Truth_Data.y = Truth_Data.y + Truth_Offset_y;
Truth_Data.z = Truth_Data.z + Truth_Offset_z;
Truth_Fit.f_x = @(t) Truth_Fit.f_x(t) + Truth_Offset_x;
Truth_Fit.f_y = @(t) Truth_Fit.f_y(t) + Truth_Offset_y;
Truth_Fit.f_z = @(t) Truth_Fit.f_z(t) + Truth_Offset_z;

if should_debug
    
    scatter3( Truth_Data.x, Truth_Data.y, Truth_Data.z, 2, ...
        Truth_Data.t )
    plot3( Truth_Fit.f_x( Truth_Data.t ), ...
        Truth_Fit.f_y( Truth_Data.t ), ...
        Truth_Fit.f_z( Truth_Data.t ), ...
        '-b', 'LineWidth', 2 )
    colormap jet
    title( 'Timestamp Corrected, Rotated Truth Data Translation: Before & After' )
    
    figure;
    plot( VANTAGE_t, VANTAGE_Fit.f_x( VANTAGE_t ), '-k', 'LineWidth', 2 )
    hold on
    plot( VANTAGE_t, Truth_Fit.f_x( VANTAGE_t ), '-r', 'LineWidth', 2 )
    title( 'X VANTAGE Measurement Best Fit VS Truth Best Fit' )
    
    figure;
    plot( VANTAGE_t, VANTAGE_Fit.f_y( VANTAGE_t ), '-k', 'LineWidth', 2 )
    hold on
    plot( VANTAGE_t, Truth_Fit.f_y( VANTAGE_t ), '-r', 'LineWidth', 2 )
    title( 'Y VANTAGE Measurement Best Fit VS Truth Best Fit' )
    
    figure;
    plot( VANTAGE_t, VANTAGE_Fit.f_z( VANTAGE_t ), '-k', 'LineWidth', 2 )
    hold on
    plot( VANTAGE_t, Truth_Fit.f_z( VANTAGE_t ), '-r', 'LineWidth', 2 )
    title( 'Z VANTAGE Measurement Best Fit VS Truth Best Fit' )
    
end

end