基于IMU的航姿算法

---

前言

航姿参考系统（ AHRS）有麦格威克算法（Madgwick）、Mahony的算法、传统卡尔曼滤波器等。他们的基本原理是：利用重力加速度的方向校正陀螺仪漂移的数据。

一、麦格威克算法

这是麦格威克这个人博士论文中写的算法，没有具体实现过，先把相关的资料收集起来。
1、[Arduino] Madgwick算法的数学推演
2、Madgwick算法详细解读
代码如下（示例）：链接: https://www.codenong.com/cs109602269/

//=====================================================================================================
// MadgwickAHRS.c
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date         Author          Notes
// 29/09/2011   SOH Madgwick    Initial release
// 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
// 19/02/2012   SOH Madgwick    Magnetometer measurement is normalised
//
//=====================================================================================================

//---------------------------------------------------------------------------------------------------
// Header files

#include "MadgwickAHRS.h"
#include 

//---------------------------------------------------------------------------------------------------
// Definitions

#define sampleFreq  512.0f      // sample frequency in Hz
#define betaDef     0.1f        // 2 * proportional gain

//---------------------------------------------------------------------------------------------------
// Variable definitions

volatile float beta = betaDef;                              // 2 * proportional gain (Kp)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  // quaternion of sensor frame relative to auxiliary frame

//---------------------------------------------------------------------------------------------------
// Function declarations

float invSqrt(float x);

//====================================================================================================
// Functions

//---------------------------------------------------------------------------------------------------
// AHRS algorithm update

void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {<!-- -->
    float recipNorm;
    float s0, s1, s2, s3;
    float qDot1, qDot2, qDot3, qDot4;
    float hx, hy;
    float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {<!-- -->
        MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
        return;
    }

    // Rate of change of quaternion from gyroscope
    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {<!-- -->

        // Normalise accelerometer measurement
        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;  

        // Normalise magnetometer measurement
        recipNorm = invSqrt(mx * mx + my * my + mz * mz);
        mx *= recipNorm;
        my *= recipNorm;
        mz *= recipNorm;

        // Auxiliary variables to avoid repeated arithmetic
        _2q0mx = 2.0f * q0 * mx;
        _2q0my = 2.0f * q0 * my;
        _2q0mz = 2.0f * q0 * mz;
        _2q1mx = 2.0f * q1 * mx;
        _2q0 = 2.0f * q0;
        _2q1 = 2.0f * q1;
        _2q2 = 2.0f * q2;
        _2q3 = 2.0f * q3;
        _2q0q2 = 2.0f * q0 * q2;
        _2q2q3 = 2.0f * q2 * q3;
        q0q0 = q0 * q0;
        q0q1 = q0 * q1;
        q0q2 = q0 * q2;
        q0q3 = q0 * q3;
        q1q1 = q1 * q1;
        q1q2 = q1 * q2;
        q1q3 = q1 * q3;
        q2q2 = q2 * q2;
        q2q3 = q2 * q3;
        q3q3 = q3 * q3;

        // Reference direction of Earth's magnetic field
        hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
        hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
        _2bx = sqrt(hx * hx + hy * hy);
        _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
        _4bx = 2.0f * _2bx;
        _4bz = 2.0f * _2bz;

        // Gradient decent algorithm corrective step
        s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
        s0 *= recipNorm;
        s1 *= recipNorm;
        s2 *= recipNorm;
        s3 *= recipNorm;

        // Apply feedback step
        qDot1 -= beta * s0;
        qDot2 -= beta * s1;
        qDot3 -= beta * s2;
        qDot4 -= beta * s3;
    }

    // Integrate rate of change of quaternion to yield quaternion
    q0 += qDot1 * (1.0f / sampleFreq);
    q1 += qDot2 * (1.0f / sampleFreq);
    q2 += qDot3 * (1.0f / sampleFreq);
    q3 += qDot4 * (1.0f / sampleFreq);

    // Normalise quaternion
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

//---------------------------------------------------------------------------------------------------
// IMU algorithm update

void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {<!-- -->
    float recipNorm;
    float s0, s1, s2, s3;
    float qDot1, qDot2, qDot3, qDot4;
    float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

    // Rate of change of quaternion from gyroscope
    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {<!-- -->

        // Normalise accelerometer measurement
        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;  

        // Auxiliary variables to avoid repeated arithmetic
        _2q0 = 2.0f * q0;
        _2q1 = 2.0f * q1;
        _2q2 = 2.0f * q2;
        _2q3 = 2.0f * q3;
        _4q0 = 4.0f * q0;
        _4q1 = 4.0f * q1;
        _4q2 = 4.0f * q2;
        _8q1 = 8.0f * q1;
        _8q2 = 8.0f * q2;
        q0q0 = q0 * q0;
        q1q1 = q1 * q1;
        q2q2 = q2 * q2;
        q3q3 = q3 * q3;

        // Gradient decent algorithm corrective step
        s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
        s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
        s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
        s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
        s0 *= recipNorm;
        s1 *= recipNorm;
        s2 *= recipNorm;
        s3 *= recipNorm;

        // Apply feedback step
        qDot1 -= beta * s0;
        qDot2 -= beta * s1;
        qDot3 -= beta * s2;
        qDot4 -= beta * s3;
    }

    // Integrate rate of change of quaternion to yield quaternion
    q0 += qDot1 * (1.0f / sampleFreq);
    q1 += qDot2 * (1.0f / sampleFreq);
    q2 += qDot3 * (1.0f / sampleFreq);
    q3 += qDot4 * (1.0f / sampleFreq);

    // Normalise quaternion
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}


int instability_fix = 1;

//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

float invSqrt(float x) {<!-- -->
    if (instability_fix == 0)
    {<!-- -->
        /* original code */
        float halfx = 0.5f * x;
        float y = x;
        long i = *(long*)&y;
        i = 0x5f3759df - (i>>1);
        y = *(float*)&i;
        y = y * (1.5f - (halfx * y * y));
        return y;
    }
    else if (instability_fix == 1)
    {<!-- -->
        /* close-to-optimal  method with low cost from http://pizer.wordpress.com/2008/10/12/fast-inverse-square-root */
        unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1);
        float tmp = *(float*)&i;
        return tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
    }
    else
    {<!-- -->
        /* optimal but expensive method: */
        return 1.0f / sqrtf(x);
    }
}

