#include "math.h"

// 最简单的航姿仪Mahony算法，所谓“最简单”就意味着在加速机动环境下效果会不太好，没做任何容错处理。

// 注意: 传感器/机体坐标系(b):x右-y前-z上; 导航坐标系(n):E东-N北-U天

// gx,gy,gz陀螺仪采样(deg/s), ax,ay,az加计采样(可为任意单位或归一化单位)

// mx,my,mz磁采样(可为任意单位或归一化单位)，若三轴磁均输入零则表示不利用磁信息

// 根据网上程序整理优化 by Yan Gongmin, 2017-06-07

void MahonyInit(float tau);

void MahonyUpdate(float gx, float gy, float gz, float ax, float ay,

float az, float mx, float my, float mz, float ts);

float MahonyGetEuler(float *pitch, float *roll, float *yaw);

void MahonyGetDrift(float *driftx, float *drifty, float

*driftz);

#define DEG (3.1415926f/180.0f)

#define DPS (DEG/1.0f)

#define DPH (DEG/3600.0f)

static float q0,q1,q2,q3, exInt,eyInt,ezInt, tk;

static float C00,C01,C02, C10,C11,C12, C20,C21,C22;

static float Kp, Ki;

static void qua2cnb(void);

void MahonyInit(float tau)

{

float beta = 2.146f/tau;

Kp = 2.0f*beta, Ki = beta*beta;

q0 = 1.0f, q1 = q2 = q3 = 0.0f;

qua2cnb();

exInt = eyInt = ezInt = 0.0f;

tk = 0.0f;

}

void MahonyUpdate(float gx, float gy, float gz, float ax,

float ay, float az, float mx, float my, float mz, float ts)

{

float norm;

float by, bz, wx, wy, wz;

float ex, ey, ez, halfT, KiT;

float _q0, _q1, _q2, _q3;

tk += ts;

halfT = ts/2.0f; KiT = Ki*ts;

norm = (float)sqrt(ax*ax + ay*ay +

az*az); // 加计归一化

norm = norm>0.1f ? 1.0f/norm : 0.0f;

ax *= norm, ay *=

norm, az *= norm;

norm = (float)sqrt(mx*mx + my*my +

mz*mz); // 磁归一化

norm = norm>0.1f ? 1.0f/norm : 0.0f;

mx *= norm, my *=

norm, mz *= norm;

bz = C20*mx + C21*my + C22*mz; // 磁场的天向投影分量

by =

(float)sqrt(1.0f-bz*bz); //

磁场的水平(磁北)投影分量

// bx =

0.0f; // 磁场的磁东向投影分量

wx = C10*by +

C20*bz; //

磁场的理想b系投影分量

wy = C11*by + C21*bz;

wz = C12*by + C22*bz;

ex = (ay*C22-az*C21) +

(my*wz-mz*wy); // 加计测水平误差、磁测方位误差

ey = (az*C20-ax*C22) + (mz*wx-mx*wz);

ez = (ax*C21-ay*C20) + (mx*wy-my*wx);

exInt += ex * KiT;

eyInt += ey * KiT;

ezInt += ez * KiT;

gx *= DPS, gy *= DPS, gz *= DPS;

gx += Kp*ex +

exInt; // 陀螺修正

gy += Kp*ey + eyInt;

gz += Kp*ez + ezInt;

_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 = 1.0f/(float)sqrt(_q0*_q0 + _q1*_q1 +

_q2*_q2 + _q3*_q3); // 四元数归一化

q0 = _q0 * norm, q1 = _q1 * norm, q2 = _q2 *

norm, q3 = _q3 * norm;

qua2cnb();

}

float MahonyGetEuler(float *pitch, float *roll, float

*yaw)

{

*pitch = (float)asin(C21) /

DEG; // deg

*roll = (float)atan2(-C20, C22)

/ DEG;

*yaw =

(float)atan2(-C01, C11) /

DEG; // +-180deg, 北偏西为正

return tk;

}

void MahonyGetDrift(float *driftx, float *drifty, float

*driftz)

{

*driftx = exInt /

DPH; // deg/hur

*drifty = eyInt / DPH;

*driftz = ezInt / DPH;

}

static void qua2cnb(void)

{

float q00 = q0*q0, q01 = q0*q1, q02 = q0*q2, q03

= q0*q3;

float q11 = q1*q1, q12 = q1*q2, q13 =

q1*q3;

float q22 = q2*q2, q23 = q2*q3;

float q33 = q3*q3;

C00 = q00+q11-q22-q33, C01 =

2*(q12-q03), C02 = 2*(q13+q02); // 四元数转姿态阵

C10 =

2*(q12+q03), C11 = q00-q11+q22-q33, C12 = 2*(q23-q01);

C20 =

2*(q13-q02), C21 =

2*(q23+q01), C22 = q00-q11-q22+q33;

}

算法原理可参考：