WebSVN – dashGPS – Blame – /branches/dashGPS-bmp/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_inverse_f32.c

Rev	Author	Line No.	Line
2	mjames	1	/* ----------------------------------------------------------------------
		2	* Project: CMSIS DSP Library
		3	* Title: arm_mat_inverse_f32.c
		4	* Description: Floating-point matrix inverse
		5	*
		6	* $Date: 27. January 2017
		7	* $Revision: V.1.5.1
		8	*
		9	* Target Processor: Cortex-M cores
		10	* -------------------------------------------------------------------- */
		11	/*
		12	* Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
		13	*
		14	* SPDX-License-Identifier: Apache-2.0
		15	*
		16	* Licensed under the Apache License, Version 2.0 (the License); you may
		17	* not use this file except in compliance with the License.
		18	* You may obtain a copy of the License at
		19	*
		20	* www.apache.org/licenses/LICENSE-2.0
		21	*
		22	* Unless required by applicable law or agreed to in writing, software
		23	* distributed under the License is distributed on an AS IS BASIS, WITHOUT
		24	* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
		25	* See the License for the specific language governing permissions and
		26	* limitations under the License.
		27	*/
		28
		29	#include "arm_math.h"
		30
		31	/**
		32	* @ingroup groupMatrix
		33	*/
		34
		35	/**
		36	* @defgroup MatrixInv Matrix Inverse
		37	*
		38	* Computes the inverse of a matrix.
		39	*
		40	* The inverse is defined only if the input matrix is square and non-singular (the determinant
		41	* is non-zero). The function checks that the input and output matrices are square and of the
		42	* same size.
		43	*
		44	* Matrix inversion is numerically sensitive and the CMSIS DSP library only supports matrix
		45	* inversion of floating-point matrices.
		46	*
		47	* \par Algorithm
		48	* The Gauss-Jordan method is used to find the inverse.
		49	* The algorithm performs a sequence of elementary row-operations until it
		50	* reduces the input matrix to an identity matrix. Applying the same sequence
		51	* of elementary row-operations to an identity matrix yields the inverse matrix.
		52	* If the input matrix is singular, then the algorithm terminates and returns error status
		53	* <code>ARM_MATH_SINGULAR</code>.
		54	* \image html MatrixInverse.gif "Matrix Inverse of a 3 x 3 matrix using Gauss-Jordan Method"
		55	*/
		56
		57	/**
		58	* @addtogroup MatrixInv
		59	* @{
		60	*/
		61
		62	/**
		63	* @brief Floating-point matrix inverse.
		64	* @param[in] *pSrc points to input matrix structure
		65	* @param[out] *pDst points to output matrix structure
		66	* @return The function returns
		67	* <code>ARM_MATH_SIZE_MISMATCH</code> if the input matrix is not square or if the size
		68	* of the output matrix does not match the size of the input matrix.
		69	* If the input matrix is found to be singular (non-invertible), then the function returns
		70	* <code>ARM_MATH_SINGULAR</code>. Otherwise, the function returns <code>ARM_MATH_SUCCESS</code>.
		71	*/
		72
		73	arm_status arm_mat_inverse_f32(
		74	const arm_matrix_instance_f32 * pSrc,
		75	arm_matrix_instance_f32 * pDst)
		76	{
		77	float32_t pIn = pSrc->pData; / input data matrix pointer */
		78	float32_t pOut = pDst->pData; / output data matrix pointer */
		79	float32_t pInT1, pInT2; /* Temporary input data matrix pointer */
		80	float32_t pOutT1, pOutT2; /* Temporary output data matrix pointer */
		81	float32_t pPivotRowIn, pPRT_in, pPivotRowDst, pPRT_pDst; /* Temporary input and output data matrix pointer */
		82	uint32_t numRows = pSrc->numRows; /* Number of rows in the matrix */
		83	uint32_t numCols = pSrc->numCols; /* Number of Cols in the matrix */
		84
		85	#if defined (ARM_MATH_DSP)
		86	float32_t maxC; /* maximum value in the column */
		87
		88	/* Run the below code for Cortex-M4 and Cortex-M3 */
		89
		90	float32_t Xchg, in = 0.0f, in1; /* Temporary input values */
		91	uint32_t i, rowCnt, flag = 0U, j, loopCnt, k, l; /* loop counters */
		92	arm_status status; /* status of matrix inverse */
		93
		94	#ifdef ARM_MATH_MATRIX_CHECK
		95
		96
		97	/* Check for matrix mismatch condition */
		98	if ((pSrc->numRows != pSrc->numCols) \|\| (pDst->numRows != pDst->numCols)
		99	\|\| (pSrc->numRows != pDst->numRows))
		100	{
		101	/* Set status as ARM_MATH_SIZE_MISMATCH */
		102	status = ARM_MATH_SIZE_MISMATCH;
		103	}
		104	else
		105	#endif /* #ifdef ARM_MATH_MATRIX_CHECK */
		106
		107	{
		108
		109	/*--------------------------------------------------------------------------------------------------------------
		110	* Matrix Inverse can be solved using elementary row operations.
		111	*
		112	* Gauss-Jordan Method:
		113	*
		114	* 1. First combine the identity matrix and the input matrix separated by a bar to form an
		115	* augmented matrix as follows:
		116	* _ _ _ _
		117	* \| a11 a12 \| 1 0 \| \| X11 X12 \|
		118	* \| \| \| = \| \|
		119	* \|_ a21 a22 \| 0 1 _\| \|_ X21 X21 _\|
		120	*
		121	* 2. In our implementation, pDst Matrix is used as identity matrix.
		122	*
		123	* 3. Begin with the first row. Let i = 1.
		124	*
		125	* 4. Check to see if the pivot for column i is the greatest of the column.
		126	* The pivot is the element of the main diagonal that is on the current row.
		127	* For instance, if working with row i, then the pivot element is aii.
		128	* If the pivot is not the most significant of the columns, exchange that row with a row
		129	* below it that does contain the most significant value in column i. If the most
		130	* significant value of the column is zero, then an inverse to that matrix does not exist.
		131	* The most significant value of the column is the absolute maximum.
		132	*
		133	* 5. Divide every element of row i by the pivot.
		134	*
		135	* 6. For every row below and row i, replace that row with the sum of that row and
		136	* a multiple of row i so that each new element in column i below row i is zero.
		137	*
		138	* 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros
		139	* for every element below and above the main diagonal.
		140	*
		141	* 8. Now an identical matrix is formed to the left of the bar(input matrix, pSrc).
		142	* Therefore, the matrix to the right of the bar is our solution(pDst matrix, pDst).
		143	----------------------------------------------------------------------------------------------------------------/
		144
		145	/* Working pointer for destination matrix */
		146	pOutT1 = pOut;
		147
		148	/* Loop over the number of rows */
		149	rowCnt = numRows;
		150
		151	/* Making the destination matrix as identity matrix */
		152	while (rowCnt > 0U)
		153	{
		154	/* Writing all zeroes in lower triangle of the destination matrix */
		155	j = numRows - rowCnt;
		156	while (j > 0U)
		157	{
		158	*pOutT1++ = 0.0f;
		159	j--;
		160	}
		161
		162	/* Writing all ones in the diagonal of the destination matrix */
		163	*pOutT1++ = 1.0f;
		164
		165	/* Writing all zeroes in upper triangle of the destination matrix */
		166	j = rowCnt - 1U;
		167	while (j > 0U)
		168	{
		169	*pOutT1++ = 0.0f;
		170	j--;
		171	}
		172
		173	/* Decrement the loop counter */
		174	rowCnt--;
		175	}
		176
		177	/* Loop over the number of columns of the input matrix.
		178	All the elements in each column are processed by the row operations */
		179	loopCnt = numCols;
		180
		181	/* Index modifier to navigate through the columns */
		182	l = 0U;
		183
		184	while (loopCnt > 0U)
		185	{
		186	/* Check if the pivot element is zero..
		187	* If it is zero then interchange the row with non zero row below.
		188	* If there is no non zero element to replace in the rows below,
		189	* then the matrix is Singular. */
		190
		191	/* Working pointer for the input matrix that points
		192	* to the pivot element of the particular row */
		193	pInT1 = pIn + (l * numCols);
		194
		195	/* Working pointer for the destination matrix that points
		196	* to the pivot element of the particular row */
		197	pOutT1 = pOut + (l * numCols);
		198
		199	/* Temporary variable to hold the pivot value */
		200	in = *pInT1;
		201
		202	/* Grab the most significant value from column l */
		203	maxC = 0;
		204	for (i = l; i < numRows; i++)
		205	{
		206	maxC = pInT1 > 0 ? (pInT1 > maxC ? pInT1 : maxC) : (-pInT1 > maxC ? -*pInT1 : maxC);
		207	pInT1 += numCols;
		208	}
		209
		210	/* Update the status if the matrix is singular */
		211	if (maxC == 0.0f)
		212	{
		213	return ARM_MATH_SINGULAR;
		214	}
		215
		216	/* Restore pInT1 */
		217	pInT1 = pIn;
		218
		219	/* Destination pointer modifier */
		220	k = 1U;
		221
		222	/* Check if the pivot element is the most significant of the column */
		223	if ( (in > 0.0f ? in : -in) != maxC)
		224	{
		225	/* Loop over the number rows present below */
		226	i = numRows - (l + 1U);
		227
		228	while (i > 0U)
		229	{
		230	/* Update the input and destination pointers */
		231	pInT2 = pInT1 + (numCols * l);
		232	pOutT2 = pOutT1 + (numCols * k);
		233
		234	/* Look for the most significant element to
		235	* replace in the rows below */
		236	if ((pInT2 > 0.0f ? pInT2: -*pInT2) == maxC)
		237	{
		238	/* Loop over number of columns
		239	* to the right of the pilot element */
		240	j = numCols - l;
		241
		242	while (j > 0U)
		243	{
		244	/* Exchange the row elements of the input matrix */
		245	Xchg = *pInT2;
		246	pInT2++ = pInT1;
		247	*pInT1++ = Xchg;
		248
		249	/* Decrement the loop counter */
		250	j--;
		251	}
		252
		253	/* Loop over number of columns of the destination matrix */
		254	j = numCols;
		255
		256	while (j > 0U)
		257	{
		258	/* Exchange the row elements of the destination matrix */
		259	Xchg = *pOutT2;
		260	pOutT2++ = pOutT1;
		261	*pOutT1++ = Xchg;
		262
		263	/* Decrement the loop counter */
		264	j--;
		265	}
		266
		267	/* Flag to indicate whether exchange is done or not */
		268	flag = 1U;
		269
		270	/* Break after exchange is done */
		271	break;
		272	}
		273
		274	/* Update the destination pointer modifier */
		275	k++;
		276
		277	/* Decrement the loop counter */
		278	i--;
		279	}
		280	}
		281
		282	/* Update the status if the matrix is singular */
		283	if ((flag != 1U) && (in == 0.0f))
		284	{
		285	return ARM_MATH_SINGULAR;
		286	}
		287
		288	/* Points to the pivot row of input and destination matrices */
		289	pPivotRowIn = pIn + (l * numCols);
		290	pPivotRowDst = pOut + (l * numCols);
		291
		292	/* Temporary pointers to the pivot row pointers */
		293	pInT1 = pPivotRowIn;
		294	pInT2 = pPivotRowDst;
		295
		296	/* Pivot element of the row */
		297	in = *pPivotRowIn;
		298
		299	/* Loop over number of columns
		300	* to the right of the pilot element */
		301	j = (numCols - l);
		302
		303	while (j > 0U)
		304	{
		305	/* Divide each element of the row of the input matrix
		306	* by the pivot element */
		307	in1 = *pInT1;
		308	*pInT1++ = in1 / in;
		309
		310	/* Decrement the loop counter */
		311	j--;
		312	}
		313
		314	/* Loop over number of columns of the destination matrix */
		315	j = numCols;
		316
		317	while (j > 0U)
		318	{
		319	/* Divide each element of the row of the destination matrix
		320	* by the pivot element */
		321	in1 = *pInT2;
		322	*pInT2++ = in1 / in;
		323
		324	/* Decrement the loop counter */
		325	j--;
		326	}
		327
		328	/* Replace the rows with the sum of that row and a multiple of row i
		329	* so that each new element in column i above row i is zero.*/
		330
		331	/* Temporary pointers for input and destination matrices */
		332	pInT1 = pIn;
		333	pInT2 = pOut;
		334
		335	/* index used to check for pivot element */
		336	i = 0U;
		337
		338	/* Loop over number of rows */
		339	/* to be replaced by the sum of that row and a multiple of row i */
		340	k = numRows;
		341
		342	while (k > 0U)
		343	{
		344	/* Check for the pivot element */
		345	if (i == l)
		346	{
		347	/* If the processing element is the pivot element,
		348	only the columns to the right are to be processed */
		349	pInT1 += numCols - l;
		350
		351	pInT2 += numCols;
		352	}
		353	else
		354	{
		355	/* Element of the reference row */
		356	in = *pInT1;
		357
		358	/* Working pointers for input and destination pivot rows */
		359	pPRT_in = pPivotRowIn;
		360	pPRT_pDst = pPivotRowDst;
		361
		362	/* Loop over the number of columns to the right of the pivot element,
		363	to replace the elements in the input matrix */
		364	j = (numCols - l);
		365
		366	while (j > 0U)
		367	{
		368	/* Replace the element by the sum of that row
		369	and a multiple of the reference row */
		370	in1 = *pInT1;
		371	pInT1++ = in1 - (in *pPRT_in++);
		372
		373	/* Decrement the loop counter */
		374	j--;
		375	}
		376
		377	/* Loop over the number of columns to
		378	replace the elements in the destination matrix */
		379	j = numCols;
		380
		381	while (j > 0U)
		382	{
		383	/* Replace the element by the sum of that row
		384	and a multiple of the reference row */
		385	in1 = *pInT2;
		386	pInT2++ = in1 - (in *pPRT_pDst++);
		387
		388	/* Decrement the loop counter */
		389	j--;
		390	}
		391
		392	}
		393
		394	/* Increment the temporary input pointer */
		395	pInT1 = pInT1 + l;
		396
		397	/* Decrement the loop counter */
		398	k--;
		399
		400	/* Increment the pivot index */
		401	i++;
		402	}
		403
		404	/* Increment the input pointer */
		405	pIn++;
		406
		407	/* Decrement the loop counter */
		408	loopCnt--;
		409
		410	/* Increment the index modifier */
		411	l++;
		412	}
		413
		414
		415	#else
		416
		417	/* Run the below code for Cortex-M0 */
		418
		419	float32_t Xchg, in = 0.0f; /* Temporary input values */
		420	uint32_t i, rowCnt, flag = 0U, j, loopCnt, k, l; /* loop counters */
		421	arm_status status; /* status of matrix inverse */
		422
		423	#ifdef ARM_MATH_MATRIX_CHECK
		424
		425	/* Check for matrix mismatch condition */
		426	if ((pSrc->numRows != pSrc->numCols) \|\| (pDst->numRows != pDst->numCols)
		427	\|\| (pSrc->numRows != pDst->numRows))
		428	{
		429	/* Set status as ARM_MATH_SIZE_MISMATCH */
		430	status = ARM_MATH_SIZE_MISMATCH;
		431	}
		432	else
		433	#endif /* #ifdef ARM_MATH_MATRIX_CHECK */
		434	{
		435
		436	/*--------------------------------------------------------------------------------------------------------------
		437	* Matrix Inverse can be solved using elementary row operations.
		438	*
		439	* Gauss-Jordan Method:
		440	*
		441	* 1. First combine the identity matrix and the input matrix separated by a bar to form an
		442	* augmented matrix as follows:
		443	* _ _ _ _ _ _ _ _
		444	* \| \| a11 a12 \| \| \| 1 0 \| \| \| X11 X12 \|
		445	* \| \| \| \| \| \| \| = \| \|
		446	* \|_ \|_ a21 a22 _\| \| \|_0 1 _\| _\| \|_ X21 X21 _\|
		447	*
		448	* 2. In our implementation, pDst Matrix is used as identity matrix.
		449	*
		450	* 3. Begin with the first row. Let i = 1.
		451	*
		452	* 4. Check to see if the pivot for row i is zero.
		453	* The pivot is the element of the main diagonal that is on the current row.
		454	* For instance, if working with row i, then the pivot element is aii.
		455	* If the pivot is zero, exchange that row with a row below it that does not
		456	* contain a zero in column i. If this is not possible, then an inverse
		457	* to that matrix does not exist.
		458	*
		459	* 5. Divide every element of row i by the pivot.
		460	*
		461	* 6. For every row below and row i, replace that row with the sum of that row and
		462	* a multiple of row i so that each new element in column i below row i is zero.
		463	*
		464	* 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros
		465	* for every element below and above the main diagonal.
		466	*
		467	* 8. Now an identical matrix is formed to the left of the bar(input matrix, src).
		468	* Therefore, the matrix to the right of the bar is our solution(dst matrix, dst).
		469	----------------------------------------------------------------------------------------------------------------/
		470
		471	/* Working pointer for destination matrix */
		472	pOutT1 = pOut;
		473
		474	/* Loop over the number of rows */
		475	rowCnt = numRows;
		476
		477	/* Making the destination matrix as identity matrix */
		478	while (rowCnt > 0U)
		479	{
		480	/* Writing all zeroes in lower triangle of the destination matrix */
		481	j = numRows - rowCnt;
		482	while (j > 0U)
		483	{
		484	*pOutT1++ = 0.0f;
		485	j--;
		486	}
		487
		488	/* Writing all ones in the diagonal of the destination matrix */
		489	*pOutT1++ = 1.0f;
		490
		491	/* Writing all zeroes in upper triangle of the destination matrix */
		492	j = rowCnt - 1U;
		493	while (j > 0U)
		494	{
		495	*pOutT1++ = 0.0f;
		496	j--;
		497	}
		498
		499	/* Decrement the loop counter */
		500	rowCnt--;
		501	}
		502
		503	/* Loop over the number of columns of the input matrix.
		504	All the elements in each column are processed by the row operations */
		505	loopCnt = numCols;
		506
		507	/* Index modifier to navigate through the columns */
		508	l = 0U;
		509	//for(loopCnt = 0U; loopCnt < numCols; loopCnt++)
		510	while (loopCnt > 0U)
		511	{
		512	/* Check if the pivot element is zero..
		513	* If it is zero then interchange the row with non zero row below.
		514	* If there is no non zero element to replace in the rows below,
		515	* then the matrix is Singular. */
		516
		517	/* Working pointer for the input matrix that points
		518	* to the pivot element of the particular row */
		519	pInT1 = pIn + (l * numCols);
		520
		521	/* Working pointer for the destination matrix that points
		522	* to the pivot element of the particular row */
		523	pOutT1 = pOut + (l * numCols);
		524
		525	/* Temporary variable to hold the pivot value */
		526	in = *pInT1;
		527
		528	/* Destination pointer modifier */
		529	k = 1U;
		530
		531	/* Check if the pivot element is zero */
		532	if (*pInT1 == 0.0f)
		533	{
		534	/* Loop over the number rows present below */
		535	for (i = (l + 1U); i < numRows; i++)
		536	{
		537	/* Update the input and destination pointers */
		538	pInT2 = pInT1 + (numCols * l);
		539	pOutT2 = pOutT1 + (numCols * k);
		540
		541	/* Check if there is a non zero pivot element to
		542	* replace in the rows below */
		543	if (*pInT2 != 0.0f)
		544	{
		545	/* Loop over number of columns
		546	* to the right of the pilot element */
		547	for (j = 0U; j < (numCols - l); j++)
		548	{
		549	/* Exchange the row elements of the input matrix */
		550	Xchg = *pInT2;
		551	pInT2++ = pInT1;
		552	*pInT1++ = Xchg;
		553	}
		554
		555	for (j = 0U; j < numCols; j++)
		556	{
		557	Xchg = *pOutT2;
		558	pOutT2++ = pOutT1;
		559	*pOutT1++ = Xchg;
		560	}
		561
		562	/* Flag to indicate whether exchange is done or not */
		563	flag = 1U;
		564
		565	/* Break after exchange is done */
		566	break;
		567	}
		568
		569	/* Update the destination pointer modifier */
		570	k++;
		571	}
		572	}
		573
		574	/* Update the status if the matrix is singular */
		575	if ((flag != 1U) && (in == 0.0f))
		576	{
		577	return ARM_MATH_SINGULAR;
		578	}
		579
		580	/* Points to the pivot row of input and destination matrices */
		581	pPivotRowIn = pIn + (l * numCols);
		582	pPivotRowDst = pOut + (l * numCols);
		583
		584	/* Temporary pointers to the pivot row pointers */
		585	pInT1 = pPivotRowIn;
		586	pOutT1 = pPivotRowDst;
		587
		588	/* Pivot element of the row */
		589	in = (pIn + (l numCols));
		590
		591	/* Loop over number of columns
		592	* to the right of the pilot element */
		593	for (j = 0U; j < (numCols - l); j++)
		594	{
		595	/* Divide each element of the row of the input matrix
		596	* by the pivot element */
		597	pInT1 = pInT1 / in;
		598	pInT1++;
		599	}
		600	for (j = 0U; j < numCols; j++)
		601	{
		602	/* Divide each element of the row of the destination matrix
		603	* by the pivot element */
		604	pOutT1 = pOutT1 / in;
		605	pOutT1++;
		606	}
		607
		608	/* Replace the rows with the sum of that row and a multiple of row i
		609	* so that each new element in column i above row i is zero.*/
		610
		611	/* Temporary pointers for input and destination matrices */
		612	pInT1 = pIn;
		613	pOutT1 = pOut;
		614
		615	for (i = 0U; i < numRows; i++)
		616	{
		617	/* Check for the pivot element */
		618	if (i == l)
		619	{
		620	/* If the processing element is the pivot element,
		621	only the columns to the right are to be processed */
		622	pInT1 += numCols - l;
		623	pOutT1 += numCols;
		624	}
		625	else
		626	{
		627	/* Element of the reference row */
		628	in = *pInT1;
		629
		630	/* Working pointers for input and destination pivot rows */
		631	pPRT_in = pPivotRowIn;
		632	pPRT_pDst = pPivotRowDst;
		633
		634	/* Loop over the number of columns to the right of the pivot element,
		635	to replace the elements in the input matrix */
		636	for (j = 0U; j < (numCols - l); j++)
		637	{
		638	/* Replace the element by the sum of that row
		639	and a multiple of the reference row */
		640	pInT1 = pInT1 - (in * *pPRT_in++);
		641	pInT1++;
		642	}
		643	/* Loop over the number of columns to
		644	replace the elements in the destination matrix */
		645	for (j = 0U; j < numCols; j++)
		646	{
		647	/* Replace the element by the sum of that row
		648	and a multiple of the reference row */
		649	pOutT1 = pOutT1 - (in * *pPRT_pDst++);
		650	pOutT1++;
		651	}
		652
		653	}
		654	/* Increment the temporary input pointer */
		655	pInT1 = pInT1 + l;
		656	}
		657	/* Increment the input pointer */
		658	pIn++;
		659
		660	/* Decrement the loop counter */
		661	loopCnt--;
		662	/* Increment the index modifier */
		663	l++;
		664	}
		665
		666
		667	#endif /* #if defined (ARM_MATH_DSP) */
		668
		669	/* Set status as ARM_MATH_SUCCESS */
		670	status = ARM_MATH_SUCCESS;
		671
		672	if ((flag != 1U) && (in == 0.0f))
		673	{
		674	pIn = pSrc->pData;
		675	for (i = 0; i < numRows * numCols; i++)
		676	{
		677	if (pIn[i] != 0.0f)
		678	break;
		679	}
		680
		681	if (i == numRows * numCols)
		682	status = ARM_MATH_SINGULAR;
		683	}
		684	}
		685	/* Return to application */
		686	return (status);
		687	}
		688
		689	/**
		690	* @} end of MatrixInv group
		691	*/

Subversion Repositories dashGPS

(root)/branches/dashGPS-bmp/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_inverse_f32.c – Rev 18