//====================================================================================================
// END OF CODE
//====================================================================================================

二、Mahony的算法

用比例和积分调节器修正陀螺仪参数，调节的依据来自通过q_{gyro}和q_{mag,ac}做叉乘得到的误差。

代码如下（示例）：链接: https://blog.csdn.net/weixin_40599145/article/details/99937264?spm=1001.2014.3001.5502

//陀螺仪、加速度计、磁力计数据融合出姿态四元数
    void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
            float norm;
            float hx, hy, hz, bx, bz;
            float vx, vy, vz, wx, wy, wz; //v*当前姿态计算得来的重力在机载坐标下三轴上的分量
            float ex, ey, ez;
 
            // auxiliary variables to reduce number of repeated operations
            //先计算好相关乘积项为了后面做矩阵运算做准备
            float q0q0 = q0*q0;
            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;
           
            // normalise the measurements
            //加速度计和地磁计向量标准化
            norm = sqrt(ax*ax + ay*ay + az*az); 
            ax = ax / norm;
            ay = ay / norm;
            az = az / norm;
            norm = sqrt(mx*mx + my*my + mz*mz); 
            mx = mx / norm;
            my = my / norm;
            mz = mz / norm;
           
            // compute reference direction of magnetic field
            //这里计算得到的是地磁计在理论地磁坐标系下的机体上三个轴的分量
            hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
            hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
            hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);   
            
            //bx计算的是当前航向角和磁北的夹角，也就是北天东坐标下的航向角
            //当罗盘水平旋转的时候，航向角在0-360之间变化
            bx = sqrt((hx*hx) + (hy*hy));
            bz = hz; 
           
           // estimated direction of gravity and magnetic field (v and w) 
           //用四元数表示的方向余弦矩阵解算出理论载体坐标b系下的三轴加速度大小。
           //加速度计重力向量转换到b系
            vx = 2*(q1q3 - q0q2);
            vy = 2*(q0q1 + q2q3);
            vz = q0q0 - q1q1 - q2q2 + q3q3;
           //地磁计在n系下磁向量转换到b系下，反向使用DCM得到
            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);  
           
            // error is sum of cross product between reference direction of fields and direction measured by sensors 
            //体现在加速计补偿和磁力计补偿，因为仅仅依靠加速计补偿没法修正Z轴的变差，所以还需要通过磁力计来修正Z轴。
            //下面是叉积运算的过程
            ex = (ay*vz - az*vy) + (my*wz - mz*wy);
            ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
            ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
           
            // integral error scaled integral gain 
            exInt = exInt + ex*Ki* (1.0f / sampleFreq);
            eyInt = eyInt + ey*Ki* (1.0f / sampleFreq);
            ezInt = ezInt + ez*Ki* (1.0f / sampleFreq);
            // adjusted gyroscope measurements
            //将误差PI后补偿到陀螺仪，即补偿零点漂移。通过调节Kp、Ki两个参数，可以控制加速度计修                正陀螺仪积分姿态的速度。（公式16和公式29）
            gx = gx + Kp*ex + exInt;
            gy = gy + Kp*ey + eyInt;
            gz = gz + Kp*ez + ezInt;
           
            // integrate quaternion rate and normalize
            //一阶龙格库塔法更新四元数
            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;  
           
            // normalise quaternion
            norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
            q0 = q0 / norm;
            q1 = q1 / norm;
            q2 = q2 / norm;
            q3 = q3 / norm;
}

三、基于EKF的ahrs算法

代码链接: https://blog.csdn.net/qq_42348833/article/details/106013882

/**************************实现函数********************************************
*函数原型:	 	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;
}

---

本人实现ekf的算法的结果如下：

ahrs算法求的pitch和roll跟组合导航的结果对比，基本上在2°以内，算法中的R阵很关键，在高动态的时候要加大噪声。