more debugging, fixed bug in matrix inversion (7379a2c1) · Commits · Elphel / motosat

PolynomialApproximation.php

+8 −3

Original line number	Original line	Diff line number	Diff line
	@@ -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
	// 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
	// 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;

test_01.php

+36 −2

Original line number	Original line	Diff line number	Diff line
	@@ -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){
	}		}

	$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
	}		}