/*
The?MIT?License?（MIT）

Copyright?（c）?2015-??suhetao

Permission?is?hereby?granted?free?of?charge?to?any?person?obtaining?a?copy?of
this?software?and?associated?documentation?files?（the?“Software“）?to?deal?in
the?Software?without?restriction?including?without?limitation?the?rights?to
use?copy?modify?merge?publish?distribute?sublicense?and/or?sell?copies?of
the?Software?and?to?permit?persons?to?whom?the?Software?is?furnished?to?do?so
subject?to?the?following?conditions:

The?above?copyright?notice?and?this?permission?notice?shall?be?included?in?all
copies?or?substantial?portions?of?the?Software.

THE?SOFTWARE?IS?PROVIDED?“AS?IS“?WITHOUT?WARRANTY?OF?ANY?KIND?EXPRESS?OR
IMPLIED?INCLUDING?BUT?NOT?LIMITED?TO?THE?WARRANTIES?OF?MERCHANTABILITY?FITNESS
FOR?A?PARTICULAR?PURPOSE?AND?NONINFRINGEMENT.?IN?NO?EVENT?SHALL?THE?AUTHORS?OR
COPYRIGHT?HOLDERS?BE?LIABLE?FOR?ANY?CLAIM?DAMAGES?OR?OTHER?LIABILITY?WHETHER
IN?AN?ACTION?OF?CONTRACT?TORT?OR?OTHERWISE?ARISING?FROM?OUT?OF?OR?IN
CONNECTION?WITH?THE?SOFTWARE?OR?THE?USE?OR?OTHER?DEALINGS?IN?THE?SOFTWARE.
*/

#include?“Control.h“
#include?“FastMath.h“

int?Matrix_Inv3x3（float*?A）
{
	//?det?=?a11（a33a22-a32a23）-a21（a33a12-a32a13）+a31（a23a12-a22a13）
	//?det?=?a00（a22a11-a21a12）-a10（a22a01-a21a02）+a20（a12a01-a11a02）
	float?det;
	float?M[9];
	//?Invert?the?matrix
	/*
	|?a11?a12?a13?|-1?|?a33a22-a32a23?-（a33a12-a32a13）?a23a12-a22a13?|
	|?a21?a22?a23?|?=?1/DET?*?|?-（a33a21-a31a23）?a33a11-a31a13?-（a23a11-a21a13）?|
	|?a31?a32?a33?|?|?a32a21-a31a22?-（a32a11-a31a12）?a22a11-a21a12?|
	|?a00?a01?a02?|-1?|?a22a11-a21a12?-（a22a01-a21a02）?a12a01-a11a02?|
	|?a10?a11?a12?|?=?1/DET?*?|?-（a22a10-a20a12）?a22a00-a20a02?-（a12a00-a10a02）?|
	|?a20?a21?a22?|?|?a21a10-a20a11?-（a21a00-a20a01）?a11a00-a10a01?|
	*/
	det??=?A[0]?*?（A[8]?*?A[4]?-?A[7]?*?A[5]）?-
		A[3]?*?（A[8]?*?A[1]?-?A[7]?*?A[2]）?+
		A[6]?*?（A[5]?*?A[1]?-?A[4]?*?A[2]）;
	//?Row?1
	//?M[0]?=?（a22a11-a21a12）/det;
	M[0]?=?（A[8]?*?A[4]?-?A[7]?*?A[5]）?/?det;
	//?M[1]?=?-（a22a01-a21a02）/det;
	M[1]?=?-（A[8]?*?A[1]?-?A[7]?*?A[2]）?/?det;
	//?M[2]?=?（a12a01-a11a02）/det;
	M[2]?=?（A[5]?*?A[1]?-?A[4]?*?A[2]）?/?det;
	//?Row?2
	//?M[3]?=?-（a22a10-a20a12）/det;
	M[3]?=?-（A[8]?*?A[3]?-?A[6]?*?A[5]）?/?det;
	//?M[4]?=?（a22a00-a20a02）/det;
	M[4]?=?（A[8]?*?A[0]?-?A[6]?*?A[2]）?/?det;
	//?M[5]?=?-（a12a00-a10a02）/det;
	M[5]?=?-（A[5]?*?A[0]?-?A[3]?*?A[2]）?/?det;
	//?Row?3
	//?M[6]?=?（a21a10-a20a11）/det;
	M[6]?=?（A[7]?*?A[3]?-?A[6]?*?A[4]）?/?det;
	//?M[7]?=?-（a21a00-a20a01）/det;
	M[7]?=?-（A[7]?*?A[0]?-?A[6]?*?A[1]）?/?det;
	//?M[8]?=?（a11a00-a10a01）/det;
	M[8]?=?（A[4]?*?A[0]?-?A[3]?*?A[1]）?/?det;

	A[0]?=?M[0];?A[1]?=?M[1];?A[2]?=?M[2];
	A[3]?=?M[3];?A[4]?=?M[4];?A[5]?=?M[5];
	A[6]?=?M[6];?A[7]?=?M[7];?A[8]?=?M[8];
	
	return?1;
}

