Replacing Matrix operations (71f850b0) · Commits · Elphel / motosat

Matrix.php

0 → 100644

+302 −0

Original line number	Original line	Diff line number	Diff line
			<?php

			class Matrix
			{
			private $M; // this matrix as a 2-D array
			private $Tol; // used in LUPDecompose
			// LUP Decomposition, calculated on deamand (before first use of LUPSolve, LUPInvert, LUPDeterminant)
			private $A = null; // LU decomposition of the matrix
			private $P = null; // permutation array as defined in Wikipedia code:
			public function __construct($M, $Tol = 1E-6){
			// print("__construct\n");
			if (is_array($M)){
			$this->M = $M;
			if (!is_array($M[0])){ // single-column array
			for ($i = 0; $i< sizeof($M); $i++){
			$this->M[i] = array($M[i]);
			}
			}
			}
			$this->Tol = $Tol;
			}
			public function get(){
			return $this->M;
			}

			public function getColumnPackedCopy(){

			}

			public function times($B){
			if ($B instanceof Matrix){
			$B = $B->get();
			}
			if (is_array($B)) {
			$rows1 = sizeof($this->M);
			$cols1 = sizeof($this->M[0]);
			if ($cols1 != sizeof($B)){
			throw new Exception('Dimensions mismatch.');
			}
			$cols2 = sizeof($B[0]);
			$R = $this->zeroMatrix($rows1, $cols2);
			for ($i = 0; $i < $rows1; $i++){
			for ($j = 0; $j < $cols2; $j++){
			for ($k = 0; $k < $cols1; $k++){
			$R[$i][$j] += $this->M[$i][$k] * $B[$k][$j];
			}
			}
			}
			} else { // times scalar
			$R=$this->M;
			$rows = sizeof($this->M);
			$cols = sizeof($this->M[0]);
			for ($i = 0; $i < $rows; $i++){
			for ($j = 0; $j < $cols; $j++){
			$R[$i][$j] *= $B;
			}
			}
			}
			return new Matrix ($R, $this->Tol);
			}

			public function plus($B) {
			$R = $this->M;
			$rows = sizeof($this->M);
			$cols = sizeof($this->M[0]);
			if (($rows != sizeof($B[0])) \|\| ($cols != sizeof($B[0]))){
			throw new Exception('Dimensions mismatch.');
			}
			for ($i = 0; $i < $rows; $i ++) {
			for ($j = 0; $j < $cols; $j ++) {
			$R[$i][$j] += $B[$i][$j];
			}
			}
			return new Matrix ($R, $this->Tol);
			}


			public function print($debugFile = null, $decimals = 6)
			{
			if ($debugFile === null){
			$debugFile = fopen('php://stdout', 'w');
			}
			foreach ($this->M as $row) {
			fprintf($debugFile, "[");
			foreach ($row as $i) {
			fprintf($debugFile, "\t" . sprintf("%01.{$decimals}f", round($i, $decimals)));
			}
			fprintf($debugFile, "\t]\n");
			}
			}


			/*
			* The permutation matrix is not stored as a matrix, but in an integer vector P of size N+1
			* containing column indexes where the permutation matrix has "1". The last element P[N]=S+N,
			* where S is the number of row exchanges needed for determinant computation, det(P)=(-1)^S
			*/
			// Ported from C code in Wikipedia https://en.wikipedia.org/wiki/LU_decomposition
			/* INPUT: A - array of pointers to rows of a square matrix having dimension N
			* Tol - small tolerance number to detect failure when the matrix is near degenerate
			* OUTPUT: Matrix A is changed, it contains a copy of both matrices L-E and U as A=(L-E)+U such that PA=LU.
			* The permutation matrix is not stored as a matrix, but in an integer vector P of size N+1
			* containing column indexes where the permutation matrix has "1". The last element P[N]=S+N,
			* where S is the number of row exchanges needed for determinant computation, det(P)=(-1)^S
			*/
			/**
			* Perform LU decomposition
			* @param $A - square 2D array as input matrix, see Wikipedia comments above
			* @param $Tol - tolerance to catch singilar matrix
			* @return - array $P for permutation matrix on success, null on failure (singular)
			*/

			public function LUPDecompose (){
			$N = sizeof($this->M);
			$this->A = $this->M;
			$this->P = array_fill(0,$N+1, 0.0);
			for ($i = 0; $i <= $N; $i++) $this->P[$i] = $i; //Unit permutation matrix, P[N] initialized with N
			for ($i = 0; $i < $N; $i++) {
			$maxA = 0.0;
			$imax = $i;
			for ($k = $i; $k < $N; $k++) {
			$absA = abs($this->A[$k][$i]);
			if ($absA > $maxA) {
			$maxA = $absA;
			$imax = $k;
			}
			}
			if ($maxA < $this->Tol) {
			throw new Exception('Matrix is degenerate.'); //failure, matrix is degenerate
			}

			if ($imax != $i) {
			//pivoting P
			$j = $this->P[$i];
			$this->P[$i] = $this->P[$imax];
			$this->P[$imax] = $j;

			//pivoting rows of A
			$ptr = $this->A[$i];
			$this->A[$i] = $this->A[$imax];
			$this->A[$imax] = $ptr;

			//counting pivots starting from N (for determinant)
			$this->P[$N]++;
			}

			for ($j = $i + 1; $j < $N; $j++) {
			$this->A[$j][$i] /= $this->A[$i][$i];

			for ($k = $i + 1; $k < $N; $k++)
			$this->A[$j][$k] -= $this->A[$j][$i] * $this->A[$i][$k];
			}
			}
			}
			/* INPUT: A,P filled in LUPDecompose; b - rhs vector; N - dimension
			* OUTPUT: x - solution vector of A*x=b
			*/
			public function solve($b) {
			if ($this->A === null){
			$this->LUPDecompose();
			}
			if ($b instanceof Matrix){
			$b = $b->get();
			}

			$N = sizeof($this->M);
			$x = array_fill(0,$N,0.0);
			for ($i = 0; $i < $N; $i++) {
			$x[$i] = $b[$this->P[$i]];
			for ($k = 0; $k < $i; $k++)
			$x[$i] -= $this->M[$i][$k] * $x[$k];
			}
			for ($i = $N - 1; $i >= 0; $i--) {
			for ($k = $i + 1; $k < $N; $k++)
			$x[$i] -= $this->M[$i][$k] * $x[$k];
			$x[$i] = $x[$i] / $this->M[$i][$i];
			}
			return new Matrix ($x, $this->Tol);
			}

			/*
			* INPUT: A,P filled in LUPDecompose; N - dimension
			* OUTPUT: IA is the inverse of the initial matrix
			*/
			public function invert()
			{
			if ($this->A === null){
			$this->LUPDecompose();
			}
			$N = sizeof($this->A);
			$IA = $this->zeroMatrix($N, $N);
			for ($j = 0; $j < $N; $j ++) {
			for ($i = 0; $i < $N; $i ++) {
			if ($this->P[$i] == $j)
			$IA[$i][$j] = 1.0;
			else
			$IA[$i][$j] = 0.0;
			for ($k = 0; $k < $i; $k ++)
			$IA[$i][$j] -= $this->A[$i][$k] * $IA[$k][$j];
			}
			for ($i = $N - 1; $i >= 0; $i --) {
			for ($k = $i + 1; $k < $N; $k ++)
			$IA[$i][$j] -= $this->A[$i][$k] * $IA[$k][$j];
			$IA[$i][$j] = $IA[$i][$j] / $this->A[$i][$i];
			}
			}
			return new Matrix ($IA, $this->Tol);
			}

			/*
			* INPUT: A,P filled in LUPDecompose; N - dimension.
			* OUTPUT: Function returns the determinant of the initial matrix
			*/
			public function det()
			{
			if ($this->A === null){
			$this->LUPDecompose();
			}
			$N = sizeof($this->A);
			$det = $this->A[0][0];
			for ($i = 1; $i < $N; $i ++)
			$det *= $this->A[$i][$i];
			if (($this->P[$N] - $N) % 2 == 0)
			return $det;
			else
			return - $det;
			}

			public function isNonsingular(){
			if ($this->A === null){
			try {
			$this->LUPDecompose();
			} catch (Exception $e){
			return 0; // Singular
			}
			}
			return ($this->det())!=0.0; // Non-singular
			}

			public static 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;
			}

			public static function identityMatrix($n)
			{
			$I = array();
			for ($i = 0; $i < $n; ++ $i) {
			for ($j = 0; $j < $n; ++ $j) {
			$I[$i][$j] = ($i == $j) ? 1 : 0;
			}
			}
			return $I;
			}

			}

			// test code here
			$A = array(
			array( 10, -15, 30, 6, -8 ),
			array( 0, -4, 60, 11, -5 ),
			array( 8, 9, 2, 3, 7 ),
			array( 25, 10, -9, 9, 0 ),
			array( 13, 3, -12, 5, 2 ),
			);
			$SING = array(
			array( 10, -15, 30, 6, -8 ),
			array( 0, -4, 60, 11, -5 ),
			array( 8, 9, 2, 3, 7 ),
			array( 25, 10, -9, 9, 0 ),
			array( 10, -15, 30, 6, -8 ),
			);


			$M1 = new Matrix($A);
			print("\nM1=\n");
			$M1->print();
			//exit(0);
			$IM1 = $M1->invert();
			print("\nIM1=\n");
			$IM1->print();
			$M2 = $IM1->invert();
			print("\nM2=\n");
			$M2->print();

			$det= $M1->det();
			print("\ndet(M1)=".$det."\n");
			$idet= $IM1->det();
			print("\ndet(IM1)=".$idet."\n");
			print("\ndet(M1)det(IM1)=".($det $idet)."\n");

			$MS = new Matrix($SING);
			print("\nM1 nonsingular=".$M1->isNonsingular()."\n");
			print("\nMS nonsingular=".$MS->isNonsingular()."\n");

			print("\nM1 * IM1=\n");
			$M1->times($IM1)->print();

			?>

