VE - A Perl Script for Virtual Elevation Calculations

#
# These commandline parameters assume file-scope. I need this to
# simplify the f, fx, fy, fxx, etc... functions that are derived
# from compute_virtual_elevation():

my $power_file = shift;
my $elev_file = shift;
my $m = shift;
my $rho = shift;
my $dt = shift;
my $v_factor = shift;
my $start_elev = shift;

#
# read the input files:
my $power_data = read_power_data( $power_file );
my $actual_elevation = read_elevation_data( $elev_file );
print_elev( "000.dat", $actual_elevation );

# Use a nonlinear solver to determine CdA and Crr.
#
# The idea is that we have a function F(x,y) which we want to
# minimize. F is the L2-norm of the difference between our
# actual and computed elevation profiles. (x,y) is the ( crr, cda )
# input vector.
#
# The problem: find (x,y) that minimizes F.
#
# Solution:
#
# Set dF/dx and DF/dy to zero and get 2 equations in 2 unknowns.
# Follow the gradient downhill to get x*,y* that sets F(x*,y*) to zero.
#

#
# Seed:
my $cda = 0.700;
my $crr = 0.030;

#
# Outer loop for (crr,cda) solve:
my $error_norm = 10000000000;
my $tolerance = 0.001;
my $delta = 0.0000001;
my $x = $crr;
my $y = $cda;
my $alpha = 0.8;
my $correction_norm = 100000000;
my $i = 0;
while( ($error_norm > $tolerance) || ($correction_norm > $tolerance) )
{
# RHS vector:
my $b0 = fx($x,$y,$delta);
my $b1 = fy($x,$y,$delta);
my @b = ( -1.0 * $b0 , -1.0 * $b1 );

# Hessian:
my @A;
$A[0][0] = fxx( $x, $y, $delta );
$A[0][1] = fxy( $x, $y, $delta );
$A[1][0] = fyx( $x, $y, $delta );
$A[1][1] = fyy( $x, $y, $delta );

# Solve for new (dx,dy) vector:
#
# A * dx = b
my $dx = solve_2x2_matrix( \@A, \@b );

#
# Apply correction to (x,y):
$x += $alpha * $dx->[0];
$y += $alpha * $dx->[1];

my $ve = compute_virtual_elevation( $x, $y );
print_elev( sprintf("%02d\.dat", $i ), $ve );

#
# Compute both correction norm and rhs norm.
# This ensures that ill-conditioned problems converge completely:
$error_norm = sqrt( $b0*$b0 + $b1*$b1 );
$correction_norm = sqrt( $dx->[0]*$dx->[0] + $dx->[1]*$dx->[1] );

print sprintf("crr=%12.8f, cda=%12.8f", $x, $y ) . " RESIDUALS:" . sprintf( "%12.6f %12.6f\n", $correction_norm, $error_norm );

$i++;
}

#
# dummy function for l2-norm of actual vs virtual elevation error:
sub f
{
my $x = shift;
my $y = shift;

my $ve = compute_virtual_elevation( $x, $y);

return l2_error( $ve, $actual_elevation);
}

#
# dummy function dfdx for l2-norm of actual vs virtual elevation error:
sub fx
{
my $x = shift;
my $y = shift;
my $delta = shift;

my $f1 = f( $x+$delta, $y);
my $f0 = f( $x, $y);

return ( $f1 - $f0 ) / $delta;
}

#
# dummy function dfdx for l2-norm of actual vs virtual elevation error:
sub fy
{
my $x = shift;
my $y = shift;
my $delta = shift;

my $f1 = f( $x, $y+$delta);
my $f0 = f( $x, $y);

return ( $f1 - $f0 ) / $delta;
}

#
# dummy function fxx for l2-norm of actual vs virtual elevation error:
sub fxx
{
my $x = shift;
my $y = shift;
my $delta = shift;

my $f1 = fx( $x+$delta, $y, $delta);
my $f0 = fx( $x, $y, $delta);

return ( $f1 - $f0 )/ $delta;
}

#
# dummy function fxy for l2-norm of actual vs virtual elevation error:
sub fxy
{
my $x = shift;
my $y = shift;
my $delta = shift;

my $f1 = fx( $x, $y+$delta, $delta);
my $f0 = fx( $x, $y, $delta);

return ( $f1 - $f0 )/ $delta;
}

#
# dummy function fyx for l2-norm of actual vs virtual elevation error:
sub fyx
{
my $x = shift;
my $y = shift;
my $delta = shift;

my $f1 = fy( $x+$delta, $y, $delta);
my $f0 = fy( $x, $y, $delta);

return ( $f1 - $f0 )/ $delta;
}

#
# dummy function fyy for l2-norm of actual vs virtual elevation error:
sub fyy
{
my $x = shift;
my $y = shift;
my $delta = shift;

my $f1 = fy( $x, $y+$delta, $delta);
my $f0 = fy( $x, $y, $delta);

return ( $f1 - $f0 )/ $delta;
}

#
# 2x2 Matrix solver
#
# Solves the system A x = b using Cramer's rule:
sub solve_2x2_matrix
{
my $A = shift; #reference to 2x2 matrix
my $b = shift; # column vector rhs

my $det_A = ($A->[0][0] * $A->[1][1]) - ($A->[1][0] * $A->[0][1]);

if( $det_A == 0 )
{
die( "Determinant of matrix system is zero!\n");
}
my $cramer_0 = $b->[0] * $A->[1][1] - $b->[1] * $A->[0][1];
my $cramer_1 = $A->[0][0] * $b->[1] - $A->[1][0] * $b->[0];

my @solution = ( $cramer_0 / $det_A , $cramer_1 / $det_A );

return \@solution;
}

