ported 1d polynomial approximation from Java code (42caa731) · Commits · Elphel / motosat

PolynomialApproximation.php

0 → 100644

+265 −0

Original line number	Original line	Diff line number	Diff line
			<?php
			//require_once ('test-matrix-invert.php'); // matrix inversion
			class PolynomialApproximation
			{
			// function __construct() {
			// }
			public $debugLevel = 1;
			public $debugFile = null;

			public function polynomialApproximation1d($data, $N){
			//$my_array = array_fill(0, $size_of_the_array, $some_data);
			if ($this->debugFile === null){
			$this->debugLevel = 0;
			}
			$S=array_fill(0,2$N +1,0); // new double [2N+1];
			$SF=array_fill(0,$N+1,0); // new double [N+1];
			for ($i=0; $i < sizeof($data); $i++){
			$wxn=(sizeof($data[$i]) > 2)? $data[$i][2] : 1.0;
			if ($wxn > 0.0){ // save time on 0.0 that can be used to mask out some samples
			$f=$data[$i][1];
			$x=$data[$i][0];
			for ($j=0; $j <= $N; $j++){
			$S[$j]+=$wxn;
			$SF[$j]+=$wxn * $f;
			$wxn *= $x;
			}
			for ($j = $N+1; $j < 2*$N;$j++){
			$S[$j] += $wxn;
			$wxn *= $x;
			}
			$S[2*$N] += $wxn;
			if ($this->debugLevel > 1){
			if (sizeof($data[$i]) > 2)
			fprintf($this->debugFile,
			"polynomialApproximation1d() \|".$i."\|: x=\|".$data[$i][0]."\| f(x)=\|".$data[$i][1]."\| (w=\t\|".$data[$i][2]."\|\t)");
			else
			fprintf($this->debugFile,
			"polynomialApproximation1d() \|".$i,"\|: x=\|".$data[$i][0]."\| f(x)=\|".$data[$i][1]."\|\t)");
			}
			}
			}
			$M = $this->zeroMatrix($N+1,$N+1);
			$B = $this->zeroMatrix($N+1,1);
			for ($i=0; $i <= $N; $i++) {
			$B[$i][0] = $SF[$i];
			for ($j = 0; $j <= $N; $j++) $M[$i][$j]= $S[$i + $j];
			}
			$N1 = $N;
			// TODO: use try/catch with solve
			if ($this->debugLevel > 1){
			fprintf($this->debugFile,
			"polynomialApproximation1d(data,".$N.") M:\n");
			$this->print_matrix($this->debugFile, $M);
			fprintf($this->debugFile,
			"polynomialApproximation1d() B:\n");
			$this->print_matrix($this->debugFile, $B);
			}
			// while (!(new LUDecomposition(M)).isNonsingular() && (N1>0)){
			$df = ($this->debugLevel > 2) ? $this->debugFile : null;
			while ($N1 >= 0) { // make N=0 legal ?
			try {
			$M_inv = $this->matrixInvert($M,$df);
			$R = $this->mmul($M_inv, $B);
			if ($this->debugLevel > 1){
			fprintf($this->debugFile,
			"polynomialApproximation1d() solution=\n");
			$this->print_matrix($this->debugFile, $R);
			}
			$result=array_fill(0,$N+1,0); // new double [N+1];
			for ($i = 0; $i <= $N1; $i++){
			$result[$i] = $R[$i][0];
			}
			return $result;

			} catch (Exception $e) {
			echo 'Caught exception: ', $e->getMessage(), ", reducing polynomial order\n";
			if ($N1 < 0 ) {
			return null;
			}
			$N1--;
			$M1 = $this->zeroMatrix($N+1,$N+1);
			$B1 = $this->zeroMatrix($N+1,1);
			for ($i = 0; $i <= $N1; $i++) {
			$B1[$i][0] = $B[$i][0];
			for ($j = 0; $j <= $N1; $j++){
			$M1[$i][$j] = $M[$i][$j];
			}
			}
			$M = $M1;
			$B = $B1;
			}
			}
			return null;
			}


			public function mmul($A, $B){
			$M = sizeof($A);
			$N = sizeof($A[0]);
			if ($N != sizeof($B)){
			throw new Exception('DImensions mismatch.');
			}
			$K = sizeof($B[0]);
			$R = $this->zeroMatrix($M,$K);
			for ($i = 0; $i < $M; $i++){
			for ($j = 0; $j < $K; $j++){
			for ($k = 0; $k < $N; $k++){
			$R[$i][$j] += $A[$i][$k] * $B[$k][$j];
			}
			}
			}
			return $R;
			}

			public function zeroMatrix($m,$n=1){
			$M = array_fill(0, $m, null);
			for ($i = 0; $i < $m; $i++){
			$M[$i] = array_fill(0, $n, 0.0);
			}
			return $M;
			}

			/**
			* Inverts a given matrix(downloaded from https://gist.github.com/unix1/7510208 )
			*
			* @param array $A matrix to invert
			* @param string $debugFile if not null - output file for debug info
			*
			* @return array inverted matrix
			*/
			public function matrixInvert($A, $debugFile = null)
			{
			/// @todo check rows = columns

			$n = count($A);

			// get and append identity matrix
			$I = $this->identity_matrix($n);
			for ($i = 0; $i < $n; ++ $i) {
			$A[$i] = array_merge($A[$i], $I[$i]);
			}

			if ($debugFile !== null) {
			fprintf($debugFile, "\nStarting matrix:\n");
			// echo "\nStarting matrix: ";
			$this->print_matrix($debugFile, $A);
			}

			// forward run
			for ($j = 0; $j < $n-1; ++ $j) {
			// for all remaining rows (diagonally)
			for ($i = $j+1; $i < $n; ++ $i) {
			// if the value is not already 0
			if ($A[$i][$j] !== 0) {
			// adjust scale to pivot row
			// subtract pivot row from current
			$scalar = $A[$j][$j] / $A[$i][$j];
			for ($jj = $j; $jj < $n*2; ++ $jj) {
			$A[$i][$jj] *= $scalar;
			$A[$i][$jj] -= $A[$j][$jj];
			}
			}
			}
			// if ($debug) {
			if ($debugFile !== null) {
			fprintf($debugFile, "\nForward iteration $j:\n");
			// echo "\nForward iteration $j: ";
			$this->print_matrix($debugFile, $A);
			}
			}

			// reverse run
			for ($j = $n-1; $j > 0; -- $j) {
			for ($i = $j-1; $i >= 0; -- $i) {
			if ($A[$i][$j] !== 0) {
			$scalar = $A[$j][$j] / $A[$i][$j];
			for ($jj = $i; $jj < $n*2; ++ $jj) {
			$A[$i][$jj] *= $scalar;
			$A[$i][$jj] -= $A[$j][$jj];
			}
			}
			}
			// if ($debug) {
			if ($debugFile !== null) {
			fprintf($debugFile, "\nReverse iteration $j:\n");
			// echo "\nReverse iteration $j: ";
			$this->print_matrix($debugFile, $A);
			}
			}

			// last run to make all diagonal 1s
			/// @note this can be done in last iteration (i.e. reverse run) too!
			for ($j = 0; $j < $n; ++ $j) {
			if ($A[$j][$j] !== 1) {
			if ($A[$j][$j] == 0.0) {
			// echo "Denominator is zero, throwing exception: \n";
			throw new Exception('Singular matrix.');
			}
			$scalar = 1 / $A[$j][$j];
			for ($jj = $j; $jj < $n*2; ++ $jj) {
			$A[$j][$jj] *= $scalar;
			}
			}
			// if ($debug) {
			if ($debugFile !== null) {
			fprintf($debugFile, "\n1-out iteration $j:\n");
			// echo "\n1-out iteration $j: ";
			$this->print_matrix($debugFile, $A);
			}
			}

			// take out the matrix inverse to return
			$Inv = array();
			for ($i = 0; $i < $n; ++ $i) {
			$Inv[$i] = array_slice($A[$i], $n);
			}

			return $Inv;
			}

			/**
			* Prints matrix
			* @param resource $debugFile debug file or null
			* @param array $A matrix
			* @param integer $decimals number of decimals
			*/
			function print_matrix($debugFile, $A, $decimals = 6)
			{
			if ($debugFile === null){
			$debugFile = fopen('php://stdout', 'w');
			}
			foreach ($A as $row) {
			fprintf($debugFile, "[");
			foreach ($row as $i) {
			fprintf($debugFile, "\t" . sprintf("%01.{$decimals}f", round($i, $decimals)));
			}
			fprintf($debugFile, "\t]\n");
			}
			}

			/**
			* Produces an identity matrix of given size
			*
			* @param integer $n size of identity matrix
			*
			* @return array identity matrix
			*/
			function identity_matrix($n)
			{
			$I = array();
			for ($i = 0; $i < $n; ++ $i) {
			for ($j = 0; $j < $n; ++ $j) {
			$I[$i][$j] = ($i == $j) ? 1 : 0;
			}
			}
			return $I;
			}






			}

Admin message