PolynomialApproximation.php

+23 −190

Original line number	Original line	Diff line number	Diff line
	<?php		<?php
	//require_once ('test-matrix-invert.php'); // matrix inversion		require_once ('Matrix.php'); // matrix inversion
	class PolynomialApproximation		class PolynomialApproximation
	{		{
	// function __construct() {		// function __construct() {
	@@ -39,32 +39,36 @@ class PolynomialApproximation
	}		}
	}		}
	}		}
	$M = $this->zeroMatrix($N+1,$N+1);		$aM = Matrix::zeroMatrix($N+1,$N+1);
	$B = $this->zeroMatrix($N+1,1);		$aB = Matrix::zeroMatrix($N+1,1);
	for ($i=0; $i <= $N; $i++) {		for ($i=0; $i <= $N; $i++) {
	$B[$i][0] = $SF[$i];		$aB[$i][0] = $SF[$i];
	for ($j = 0; $j <= $N; $j++) $M[$i][$j]= $S[$i + $j];		for ($j = 0; $j <= $N; $j++) $aM[$i][$j]= $S[$i + $j];
	}		}
	$N1 = $N;		$N1 = $N;
	// TODO: use try/catch with solve		$M = new Matrix($aM);
			$B = new Matrix($aB);

	if ($this->debugLevel > 1){		if ($this->debugLevel > 1){
	fprintf($this->debugFile,		fprintf($this->debugFile,
	"polynomialApproximation1d(data,".$N.") M:\n");		"polynomialApproximation1d(data,".$N.") M:\n");
	$this->print_matrix($this->debugFile, $M);		$M->print($this->debugFile);
	fprintf($this->debugFile,		fprintf($this->debugFile,
	"polynomialApproximation1d() B:\n");		"polynomialApproximation1d() B:\n");
	$this->print_matrix($this->debugFile, $B);		$B->print($this->debugFile);
	}		}
	// while (!(new LUDecomposition(M)).isNonsingular() && (N1>0)){		// while (!(new LUDecomposition(M)).isNonsingular() && (N1>0)){
	$df = ($this->debugLevel > 2) ? $this->debugFile : null;		// $df = ($this->debugLevel > 2) ? $this->debugFile : null;
	while ($N1 >= 0) { // make N=0 legal ?		while ($N1 >= 0) { // make N=0 legal ?
	try {		try {
	$M_inv = $this->matrixInvert($M,$df);		// $M_inv = $M.invert();
	$R = $this->mmul($M_inv, $B);		// $R = $M_inv->times($B);
			$R = $M->solve($B);

	if ($this->debugLevel > 1){		if ($this->debugLevel > 1){
	fprintf($this->debugFile,		fprintf($this->debugFile,
	"polynomialApproximation1d() solution=\n");		"polynomialApproximation1d() solution=\n");
	$this->print_matrix($this->debugFile, $R);		$R->print($this->debugFile);
	}		}
	$result=array_fill(0,$N+1,0); // new double [N+1];		$result=array_fill(0,$N+1,0); // new double [N+1];
	for ($i = 0; $i <= $N1; $i++){		for ($i = 0; $i <= $N1; $i++){
	@@ -78,193 +82,22 @@ class PolynomialApproximation
	return null;		return null;
	}		}
	$N1--;		$N1--;
	$M1 = $this->zeroMatrix($N+1,$N+1);		$aM1 = Matrix::zeroMatrix($N+1,$N+1);
	$B1 = $this->zeroMatrix($N+1,1);		$aB1 = Matrix::zeroMatrix($N+1,1);
	for ($i = 0; $i <= $N1; $i++) {		for ($i = 0; $i <= $N1; $i++) {
	$B1[$i][0] = $B[$i][0];		$aB1[$i][0] = $aB[$i][0];
	for ($j = 0; $j <= $N1; $j++){		for ($j = 0; $j <= $N1; $j++){
	$M1[$i][$j] = $M[$i][$j];		$aM1[$i][$j] = $aM[$i][$j];
	}		}
	}		}
	$M = $M1;		$M = new Matrix($aM1);
	$B = $B1;		$B = new Matrix($aB1);
	}		}
	}		}
	return null;		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
	if ($debugFile !== null) {
	fprintf($debugFile, "\Input matrix for inversion:\n");
	$this->print_matrix($debugFile, $A);
	}


	$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]; // division by zero
	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;
	}






	}

