姿态解算解析
用四元素法进行姿态解算，其步骤如下：
1. 四元素初始化（第一次解算时计算）
①静止状态下由加速度，磁力计值求取初始${\rm{roll}}\gamma $、${\rm{pitch}}\theta $、${\rm{hdg}}\psi $：
$\gamma = {\rm{ arctan}}\frac{{{\rm{ - ay}}}}{{{\rm{ - az}}}}$
$\theta = {\rm{arctan}}\frac{{{\rm{ax}}}}{{{\rm{ - ay}}}}$
$\psi = {\rm{arctan}}\frac{{{\rm{ - magy}}}}{{{\rm{magx}}}}$
②初始化四元素
$\left[ \begin{array}{l}
{{\rm{q}}_{\rm{0}}}\\
{q_1}\\
{q_2}\\
{q_3}
\end{array} \right] = \left[ \begin{array}{l}
\cos \frac{\gamma }{2}\cos \frac{\theta }{2}\cos \frac{\psi }{2} + \sin \frac{\gamma }{2}\sin \frac{\theta }{2}\sin \frac{\psi }{2}\\
\sin \frac{\gamma }{2}\cos \frac{\theta }{2}\cos \frac{\psi }{2} - \cos \frac{\gamma }{2}\sin \frac{\theta }{2}\sin \frac{\psi }{2}\\
\cos \frac{\gamma }{2}\sin \frac{\theta }{2}\cos \frac{\psi }{2} + \sin \frac{\gamma }{2}\cos \frac{\theta }{2}\sin \frac{\psi }{2}\\
\cos \frac{\gamma }{2}\cos \frac{\theta }{2}\sin \frac{\psi }{2} - \sin \frac{\gamma }{2}\sin \frac{\theta }{2}\cos \frac{\psi }{2}
\end{array} \right]$
例程：
void MargAHRSinit(float ax, float ay, float az, float mx, float my, float mz)
{
float initialRoll, initialPitch;
float cosRoll, sinRoll, cosPitch, sinPitch;
float magX, magY;
float initialHdg, cosHeading, sinHeading;
//使用加速度数据计算欧拉角 ，滚转角和俯仰角
initialRoll = atan2(-ay, -az);
initialPitch = atan2(ax, -az);
magX = 1.0f;
magY = 0.0f;
initialHdg = atan2f(-magY, magX);//解算航向角
cosRoll = cosf(initialRoll * 0.5f);
sinRoll = sinf(initialRoll * 0.5f);
cosPitch = cosf(initialPitch * 0.5f);
sinPitch = sinf(initialPitch * 0.5f);
cosHeading = cosf(initialHdg * 0.5f);
sinHeading = sinf(initialHdg * 0.5f);
q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
}
2. 加速度归一化
$\left\{ \begin{array}{l}
norm = \sqrt {a{x^2} + a{y^2} + a{z^2}} \\
ax = \frac{{ax}}{{norm}}\\
ay = \frac{{ay}}{{norm}}\\
az = \frac{{az}}{{norm}}
\end{array} \right.$
3. 提取四元素等效余弦矩阵中的重力分量
四元素等效矩阵
$M = \left[ {\begin{array}{*{20}{c}}
{q_0^2 + q_1^2 - q_2^2 - q_3^2}&{2({q_1}{q_2} - {q_0}{q_3})}&{2({q_1}{q_3} + {q_0}{q_2})}\\
{2({q_1}{q_2} + {q_0}{q_3})}&{q_0^2 - q_1^2 + q_2^2 - q_3^2}&{2({q_2}{q_3} - {q_0}{q_1})}\\
{2({q_1}{q_3} - {q_0}{q_2})}&{2({q_2}{q_3} + {q_0}{q_1})}&{q_0^2 - q_1^2 - q_2^2 + q_3^2}
\end{array}} \right]$
重力矩阵$\left[ {\begin{array}{*{20}{c}}0&0&1\end{array}} \right]$左乘上面矩阵$M$即可提取出重力分量
$\left\{ \begin{array}{l}
vx = 2({q_1}{q_3} - {q_0}{q_2})\\
vy = 2({q_0}{q_1} + {q_2}{q_3})\\
vz = 1 - 2(q_1^2 + q_2^2) = q_0^2 - q_1^2 - q_2^2 + q_3^2
\end{array} \right.$
4. 向量叉积求取姿态误差
$\mathop a\limits^ \to \times \mathop b\limits^ \to = ||a||{\rm{ }}||b||{\rm{ sin}}\theta $
向量的叉积可以用来判断两个向量是否平行，当两个向量都为单位向量的时候，它们之间的叉积就代表了它们之间的平行度，若平行则叉积为0，若垂直则叉积为1，两向量的方向差越小，叉积也越小，因此可以用叉积来表示两归一化向量的方向误差
根据向量的叉乘规则：
若
${\rm{a}} = \left[ \begin{array}{l}{a_x}\\{a_y}\\{a_z}\end{array} \right]$，
$b = \left[ \begin{array}{l}
{b_x}\\
{b_y}\\
{b_z}
\end{array} \right]$，
$c = \left[ \begin{array}{l}
{c_x}\\
{c_y}\\
{c_z}
\end{array} \right]$；
$a = b \times c$
则
$\begin{array}{l}
{a_x} = {b_y}{c_z} - {b_z}{c_y}\\
{a_y} = {b_z}{c_x} - {b_x}{c_z}\\
{a_z} = {b_x}{c_y} - {b_y}{c_x}
\end{array}$
可得
$\begin{array}{l}
ex = vy * az - vz*ay\\
ey = vz * ax - vx*az\\
ez = vx * ay - vy*ax
\end{array}$
5. 对误差进行比例积分运算，更新陀螺仪角速度值
$\begin{array}{l}
exInt = exInt + ex * ki\\
eyInt = eyInt + ey * ki\\
ezInt = ezInt + ez * ki
\end{array}$
$\begin{array}{l}
gx = gx + ex * kp + exInt\\
gy = gy + ey * kp + eyInt\\
gz = gz + ez * kp + ezInt
\end{array}$
6．一阶龙格库塔法求解四元素
${\left( \begin{array}{l}
{q_0}\\
{q_1}\\
{q_2}\\
{q_3}
\end{array} \right)_{t + \Delta t}} = {\left( \begin{array}{l}
{q_0}\\
{q_1}\\
{q_2}\\
{q_3}
\end{array} \right)_t} + \frac{{\Delta t}}{2}\left( \begin{array}{l}
- {\omega _x}.{q_1} - {\omega _y}.{q_2} - {\omega _z}.{q_3}\\
+ {\omega _x}.{q_0} - {\omega _y}.{q_3} + {\omega _z}.{q_2}\\
+ {\omega _x}.{q_3} + {\omega _y}.{q_0} - {\omega _z}.{q_1}\\
- {\omega _x}.{q_2} + {\omega _y}.{q_1} + {\omega _z}.{q_0}
\end{array} \right)$
${\omega _x}$、${\omega _y}$、${\omega _z}$为经过数据融合后的三轴角速度，本例中即为上一步计算出的$gx$、$gy$、$gz$
因为引入了误差，使得变换出来的四元素模不再等于1，即失去了规范性，因此在下一步计算前要进行规范化
7．四元素反正切求取欧拉角
反正切解算出${\rm{roll}}\gamma $、${\rm{pitch}}\theta $、${\rm{hdg}}\psi $：
$\left\{ \begin{array}{l}
\gamma = {\rm{arctan}}\frac{{2\left( {{q_0}{q_1} + {q_2}{q_3}} \right)}}{{{q_0}{q_0} - {q_1}{q_1} - {q_2}{q_2} + {q_3}{q_3}}}\\
\theta = \arcsin \left[ {2({q_0}{q_2} - {q_1}{q_3})} \right]\\
\psi = \arctan \frac{{2\left( {{q_0}{q_3} + {q_1}{q_2}} \right)}}{{{q_0}{q_0} + {q_1}{q_1} - {q_2}{q_2} - {q_3}{q_3}}}
\end{array} \right.$
例程如下：
磁力计部分请参考磁力计四元素融合
void MargAHRSupdate(float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz,
uint8_t magDataUpdate, float dt)
{
float norm, normR;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float q0i, q1i, q2i, q3i;
//-------------------------------------------
if ((MargAHRSinitialized == false)) // HJI && (magDataUpdate == true))
{
MargAHRSinit(ax, ay, az, mx, my, mz);
MargAHRSinitialized = true;
}
//-------------------------------------------
if (MargAHRSinitialized == true)
{
halfT = dt * 0.5f;
norm = sqrt(SQR(ax) + SQR(ay) + SQR(az));
if (norm != 0.0f)
{
calculateAccConfidence(norm);
kpAcc = eepromConfig.KpAcc * accConfidence;
kiAcc = eepromConfig.KiAcc * accConfidence;
normR = 1.0f / norm;
ax *= normR;
ay *= normR;
az *= normR;
// estimated direction of gravity (v)
vx = 2.0f * (q1q3 - q0q2);
vy = 2.0f * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
// error is sum of cross product between reference direction
// of fields and direction measured by sensors
exAcc = vy * az - vz * ay;
eyAcc = vz * ax - vx * az;
ezAcc = vx * ay - vy * ax;
gx += exAcc * kpAcc;
gy += eyAcc * kpAcc;
gz += ezAcc * kpAcc;
if (kiAcc > 0.0f)
{
exAccInt += exAcc * kiAcc;
eyAccInt += eyAcc * kiAcc;
ezAccInt += ezAcc * kiAcc;
gx += exAccInt;
gy += eyAccInt;
gz += ezAccInt;
}
}
//-------------------------------------------
norm = sqrt(SQR(mx) + SQR(my) + SQR(mz));
if ((magDataUpdate == true) && (norm != 0.0f))
{
normR = 1.0f / norm;
mx *= normR;
my *= normR;
mz *= normR;
// compute reference direction of flux
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1))
hz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
bx = sqrt((hx * hx) + (hy * hy));
bz = hz;
// estimated direction of flux (w)
wx = 2.0f * (bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2));
wy = 2.0f * (bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3));
wz = 2.0f * (bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2));
exMag = my * wz - mz * wy;
eyMag = mz * wx - mx * wz;
ezMag = mx * wy - my * wx;
// use un-extrapolated old values between magnetometer updates
// dubious as dT does not apply to the magnetometer calculation so
// time scaling is embedded in KpMag and KiMag
gx += exMag * eepromConfig.KpMag;
gy += eyMag * eepromConfig.KpMag;
gz += ezMag * eepromConfig.KpMag;
if (eepromConfig.KiMag > 0.0f)
{
exMagInt += exMag * eepromConfig.KiMag;
eyMagInt += eyMag * eepromConfig.KiMag;
ezMagInt += ezMag * eepromConfig.KiMag;
gx += exMagInt;
gy += eyMagInt;
gz += ezMagInt;
}
}
//-------------------------------------------
// integrate quaternion rate
q0i = (-q1 * gx - q2 * gy - q3 * gz) * halfT;
q1i = (q0 * gx + q2 * gz - q3 * gy) * halfT;
q2i = (q0 * gy - q1 * gz + q3 * gx) * halfT;
q3i = (q0 * gz + q1 * gy - q2 * gx) * halfT;
q0 += q0i;
q1 += q1i;
q2 += q2i;
q3 += q3i;
// normalise quaternion
normR = 1.0f / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= normR;
q1 *= normR;
q2 *= normR;
q3 *= normR;
// auxiliary variables to reduce number of repeated operations
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;
sensors.margAttitude500Hz[ROLL ] = atan2f(2.0f * (q0q1 + q2q3), q0q0 - q1q1 - q2q2 + q3q3);
sensors.margAttitude500Hz[PITCH] = -asinf(2.0f * (q1q3 - q0q2));
sensors.margAttitude500Hz[YAW ] = atan2f(2.0f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3);
}
}
