Commit 7379a2c1 authored by Andrey Filippov's avatar Andrey Filippov

more debugging, fixed bug in matrix inversion

parent 42caa731
...@@ -131,6 +131,11 @@ class PolynomialApproximation ...@@ -131,6 +131,11 @@ class PolynomialApproximation
public function matrixInvert($A, $debugFile = null) public function matrixInvert($A, $debugFile = null)
{ {
/// @todo check rows = columns /// @todo check rows = columns
if ($debugFile !== null) {
fprintf($debugFile, "\Input matrix for inversion:\n");
$this->print_matrix($debugFile, $A);
}
$n = count($A); $n = count($A);
...@@ -151,10 +156,10 @@ class PolynomialApproximation ...@@ -151,10 +156,10 @@ class PolynomialApproximation
// for all remaining rows (diagonally) // for all remaining rows (diagonally)
for ($i = $j+1; $i < $n; ++ $i) { for ($i = $j+1; $i < $n; ++ $i) {
// if the value is not already 0 // if the value is not already 0
if ($A[$i][$j] !== 0) { if ($A[$i][$j] != 0) {
// adjust scale to pivot row // adjust scale to pivot row
// subtract pivot row from current // subtract pivot row from current
$scalar = $A[$j][$j] / $A[$i][$j]; $scalar = $A[$j][$j] / $A[$i][$j]; // division by zero
for ($jj = $j; $jj < $n*2; ++ $jj) { for ($jj = $j; $jj < $n*2; ++ $jj) {
$A[$i][$jj] *= $scalar; $A[$i][$jj] *= $scalar;
$A[$i][$jj] -= $A[$j][$jj]; $A[$i][$jj] -= $A[$j][$jj];
...@@ -172,7 +177,7 @@ class PolynomialApproximation ...@@ -172,7 +177,7 @@ class PolynomialApproximation
// reverse run // reverse run
for ($j = $n-1; $j > 0; -- $j) { for ($j = $n-1; $j > 0; -- $j) {
for ($i = $j-1; $i >= 0; -- $i) { for ($i = $j-1; $i >= 0; -- $i) {
if ($A[$i][$j] !== 0) { if ($A[$i][$j] != 0) {
$scalar = $A[$j][$j] / $A[$i][$j]; $scalar = $A[$j][$j] / $A[$i][$j];
for ($jj = $i; $jj < $n*2; ++ $jj) { for ($jj = $i; $jj < $n*2; ++ $jj) {
$A[$i][$jj] *= $scalar; $A[$i][$jj] *= $scalar;
......
...@@ -178,8 +178,15 @@ function peakAzimuth($xml_state, $az_play,$az_range, $az_step, $fract, $pow = 1. ...@@ -178,8 +178,15 @@ function peakAzimuth($xml_state, $az_play,$az_range, $az_step, $fract, $pow = 1.
fprintf($dbg_file, "azimuth argmax = %f (was %f)\n", $az_argmax, $az_start_center); fprintf($dbg_file, "azimuth argmax = %f (was %f)\n", $az_argmax, $az_start_center);
} }
$DishAngle[0] = $az_argmax - $az_play; $DishAngle[0] = $az_argmax - $az_play;
if ($dbg_file !== null) {
fprintf($dbg_file, "====== Moving azimuth to compensate motor play to = %f\n", $DishAngle[0]);
}
moveElAzSk_degrees($DishAngle); moveElAzSk_degrees($DishAngle);
$DishAngle[0] = $az_argmax; $DishAngle[0] = $az_argmax;
if ($dbg_file !== null) {
fprintf($dbg_file, "====== Moving azimuth to argmax position = %f\n", $DishAngle[0]);
}
moveElAzSk_degrees($DishAngle); moveElAzSk_degrees($DishAngle);
} else { } else {
// for now - move back // for now - move back
...@@ -227,17 +234,44 @@ function findMax1d($data, $fract, $pow, $frac_outside = 0.2){ ...@@ -227,17 +234,44 @@ function findMax1d($data, $fract, $pow, $frac_outside = 0.2){
} }
$pa = new PolynomialApproximation(); $pa = new PolynomialApproximation();
$pa->debugLevel = 3;
$pa->debugFile = $dbg_file;
$rslt = $pa->polynomialApproximation1d($poly_data, 2); $rslt = $pa->polynomialApproximation1d($poly_data, 2);
if ($dbg_file) { if ($dbg_file) {
fprintf($dbg_file, "argmax= %d , k_min= %d, k_max = %d, max = %f, threshold = %f, rslt:\n", fprintf($dbg_file, "argmax= %d , k_min= %d, k_max = %d, max = %f, threshold = %f, rslt:\n",
$argmax, $k_min, $k_max, $max, $threshold); $argmax, $k_min, $k_max, $max, $threshold);
fprintf($dbg_file,print_r($rslt,1)); fprintf($dbg_file, print_r($rslt,1));
$pre = array_fill(0, sizeof($data),0);
$diff= array_fill(0, sizeof($data),0);
for ($k = $k_min; $k <= $k_max; $k++){
$x = $k-$argmax;
$pre[$k]=$rslt[0]+$rslt[1]*$x+$rslt[2]*$x*$x;
$diff[$k]=$pre[$k]-$datap[$k];
}
for ($k = $k_min; $k <= $k_max; $k++){
fprintf($dbg_file, "[%02d]: %8.3f %8.3f %8.3f %8.3f\n", $k, $data[$k], $datap[$k], $pre[$k], $diff[$k]);
}
fprintf($dbg_file, "poly_data= \n".print_r($poly_data,1)."\n");
} }
if (is_nan($rslt[0]) || is_nan($rslt[1]) || is_nan($rslt[2])){
if ($dbg_file) {
fprintf($dbg_file, "Could not find maximum (NaN): coeff = \n".print_r($rslt,1)."\n");
}
return null; // no maximum
}
if ($rslt[2] >= 0){ if ($rslt[2] >= 0){
if ($dbg_file) { if ($dbg_file) {
fprintf($dbg_file, "Could not find maximum coeff = \n".printr($rslt,1)."\n"); fprintf($dbg_file, "Could not find maximum coeff = \n".print_r($rslt,1)."\n");
} }
return null; // no maximum return null; // no maximum
} }
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment