使用扩展卡尔曼滤波（EKF）进行AHRS九轴姿态融合

AHRS九轴姿态融合 EKF滤波 卡尔曼滤波

在做九轴姿态融合的过程中，这里介绍一种融合算法，基于EKF的九轴姿态融合算法：
首先表明，该算法并非自己想到的，算法原理参考了这篇论文：Dale E. Schinstock的论文GPS-aided INS Solution for OpenPilot ，这篇论文真的写的非常棒，符号、公式很详细却并不啰嗦。
程序实现其实就是将EKF算法搬移到单片机上了而已，同时这个程序也不是我写的，非常感谢第七实验室开放了这段代码以供开发者学习，当然仅供学习使用。
我也就借花献佛，将这段代码进行了详细的注释，并调整了相关代码的顺序，同时，对代码背后的原理进行了简单解析，由于网上有的博文对这些知识的讲解良莠不齐，甚至有错误，因此我提供了基本无错误的相关知识的查询链接。这对于像我一样想学习EKF姿态融合的小白和专业知识不是很清晰并不知道从何学起的想必会大有裨益。
当然，EKF在姿态融合时当遭遇大噪声时的起始效果并不好，所以该函数的输入参数是经过初始滤波的，可以采用高低通滤波[实际代码可参见正点原子悟空四轴]【原理】、滑动窗口滤波（使用FIFO实现）、椭球拟合校正【原理 】 等方式对传感器输出数据进行初始滤波。

所有椭球拟合相关资料文档下载链接
《惯性导航 》+ 《卡尔曼滤波原理与应用》+ 四元数解算 + 旋转矩阵相关理论 PDF下载链接（这个好像是不能下载还是怎么的，过不了审核，需要的私信我）

相关知识：

卡尔曼滤波原理的通俗理解–>《卡尔曼滤波原理与应用》 3.13节 黄小平·著
卡尔曼滤波原理的通俗理解–>知乎
卡尔曼滤波器UD分解滤波算法的改进（防止矩阵求逆发散）
四元数表示姿态 -->《惯性导航》 9.2节 秦永元·著
左乘旋转矩阵和右乘旋转矩阵
EKF(扩展卡尔曼滤波流程)–>《卡尔曼滤波原理与应用》 4.2.1节 黄小平·著
EKF九轴姿态融合

/**************************实现函数********************************************
*函数原型:	 	void AHRS_AHRSupdate
*功　　能:	 	更新AHRS 更新四元数 
*输入参数： 	当前经过校正的测量值
*输入参数：		gx gy gz 三轴角速度 单位rad/s
				ax ay az 三轴加速度（在静态是为重力加速度的三轴投影，无单位，只需要比例关系）当需要加速度积分速度位移时需要单位
				mx my mz 三轴磁场强度 无单位 因为只需要比例关系 
*相关知识： //四元数
				①秦永元 《惯性导航 9.2节》
				②https://blog.csdn.net/shenshikexmu/article/details/53608224?locationNum=8&fps=1  【讲了左乘矩阵和右乘矩阵的差别】
			//扩展卡尔曼滤波
				①黄小平 《卡尔曼滤波原理与应用 4.2.1节》【EKF滤波流程】
*******************************************************************************/

