more debugging, fixed bug in matrix inversion

PolynomialApproximation.php

@@ -131,6 +131,11 @@ class PolynomialApproximation
     public function matrixInvert($A, $debugFile = null)
     {
         /// @todo check rows = columns
+        if ($debugFile !== null) {
+            fprintf($debugFile, "\Input matrix for inversion:\n");
+            $this->print_matrix($debugFile, $A);
+        }
+        
         
         $n = count($A);
         
@@ -151,10 +156,10 @@ class PolynomialApproximation
             // for all remaining rows (diagonally)
             for ($i = $j+1; $i < $n; ++ $i) {
                 // if the value is not already 0
-                if ($A[$i][$j] !== 0) {
+                if ($A[$i][$j] != 0) {
                     // adjust scale to pivot row
                     // 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) {
                         $A[$i][$jj] *= $scalar;
                         $A[$i][$jj] -= $A[$j][$jj];
@@ -172,7 +177,7 @@ class PolynomialApproximation
         // reverse run
         for ($j = $n-1; $j > 0; -- $j) {
             for ($i = $j-1; $i >= 0; -- $i) {
-                if ($A[$i][$j] !== 0) {
+                if ($A[$i][$j] != 0) {
                     $scalar = $A[$j][$j] / $A[$i][$j];
                     for ($jj = $i; $jj < $n*2; ++ $jj) {
                         $A[$i][$jj] *= $scalar;

test_01.php

@@ -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);
         }
         $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);
         $DishAngle[0] = $az_argmax;
+        if ($dbg_file !== null) {
+            fprintf($dbg_file, "====== Moving azimuth to argmax position = %f\n", $DishAngle[0]);
+        }
         moveElAzSk_degrees($DishAngle);
     } else {
         // for now - move back
@@ -227,17 +234,44 @@ function findMax1d($data, $fract, $pow, $frac_outside = 0.2){
     }
     
     $pa = new PolynomialApproximation();
+    $pa->debugLevel = 3;
+    $pa->debugFile = $dbg_file;
+    
+    
     $rslt = $pa->polynomialApproximation1d($poly_data, 2);
     
+    
     if ($dbg_file) {
+        
         fprintf($dbg_file, "argmax= %d , k_min= %d, k_max = %d, max = %f, threshold = %f, rslt:\n",
             $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 ($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
     }