%***********************************************************************
%
%	COMPUTES THE AFFINE TRANSFORMATION USED IN ACCELEROMETER SENSOR
%       USING THE DATA PROVIDED IN A FILE
%
%	By	:	Haldun KOMSUOGLU
%	Start	:	03/06/2002
%	Last	:	11/10/2002
%	Statue  :	DARPA Project Material
%
%       Inputs:
%
%       Outputs:
%
%       Notes:
%        1) Assumes that the data file is a text file with 4 columns where
%           the columns are ordered as follows:
%                   sweepType  supportPhi  acc_x  acc_y  acc_z
%
%***********************************************************************
function compute_accel_calib;

% ### Constants
% ###
% --- Control flags
ELIMINATE_OUTLIERS = 1;
X_THRESHOLD        = 0.8;
Y_THRESHOLD        = 1.0;
Z_THRESHOLD        = 1.0;
% --- Body dimensions
h = 40.0;    % Front hip to the rear of the body (cm)
w = 25.0;    % Front hip to other side of the body (cm)
r =  5.0;    % Front hip to bottom of the body (cm)
l = 17.0;    % Leg length
% --- Physical constants
g = 9.81;

% ### The experimental data on battery current and voltage and their
%     associated measurements
% ###
DATA = load( 'acc.data' );
% ---- Battery voltage related measurements
sweepType    = DATA( :, 1   );
phiRAW       = DATA( :, 2   );
accRAW       = DATA( :, 3:5 );





% ### For YAXIS sweep (sweepType = 0) compute the body pitch
% ###
% --- Extract those data from the YAXIS sweep 
YAXIS_ind = find( sweepType == 0 );
phi       = phiRAW( YAXIS_ind );
acc_pitch = accRAW( YAXIS_ind, : );
N         = length( YAXIS_ind );
% --- Compute the body pitch for each leg configuration
p     = sqrt( h.^2 + r.^2 );
gamma = atan( r / h );
ksi   = atan( h / r );
alpha = ksi + phi;
q     = sqrt( p.^2 + l.^2 - 2 * p * l * cos( alpha ) );
beta  = acos( ( p.^2 + q.^2 - l.^2  ) ./ ( 2 * p * q ) );
pitch = beta - gamma;
% --- Compute the expected accelerations along three axes
acc_pitch_exp( :, 1 ) = zeros( N, 1 );
acc_pitch_exp( :, 2 ) = -g * sin( pitch );
acc_pitch_exp( :, 3 ) = -g * cos( pitch );


% ### For XAXIS sweep (sweepType = 1) compute the body roll
% ###
% --- Extract those data from the YAXIS sweep 
XAXIS_ind = find( sweepType == 1 );
phi      = phiRAW( XAXIS_ind );
acc_roll = accRAW( XAXIS_ind, : );
% --- Compute the body pitch for each leg configuration
p     = sqrt( w.^2 + r.^2 );
l_bar = l * cos( phi );
gamma = atan( r / w );
alpha   = atan( w / r );
q     = sqrt( p.^2 + l_bar.^2 - 2 * p * l_bar .* cos( alpha ) );
beta  = acos( ( p.^2 + q.^2 - l.^2  ) ./ ( 2 * p * q ) );
roll  = beta - gamma;
% --- Compute the expected accelerations along three axes
acc_roll_exp( :, 1 ) = g * sin( roll );
acc_roll_exp( :, 2 ) = zeros( N, 1 );
acc_roll_exp( :, 3 ) = -g * cos( roll );


% ### Group the measurement and expectation sets
% ###
% --- Raw measurements
AccX_m = acc_roll( :, 1 );
AccY_m = acc_pitch( :, 2 );
AccZ_m = [ acc_pitch( :, 3 ); acc_roll( :, 3 ) ];
% --- Expected values
AccX_e = acc_roll_exp( :, 1 );
AccY_e = acc_pitch_exp( :, 2 );
AccZ_e = [ acc_pitch_exp( :, 3 ); acc_roll_exp( :, 3 ) ];


