使用扩展卡尔曼滤波（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