test_01.php

+2 −2

Original line number	Original line	Diff line number	Diff line
	@@ -51,10 +51,10 @@ http://192.168.1.250/motor.xml?Action=Stop&Distance=100&Units=1 - stop

	//php > var_dump($DishAngle);		//php > var_dump($DishAngle);
	//array(3) {[0]=> float(9.97), [1]=> float(0), [2]=> float(0)}		//array(3) {[0]=> float(9.97), [1]=> float(0), [2]=> float(0)}
	$dbg_file = fopen("/home/eyesis/git/motosat/attic/logs/test03.log","w");		$dbg_file = fopen("/home/eyesis/git/motosat/attic/logs/test04.log","w");
	fprintf($dbg_file,"test log\n");		fprintf($dbg_file,"test log\n");

	if (false) {		if (true) {
	$data = Array(5,6,7,9,9,10,11,11,12,12,11,11,10,10,10,36,40,45,47,51,51,51,49,48,43,38,30,30,30,10,10,10,10);		$data = Array(5,6,7,9,9,10,11,11,12,12,11,11,10,10,10,36,40,45,47,51,51,51,49,48,43,38,30,30,30,10,10,10,10);
	$rslt = findMax1d($data, $USABLE_FRACT, $USABLE_POW, $ARGMAX_OURSIDE);		$rslt = findMax1d($data, $USABLE_FRACT, $USABLE_POW, $ARGMAX_OURSIDE);
	print_r($rslt);		print_r($rslt);

Admin message