% ### Eliminate outliers
% ###
if ( ELIMINATE_OUTLIERS ),
  % --- X-axis data
  dist_x = [ AccX_m , AccX_e ];
  dist_x_raw = dist_x;
  oneV   = ones( size(dist_x,1), 1 );
  m_x         = mean( dist_x );
  C_x         = cov( dist_x );
  [V_x, D_x ] = eig( C_x );
  V_x = 0.1 * V_x;
  for i = 1:size(dist_x,1),
    pointer = [ dist_x(i, :) - m_x, 0 ];
    err_x(i) = norm( cross( [ V_x(:, 2)', 0], pointer ) );
  end;
  thr_err_x   = X_THRESHOLD * mean( err_x );
  x_valid_ind = find( err_x < thr_err_x );
  dist_x = dist_x( x_valid_ind, : );
  AccX_m = dist_x( :, 1 );
  AccX_e = dist_x( :, 2 );
  % --- Y-axis data
  dist_y = [ AccY_m , AccY_e ];
  dist_y_raw = dist_y;
  oneV   = ones( size(dist_y,1), 1 );
  m_y         = mean( dist_y );
  C_y         = cov( dist_y );
  [V_y, D_y ] = eig( C_y );
  V_y = 0.1 * V_y;
  for i = 1:size(dist_y,1),
    pointer = [ dist_y(i, :) - m_y, 0 ];
    err_y(i) = norm( cross( [ V_y(:, 2)', 0], pointer ) );
  end;
  thr_err_y = Y_THRESHOLD * mean( err_y );
  y_valid_ind = find( err_y < thr_err_y );
  dist_y = dist_y( y_valid_ind, : );
  AccY_m = dist_y( :, 1 );
  AccY_e = dist_y( :, 2 );
  % --- Z-axis data
  dist_z = [ AccZ_m , AccZ_e ];
  dist_z_raw = dist_z;
  oneV   = ones( size(dist_z,1), 1 );
  m_z         = mean( dist_z );
  C_z         = cov( dist_z );
  [V_z, D_z ] = eig( C_z );
  V_z = 0.1 * V_z;
  for i = 1:size(dist_z,1),
    pointer = [ dist_z(i, :) - m_z, 0 ];
    err_z(i) = norm( cross( [ V_z(:, 2)', 0], pointer ) );
  end;
  thr_err_z = Z_THRESHOLD * mean( err_z );
  z_valid_ind = find( err_z < thr_err_z );
  dist_z = dist_z( z_valid_ind, : );
  AccZ_m = dist_z( :, 1 );
  AccZ_e = dist_z( :, 2 );
end;

% ### Using Linear Squares Fit find the affine maps from the measurements
%     to the real values
% ###
% ---- Compute affine map for the voltage
[P{1}, S{1} ] = polyfit( AccX_m , AccX_e, 1 );
[P{2}, S{2} ] = polyfit( AccY_m , AccY_e, 1 );
[P{3}, S{3} ] = polyfit( AccZ_m , AccZ_e, 1 );




% ### Plot the data to show the fit performance
% ###
% ---- Plot the X-axis measurement and fit
figure( 1 );
clf;
hold on;
plot( dist_x_raw(:,1), dist_x_raw(:,2), 'm*' );
plot( AccX_m , AccX_e, 'r*' );
plot( AccX_m, polyval( P{1}, AccX_m ), 'b-' );
plot( [ m_x(1), m_x(1)+V_x(1,1) ], [ m_x(2), m_x(2)+V_x(2,1) ], 'g' );
plot( [ m_x(1), m_x(1)+V_x(1,2) ], [ m_x(2), m_x(2)+V_x(2,2) ], 'r' );
grid;
title( 'X-axis accelerometer' ); 
xlabel( 'measurment' );
ylabel( 'acceleration' );
% ---- Plot the Y-axis measurement and fit
figure( 2 );
clf;
hold on;
plot( dist_y_raw(:,1), dist_y_raw(:,2), 'm*' );
plot( AccY_m, AccY_e, 'r*' );
plot( AccY_m, polyval( P{2}, AccY_m ), 'b-' );
plot( [ m_y(1), m_y(1)+V_y(1,1) ], [ m_y(2), m_y(2)+V_y(2,1) ], 'g' );
plot( [ m_y(1), m_y(1)+V_y(1,2) ], [ m_y(2), m_y(2)+V_y(2,2) ], 'r' );
grid;
title( 'Y-axis accelerometer' ); 
xlabel( 'measurment' );
ylabel( 'acceleration' );
% ---- Plot the Z-axis measurement and fit
figure( 3 );
clf;
hold on;
plot( dist_z_raw(:,1), dist_z_raw(:,2), 'm*' );
plot( AccZ_m, AccZ_e, 'r*' );
plot( AccZ_m, polyval( P{3}, AccZ_m ), 'b-' );
plot( [ m_z(1), m_z(1)+V_z(1,1) ], [ m_z(2), m_z(2)+V_z(2,1) ], 'g' );
plot( [ m_z(1), m_z(1)+V_z(1,2) ], [ m_z(2), m_z(2)+V_z(2,2) ], 'r' );
grid;
title( 'Z-axis accelerometer' ); 
xlabel( 'measurment' );
ylabel( 'acceleration' );


% ### Display the coefficients to be used in RHexLib
% ###
disp( 'RHexLib accelerometer calibration settings   ' );
disp( '---------------------------------------------' );
% ---- Build the accelerometer gain configuration line
gain_text = sprintf( 'float accelerometer_gains   = %.5f %.5f %.5f ;', ...
                      P{1}(1), P{2}(1), P{3}(1)   );
disp( gain_text );
% ---- Build the accelerometer gain configuration line
offsets_text = sprintf( 'float accelerometer_offsets = %.5f %.5f %.5f ;', ...
                       P{1}(2), P{2}(2), P{3}(2)  );
disp( offsets_text );