void AHRS_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
	/*
	=============================================================================
	=============================================================================
	=============================================================================
	X = [q0 q1 q2 q3 w1 w2 w3]' 四元数 + 角速度偏移 认为两次采样中角速度偏移不变
	状态方程：
	X(k+1) = F*X(k)
	状态转移矩阵：
	F = |1_(4*4) + (dq/dt)*dt 		0_(4*3)	|
		|0_(3*4)				Eye_(3*3)	|(7*7)
	
	其中：	https://wenku.baidu.com/view/2f3d5bc0d5bbfd0a795673fa.html 【四元数微分方程的推导】
			https://wenku.baidu.com/view/b2f5ac27a5e9856a561260ce.html 【高阶四元数求导】《惯性导航》P302
				|0  -Wx -Wy  -Wz||q0|
	dq/dt = 1/2*|Wx  0   Wz  -Wy||q1|  Wx Wy Wz 是角速率 【此处为 1 阶微分】
				|Wy -Wz  0    Wx||q2|
				|Wz  Wy -Wx   0 ||q3|
	
	X(k+1) = 	F*X(k) = |1_(4*4) + (dq/dt)*dt 		 		0_(4*3)	|		*  	|q0 q1 q2 q3 q4 w1 w2 w3|'(7*1)	
						|0_(3*4)						Eye_(3*3)	|(7*7)
	=============================================================================
	=============================================================================
	=============================================================================
	Y = [ax ay az mx my mz]'    固联坐标系实测的重力加速度 + 固联坐标系实测的磁场强度 
	观测方程：
	Y(k+1) = H*X(k)
	观测转移矩阵：
			|	-q2*g			q3*g			-q0*g  			-q1*g			0	0  	0	|
			|	q1*g			q0*g  			q3*g  			q2*g			0	0  	0	|
	H = 2*	|	q0*g 	 		-q1*g			-q2*g 			q3*g			0	0  	0	|
			|bx*q0-bz*q2	bx*q1+bz*q3  	-bx*q2-bz*q0  -bx*q3+bz*q1			0	0  	0	|
			|-bx*q3+bz*q1	bx*q2+bz*q0  	bx*q1+bz*q3 	-bx*q0+bz*q2		0	0  	0	|
			|bx*q2+bz*q0	bx*q3-bz*q1  	bx*q0-bz*q2  	bx*q1+bz*q3			0	0  	0	|(6*7)
					
				|-q2*g				q3*g			-q0*g  			-q1*g		0	0  	0	|				|q0|
				|	q1*g			q0*g  			q3*g  			q2*g		0	0  	0	|				|q1|
	Y(k+1) =  2*|	q0*g 	 		-q1*g			-q2*g 			q3*g		0	0  	0	|			* |q2|
				|bx*q0-bz*q2	bx*q1+bz*q3  	-bx*q2-bz*q0  -bx*q3+bz*q1		0	0  	0	|				|q3|
				|-bx*q3+bz*q1	bx*q2+bz*q0  	bx*q1+bz*q3 	-bx*q0+bz*q2	0	0  	0	|				|w1|
				|bx*q2+bz*q0	bx*q3-bz*q1  	bx*q0-bz*q2  	bx*q1+bz*q3		0	0  	0	|(6*7)	|w2|
																																															|w3|(7*1)
	=============================================================================
	=============================================================================
	=============================================================================
	*/
	float norm;//模值
	float bx, bz;//地理真实磁场（用于根据姿态变化计算磁场预测值）
	float vx, vy, vz, wx, wy, wz;//加速度观测的预测值 和 磁场观测的预测值
	float g=9.79973;//当地重力加速度
	
	/*观测方程的系数矩阵的暂存值,需要用到的*/ 
	/*分别是加速度对四元数的偏导和磁场强度对四元数的偏导*/
	float Ha1,Ha2,Ha3,Ha4,Hb1,Hb2,Hb3,Hb4;
	float e1,e2,e3,e4,e5,e6;//误差值
	float halfT;//四元数微分中需要 T/2
	
	float q0q0 = q0*q0;	//四元素运算会用到的值 共 1+2+3+4=10个值
	float q0q1 = q0*q1;	//主要用于旋转矩阵的表示  以及  坐标系的转换
	float q0q2 = q0*q2;
	float q0q3 = q0*q3;
	float q1q1 = q1*q1;
	float q1q2 = q1*q2;
	float q1q3 = q1*q3;
	float q2q2 = q2*q2;
	float q2q3 = q2*q3;
	float q3q3 = q3*q3;   

	//坐标系为NED(北东地)-->xyz(右手坐标系)
	bx = 0.5500;// bx指向北
	bz = 0.8351; //bz指向地
	
	now = micros();  //读取时间 32位定时器
	if(now<lastUpdate){//定时器溢出过了
		halfT=((float)(now+(0xffffffff-lastUpdate))/2000000.0f);//发生时间溢出后
	}
	else	{
	halfT=((float)(now-lastUpdate)/2000000.0f);//得到时间并将单位us转换为单位为s
	}
	lastUpdate = now;	//更新时间
	
	norm = invSqrt(ax*ax + ay*ay + az*az);//加速度平方根分之一       
	ax = ax * norm*g;//加速度归一化
	ay = ay * norm*g;//加速度归一化
	az = az * norm*g;//加速度归一化

	norm = invSqrt(mx*mx + my*my + mz*mz);//磁力计平方根分之一            
	mx = mx * norm;//磁场强度归一化
	my = my * norm;//磁场强度归一化
	mz = mz * norm;//磁场强度归一化

	gx=gx-w1;//角速度 w1 w2 w3 是角速度测量误差
	gy=gy-w2;//角速度 w1 w2 w3 是角速度测量误差
	gz=gz-w3;//角速度 w1 w2 w3 是角速度测量误差

	

	/*****************************************************************************************************************************************/
	/*****************************************************************************************************************************************/
	/*****************************************************************************************************************************************/
	/*****************************************************************************************************************************************/
	/*****************************************************************************************************************************************/	
	/*****************************************************************************************************************************************
	黄小平 《卡尔曼滤波原理与应用》4.2.1节 P80页【EKF滤波流程】
	第2步：状态预测
	X(k|k-1)=FX(k-1)
	状态更新 一阶龙格库塔法更新四元数  四元数表示的是加速度的方向向量
					|0  -Wx -Wy  -Wz|		|q0|
	dq/dt =	1/2	*	|Wx  0   Wz  -Wy|	*	|q1|
					|Wy -Wz  0    Wx|		|q2|
					|Wz  Wy -Wx   0 |		|q3|
	*/
	q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
	q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
	q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
	q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
	/* 四元数归一*/
	norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
	q0 = q0 * norm;
	q1 = q1 * norm;
	q2 = q2 * norm;
	q3 = q3 * norm;



	/*****************************************************************************************************************************************
	黄小平 《卡尔曼滤波原理与应用》4.2.1节 P80页【EKF滤波流程】
	第3步：观测预测
	Y(k|k-1)=HX(k-1)
	原理参见第5步的旋转矩阵
	*/
	vx = 2*(q1q3 - q0q2)*g;//ax 的预测值
	vy = 2*(q0q1 + q2q3)*g;//ay 的预测值
	vz = (q0q0 - q1q1 - q2q2 + q3q3)*g;//az 的预测值
	wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);//mx 的预测值
	wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);//my 的预测值
	wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2); //mz 的预测值 

	
	
	/*****************************************************************************************************************************************
	黄小平 《卡尔曼滤波原理与应用》4.2.1节 P80页【EKF滤波流程】
	第4步：状态转移矩阵赋值
	F = dF(X)/dX
	*/
	F[0]=1;			F[1]=-gx*halfT;	F[2]=-gz*halfT;	F[3]=-gz*halfT;	F[4]=0; 	F[5]=0; 	F[6]=0;
	F[7]=gx*halfT;	F[8]=1;			F[9]=gz*halfT;	F[10]=-gy*halfT;F[11]=0; 	F[12]=0; 	F[13]=0;
	F[14]=gy*halfT;	F[15]=-gz*halfT;F[16]=1;		F[17]=gx*halfT;	F[18]=0; 	F[19]=0;	F[20]=0;
	F[21]=gz*halfT;	F[22]=gy*halfT;	F[23]=-gx*halfT;F[24]=1;		F[25]=0; 	F[26]=0; 	F[27]=0;
	F[28]=0;		F[29]=0;		F[30]=0;		F[31]=0;		F[32]=1;	F[33]=0;	F[34]=0;
	F[35]=0;		F[36]=0;		F[37]=0;		F[38]=0;		F[39]=0;	F[40]=1;	F[41]=0;
	F[42]=0;		F[43]=0;		F[44]=0;		F[45]=0;		F[46]=0;	F[47]=0;	F[48]=1;
	
	
	
	/*****************************************************************************************************************************************
	黄小平 《卡尔曼滤波原理与应用》4.2.1节 P80页【EKF滤波流程】
	第5步：观测转移矩阵赋值
	H = dH(X)/dX
	=============================================================================
	=============================重力加速度======================================
	=============================================================================
	根据四元数表示的姿态角计算重力加速度在固联坐标系下的预测值
	q0q0 + q1q1 + q2q2 + q3q3 = 1
	Reb表示RotationEarthFromBody 从固联坐标系转为地球坐标系(右手坐标系)
	Rbe表示RotationBodyFromEarth 从地球坐标系转为固联坐标系(右手坐标系)
				|q0q0+q1q1-q2q2-q3q3  	2(q1q2+q0q3)  				2(q1q3-q0q2)| 
	Rbe = 		|2(q1q2-q0q3)   		q0q0-q1q1+q2q2-q3q3  		2(q2q3+q0q1)| 【这是右乘矩阵： b' = R(rigth)*a'】
				|2(q1q3+q0q2)  			2(q2q3-q0q1)  		 q0q0-q1q1-q2q2+q3q3|	惯性导航P295页所列的为Reb的右乘矩阵（固联坐标系到地球坐标系）		
	
	[Gx Gy Gz]'=Rbe*[0 0 g]'
	
	因此：（其中vx vy vz为重力加速度在固联坐标系的投影）
	vx = 2*(q1q3 - q0q2)*g;
	vy = 2*(q0q1 + q2q3)*g;
	vz = (q0q0 - q1q1 - q2q2 + q3q3)*g;
	=============================================================================
	===============================磁场强度======================================
	=============================================================================
	同理：根据四元数表示的姿态角计算磁场强度在固联坐标系下的预测值
	Reb表示RotationEarthFromBody 从固联坐标系转为地球坐标系(右手坐标系)
	Rbe表示RotationBodyFromEarth 从地球坐标系转为固联坐标系(右手坐标系)
				|q0q0+q1q1-q2q2-q3q3  	2(q1q2+q0q3)  					2(q1q3-q0q2)|
	Rbe = 		|2(q1q2-q0q3)   		q0q0-q1q1+q2q2-q3q3  			2(q2q3+q0q1)|【这是右乘矩阵： b' = R(rigth)*a'】
				|2(q1q3+q0q2)  			2(q2q3-q0q1)  			 q0q0-q1q1-q2q2+q3q3| 惯性导航P295页所列的为Reb的右乘矩阵（固联坐标系到地球坐标系）			
	[Mx My Mz]'=Rbe*[bx 0 bz]'
	
	因此：(其中 wx wy wz 为磁场强度在固联坐标系的投影)
	wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
	wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
	wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
	=============================================================================
	=============================================================================
	=============================================================================
	则有：
			|@ax/@q0		@ax/@q1			@ax/@q2  		@ax/@q3			@ax/@w1			@ax/@w2  		@ax/@w3	|
			|@ay/@q0		@ay/@q1  		@ay/@q2  		@ay/@q3			@ay/@w1			@ay/@w2  		@ay/@w3	|
	 H =  	|@az/@q0 		@az/@q1			@az/@q2 		@az/@q3			@az/@w1			@az/@w2  		@az/@w3	|
			|@mx/@q0		@mx/@q1  		@mx/@q2  		@mx/@q3			@mx/@w1			@mx/@w2  		@mx/@w3	|
			|@my/@q0		@my/@q1  		@my/@q2  		@my/@q3			@my/@w1			@my/@w2  		@my/@w3	|
			|@mz/@q0		@mz/@q1  		@mz/@q2  		@mz/@q3			@mz/@w1			@mz/@w2  		@mz/@w3	|

			|	-q2*g			q3*g			-q0*g  			-q1*g		0	0  	0	|
			|	q1*g			q0*g  			q3*g  			q2*g		0	0  	0	|
		= 2*|	q0*g 	 		-q1*g			-q2*g 			q3*g		0	0  	0	|
			|bx*q0-bz*q2	bx*q1+bz*q3  	-bx*q2-bz*q0  -bx*q3+bz*q1		0	0  	0	|
			|-bx*q3+bz*q1	bx*q2+bz*q0  	bx*q1+bz*q3 	-bx*q0+bz*q2	0	0  	0	|
			|bx*q2+bz*q0	bx*q3-bz*q1  	bx*q0-bz*q2  	bx*q1+bz*q3		0	0  	0	|				
	=============================================================================
	=============================================================================
	=============================================================================
	*/
	Ha1=2*-q2*g; /*对观测方程系数矩阵相关偏导数值预先计算*/
	Ha2=2*q3*g;/*对观测方程系数矩阵相关偏导数值预先计算*/
	Ha3=2*-q0*g;/*对观测方程系数矩阵相关偏导数值预先计算*/
	Ha4=2*q1*g;	/*对观测方程系数矩阵相关偏导数值预先计算*/
	Hb1=2*(bx*q0-bz*q2);/*对观测方程系数矩阵相关偏导数值预先计算*/
	Hb2=2*(bx*q1+bz*q3);/*对观测方程系数矩阵相关偏导数值预先计算*/
	Hb3=2*(-bx*q2-bz*q0);/*对观测方程系数矩阵相关偏导数值预先计算*/
	Hb4=2*(-bx*q3+bz*q1);/*对观测方程系数矩阵相关偏导数值预先计算*/
	H[0]=Ha1;	H[1]= Ha2;	H[2]= Ha3;	H[3]= Ha4;
	H[7]= Ha4;	H[8]=-Ha3;	H[9]= Ha2;	H[10]=-Ha1;
	H[14]=-Ha3;	H[15]=-Ha4;	H[16]= Ha1;	H[17]= Ha2;
	H[21]= Hb1;	H[22]= Hb2;	H[23]= Hb3;	H[24]= Hb4;
	H[28]= Hb4;	H[29]=-Hb3;	H[30]= Hb2;	H[31]=-Hb1;
	H[35]=-Hb3;	H[36]=-Hb4;	H[37]= Hb1;	H[38]= Hb2;



	/****************************************************************************************************************************************
	黄小平 《卡尔曼滤波原理与应用》4.2.1节 P80页【EKF滤波流程】
	第6步：求协方差矩阵
	P(k|k-1) = F(k)P(k-1|k-1)F' + Q
	*/
	MatrixMultiply(F,7,7,P,7,7,A );	//A=F*P
	MatrixTranspose(F,7,7,Ft);	  //F转置F'
	MatrixMultiply(A,7,7,Ft,7,7,B); // B=F*P*F'
	MatrixAdd( B,Q,P1,7,7 );
	
	
	
	/****************************************************************************************************************************************
	*黄小平 《卡尔曼滤波原理与应用》4.2.1节 P80页【EKF滤波流程】
	*第7步：求卡尔曼滤波增益
	*K(k) = P(k|k-1)H(k)'[H(k)P(k|k-1)H' + R]
	*/
	//[H(k)P(k|k-1)H' + R]
	MatrixTranspose(H,6,7,Ht);	  //Ht = H'
	MatrixMultiply(P1,7,7,Ht,7,6,E ); //E=P*H'
	MatrixMultiply(H,6,7,E,7,6,F1 ); //	 F1=H*P*H'
	MatrixAdd(F1,R,X,6,6 );           //X=F1+R
	//K(k) = P(k|k-1)H(k)'[H(k)P(k|k-1)H' + R] 
	//卡尔曼滤波器UD分解滤波算法的改进 http://www.cnki.com.cn/Article/CJFDTotal-YCYK200501002.htm
	UD(X,6,U1,D1);	   //求逆前做UD分解 防止发散
	MatrixTranspose(U1,6,6,U1t);//U1
	MatrixMultiply(U1,6,6,D1,6,6,X1); //X1=U1*D1
	MatrixMultiply(X1,6,6,U1t,6,6,X2); //X2=U1*D1*U1t  （还原矩阵）
	MatrixInverse(X2,6,0);	//对X2求逆  
	MatrixMultiply(E,7,6,X2,6,6,K ); //增益K   7*6
	
	
	
	/****************************************************************************************************************************************
	黄小平 《卡尔曼滤波原理与应用》4.2.1节 P80页【EKF滤波流程】
	第8步：更新状态 X （求更新量）
	X(k) = X(k|k-1)+K[(Y(k)-HX(k-1)]
	*/
	/*计算观测值和预测值之间的偏差 为卡尔曼滤波提供依据*/
	e1=ax-vx;e2=ay-vy;e3=az-vz;//加速度偏差
	e4=mx-wx;e5=my-wy;e6=mz-wz;//磁场偏差
	/*给误差矩阵 T 赋值*/	
	T[0]=e1;T[1]=e2;T[2]=e3;T[3]=e4;T[4]=e5;T[5]=e6;
	MatrixMultiply(K,7,6,T,6,1,Y );   //Y=K*(Z-Y)	7*1 其中 T = Z(观测) - Y(预测)
	//根据变化量更新状态向量(四元数和固联坐标系下的磁场)
	//X = [q0 q1 q2 q3 w1 w2 w3]'	
	q0= q0+Y[0];//更新四元数
	q1= q1+Y[1];//更新四元数
	q2= q2+Y[2];//更新四元数
	q3= q3+Y[3];//更新四元数
	w1= w1+Y[4];//更新角速度偏移
	w2= w2+Y[5];//更新角速度偏移
	w3= w3+Y[6];//更新角速度偏移
	
	
	
	/*****************************************************************************************************************************************
	黄小平 《卡尔曼滤波原理与应用》4.2.1节 P80页【EKF滤波流程】
	第9步：更新协方差
	K(k) = [I-K(k)H(k)]P(k|k-1)
	*/
	MatrixMultiply(K,7,6,H,6,7,Z); //Z= K*H		7*7
	MatrixSub(I,Z,O,7,7 );//O=I-K*H
	MatrixMultiply(O,7,7,P1,7,7,P);



	/*****************************************************************************************************************************************/
	/*****************************************************************************************************************************************/
	/*****************************************************************************************************************************************/
	/*****************************************************************************************************************************************/
	/*****************************************************************************************************************************************/

	/*四元数归一化 保证下一次使用的四元数是单位四元数*/
	norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
	q0 = q0 * norm;
	q1 = q1 * norm;
	q2 = q2 * norm;
	q3 = q3 * norm;
}