//sweep
void?Matrix_Inv（float?*A?int?n）
{
	float?d;
	int?i?j?k;
	int?kn?kk;
	int?ln?lk;

	for?（k?=?0;?k?		kn?=?k?*?n;
		kk?=?kn?+?k;

		d?=?1.0f?/?A[kk];
		A[kk]?=?d;

		for?（i?=?0;?i?			if?（i?!=?k）{
				A[kn?+?i]?*=?-d;
			}
		}

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2015-08-06?15:26??stm32f4_mpu9250-master\
?????文件?????????223??2015-08-06?15:26??stm32f4_mpu9250-master\.gitignore
?????目錄???????????0??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\
?????目錄???????????0??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\inc\
?????文件????????4162??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\inc\CKF.h
?????文件????????2578??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\inc\Control.h
?????文件????????4998??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\inc\Double.h
?????文件????????3729??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\inc\EKF.h
?????文件????????3805??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\inc\INS_EKF.h
?????文件????????2220??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\inc\PID.h
?????文件????????2390??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\inc\Quaternion.h
?????文件????????4908??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\inc\SRCKF.h
?????文件????????4118??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\inc\UKF.h
?????目錄???????????0??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\src\
?????文件???????18341??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\src\CKF.C
?????文件????????7504??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\src\Control.c
?????文件???????13341??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\src\EKF.c
?????文件???????22460??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\src\INS_EKF.c
?????文件????????1101??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\src\PID.c
?????文件????????7901??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\src\Quaternion.c
?????文件???????14204??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\src\SRCKF.c
?????文件???????16329??2015-08-06?15:26??stm32f4_mpu9250-master\Algorithm\src\UKF.c
?????目錄???????????0??2015-08-06?15:26??stm32f4_mpu9250-master\Application\
?????目錄???????????0??2015-08-06?15:26??stm32f4_mpu9250-master\Application\inc\
?????文件????????1167??2015-08-06?15:26??stm32f4_mpu9250-master\Application\inc\stm32f4_common.h
?????文件????????1219??2015-08-06?15:26??stm32f4_mpu9250-master\Application\inc\stm32f4_crc.h
?????文件????????1308??2015-08-06?15:26??stm32f4_mpu9250-master\Application\inc\stm32f4_delay.h
?????文件????????1928??2015-08-06?15:26??stm32f4_mpu9250-master\Application\inc\stm32f4_dmp.h
?????文件????????1859??2015-08-06?15:26??stm32f4_mpu9250-master\Application\inc\stm32f4_exti.h
?????文件????????1137??2015-08-06?15:26??stm32f4_mpu9250-master\Application\inc\stm32f4_gps.h
?????文件???????10610??2015-08-06?15:26??stm32f4_mpu9250-master\Application\inc\stm32f4_mpu9250.h
............此處省略224個文件信息