#
# Determine elevation vector:
sub compute_virtual_elevation
{
my $crr = shift;
my $cda = shift;
# my $power_data = shift;
# my $m = shift;
# my $rho = shift;
# my $dt = shift;
# my $v_factor = shift;
# my $start_elev = shift;

#
# Unpack:
my $p = $power_data->{p};
my $d = $power_data->{d};
my $v = $power_data->{v};

#
# With the input crr and cda, compute the virtual elevation
# profile:
my @ve;

#
# Unroll first row of calculation, to avoid if-statement in loop:
my $v1 = $v->[ 0 ]/$v_factor;
my $v2 = $v->[ 0 ]/$v_factor;
my $elev = $start_elev;
my @ve;
$ve[ 0 ] = 0; # CAUTION: assume ZERO starting elevation
#
# Now compute the acceleration, slope, and elevation change:
for( my $i=1; $i<$power_data->{n}; $i++ )
{
#
# unpack:
$v2 = $v->[ $i ]/$v_factor;

# use v2^2 - v1^2 = 2 * a * d to solve for a:
my $a = ( ($v2*$v2) - ($v1*$v1) ) / ( 2 * $dt * $v2 );
my $slope1 = slope( 0, $p->[$i]/$v2, $a, $m, $crr, $cda, $rho, $v2 );

#
my $delta_elev = $slope1 * $v2 * $dt;
$elev += $delta_elev;
#print "DATA:". join( ",", ( $i+1, $v->[$i], $p->[$i], $d->[$i], $v2, $a, $slope1, $delta_elev, $elev ) ) . "\n";
$ve[ $i ] = $elev;

$v1 = $v2;
}

return \@ve;

}

#
# Compute the L^2 error of 2 vectors:
sub l2_error
{
my $a = shift;
my $b = shift;

#
# are the 2 vectors the same length?
# no: die
if( scalar( @$a ) != scalar( @$b ) )
{
die "Cannot compute errors: " .
"vector a has length " . scalar( @$a ) . " and " .
"vector b has length " . scalar( @$b );
}

#
my $l2 = 0;
for( my $i=0; $i[ $i ] - $b->[ $i ];
$l2 += $delta * $delta;
}

return $l2;
}

#
# Given the formula
#
# F_drive - m g sin(theta) - Crr m g cos(theta) - CdA rho v^2 = m a
#
# compute theta (slope angle) given all the other variables, using
# regular iteration.
#
# NOTE:
# This equation is the exact form of the equations of motion, with
# no small-angle approximation as in the original Chung formulation.
# Whether this is justified or not, I don't personally believe in
# introducing more noise than necessary in the equations of motion.
# Only during the solution process should one decide on the
# tolerances.
#
#
# Test with:
# my $p = 321;
# my $m = 89;
# my $v_factor = 1.0;
# my $v = 7.111111;
# my $a = -0.665251;
# my $cda = 0.3845;
# my $crr = 0.005;
# my $rho = 1.236;
#
# slope( $p/$v, $a, $m, $crr, $cda, $rho, $v );
#
# should give: a return value of 0.039771537080168
#
# (Presently the slope is over-toleranced, which leads to
# a slow calculation. I will determine the tolerance that
# works.)
#
sub slope
{
my $compute_mode = shift;
my $f = shift;
my $a = shift;
my $m = shift;
my $crr = shift;
my $cda = shift;
my $rho = shift;
my $v = shift;

my $g = 9.80665;
#print "slope: " . join( ",", ( $f, $a, $m, $crr, $cda, $rho, $v ) ) . "\n";;

if( $compute_mode == 0 )
{
return ( ($f/($m*$g)) - $crr - ($cda* $rho*$v*$v/2)/($m*$g) - ($a/$g) );
}

my $tol = 0.000001;

my $error = 100000000;
my $theta = 0.01;
my $u = sin( $theta );
my $u_last = 0.01;
my $n_iters = 0;
while( ($error > $tol) && ($n_iters < 10) )
{
#print " " . tan( asin( $u ) ) . ":\n";
my $numerator = ( $f - $m * $a - $crr * $m * $g * sqrt( 1 - ($u*$u) ) - ($cda* $rho*$v*$v)/2);
my $denominator = ( $m * $g );

$u = $numerator / $denominator;

$error = abs( $u - $u_last );
$u_last = $u;

$n_iters++;
}
$theta = asin( $u );

return tan( $theta );
}

#
# read the power data from a CSV file:
# Minutes, Torq (N-m), Km/h, Watts, Km, Cadence, Hrate, ID

sub read_power_data
{
my $file = shift;

open POWER, $file;
my $i = 0;
my @p;
my @v;
my @d;
foreach my $line (
)
{
chomp $line;
my ( $t, $torque, $vel, $power, $dist, @others ) = split( /,/, $line );
$p[ $i ] = $power;
$d[ $i ] = $dist;
$v[ $i ] = $vel;

$i++;
}
close POWER;

my %power_data;
$power_data{n} = $i;
$power_data{p} = \@p;
$power_data{v} = \@v;
$power_data{d} = \@d;

return \%power_data;
}

#
# read elevation data:
sub read_elevation_data
{
my $elev_file = shift;

open ELEV, $elev_file;
my @e;
my $i = 0;
foreach my $line ( )
{
chomp $line;
$e[ $i ] = $line;
$i++;
}
close ELEV;

return \@e;
}

#
# print the elevation datai to file:
sub print_elev
{
my $file = shift;
my $vec = shift;

open ELEV, ">$file";
foreach my $v ( @$vec )
{
print ELEV $v . "\n";
}
close ELEV;