电子产品世界

------------------------------------------------------
来点干货，注意以下的欧拉角都是这样的顺序：先航向-再俯仰-然后横滚
公式截图来自：袁信、郑锷的《捷联式惯性导航原理》，邓正隆的《惯性技术》。
--------------------------------------------------
根据加计计算初始欧拉角
这个无论欧拉角算法还是四元数算法还是方向余弦矩阵都需要，因为加计和电子罗盘给出欧拉角的描述方式比较方便。
imu.euler.x = atan2(imu.accel.y, imu.accel.z);
imu.euler.y = -asin(imu.accel.x / ACCEL_1G);
ACCEL_1G 为9.81米/秒^2，accel.xyz的都为这个单位，算出来的euler.xyz单位是弧度
航向imu.euler.z可以用电子罗盘计算
--------------------------------------------------
欧拉角微分方程
如果用欧拉角算法，那么这个公式就够了，不需要来回转换。

矩阵上到下三个角度（希腊字母）是roll pitch和yaw，公式最左边的上面带点的三个是本次更新后的角度，不带点的是上个更新周期算出来的角度。
Wx,y,z是roll pitch和yaw方向的三个陀螺在这个周期转动过的角度，单位为弧度，计算为间隔时间T*陀螺角速度,比如0.02秒*0.01弧度/秒=0.0002弧度.


--------------------------------------------------
以下是四元数
--------------------------------------------------
四元数初始化
q0-3为四元数四个值,用最上面公式根据加计计算出来的欧拉角来初始化

--------------------------------------------------
四元数微分方程
四元数更新算法，一阶龙库法，同样4个量（入、P1-3）也为四元数的四个值,即上面的q0-3。
Wx,y,z是三个陀螺的这个周期的角速度，比如欧拉角微分方程中的0.01弧度/秒，T为更新周期,比如上面的0.02秒。

再来一张，另外一本书上的，仔细看和上面是一样的delta角度，就是上面的角速度*周期，单位为弧度

--------------------------------------------------
四元数微分方程更新后的 规范化
每个周期更新完四元数，需要对四元数做规范化处理。因为四元数本来就定义为四维单位向量。
求q0-3的平方和，再开根号算出的向量长度length。然后每个q0-3除这个length。

--------------------------------------------------
四元数转欧拉角 公式
把四元数转成了方向余弦矩阵中的几个元素，再用这几个元素转成了欧拉角
先从四元数q0-3转成方向余弦矩阵：

再从方向余弦矩阵转成欧拉角


代码：

//更新方向余弦矩阵 t11=q.q0*q.q0+q.q1*q.q1-q.q2*q.q2-q.q3*q.q3; t12=2.0*(q.q1*q.q2+q.q0*q.q3); t13=2.0*(q.q1*q.q3-q.q0*q.q2); t21=2.0*(q.q1*q.q2-q.q0*q.q3); t22=q.q0*q.q0-q.q1*q.q1+q.q2*q.q2-q.q3*q.q3; t23=2.0*(q.q2*q.q3+q.q0*q.q1); t31=2.0*(q.q1*q.q3+q.q0*q.q2); t32=2.0*(q.q2*q.q3-q.q0*q.q1); t33=q.q0*q.q0-q.q1*q.q1-q.q2*q.q2+q.q3*q.q3; //求出欧拉角 imu.euler.roll = atan2(t23,t33); imu.euler.pitch = -asin(t13); imu.euler.yaw = atan2(t12,t11); if (imu.euler.yaw < 0){ imu.euler.yaw += ToRad(360);

----------------------------------------------------

以下代码摘自网上，很巧妙，附上注释，有四元数微分，有加计耦合。
没电子罗盘，其实耦合原理也一样。

//===================================================================================================== // IMU.c // S.O.H. Madgwick // 25th September 2010 //===================================================================================================== // Description: // // Quaternion implementation of the 'DCM filter' [Mayhony et al]. // // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'. // // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated // orientation. See my report for an overview of the use of quaternions in this application. // // User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz') // and accelerometer ('ax', 'ay', 'ay') data. Gyroscope units are radians/second, accelerometer // units are irrelevant as the vector is normalised. // //===================================================================================================== //---------------------------------------------------------------------------------------------------- // Header files #include "IMU.h" #include //---------------------------------------------------------------------------------------------------- // Definitions #define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer #define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases #define halfT 0.5f // half the sample period //--------------------------------------------------------------------------------------------------- // Variable definitions float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error //==================================================================================================== // Function //==================================================================================================== void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; // normalise the measurements norm = sqrt(ax*ax + ay*ay + az*az); ax = ax / norm; ay = ay / norm; az = az / norm; 把加计的三维向量转成单位向量。 // estimated direction of gravity vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; 这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。 根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。 所以这里的vx\y\z，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的重力单位向量。 // error is sum of cross product between reference direction of field and direction measured by sensor ex = (ay*vz - az*vy); ey = (az*vx - ax*vz); ez = (ax*vy - ay*vx); axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。 axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。 那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。 向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。 这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。 // integral error scaled integral gain exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki; // adjusted gyroscope measurements gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; 用叉积误差来做PI修正陀螺零偏 // integrate quaternion rate and normalise 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; 四元数规范化 } //==================================================================================================== // END OF CODE //====================================================================================================

陀螺一般有零点漂移、非线性、加速度影响等等多种误差。
其中零点漂移影响最大，但在初始化（保持静止读取一段时间平均值作为零点）后，漂移还可以忍受。不过温度变化大，漂移也比较厉害。
没有磁力计，是无法对航向角做纠正融合的。

以下代码是上面那段代码的磁力计+加计+陀螺版，没仔细研究过，粗看看像是把磁阻mxyz的向量转到地理坐标系，然后用地理坐标系的正北向标准磁场向量取代变成bxyz？又转回机体坐标系变成wxyz，最后和原始磁阻测量值mxyz做向量叉积来修正陀螺。
想学习的就研究研究吧

我自己做的磁阻耦合很简单很粗暴。把磁阻方向角解出来，和四元数解除的欧拉角航向角做个差，再用k滤波器把差值融合到欧拉角航向角上去，然后直接把欧拉角转成四元数。

//===================================================================================================== // AHRS.c // S.O.H. Madgwick // 25th August 2010 //===================================================================================================== // Description: // // Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion // compensation algorithms from my filter [Madgwick] which eliminates the need for a reference // direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw // axis only. // // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'. // // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated // orientation. See my report for an overview of the use of quaternions in this application. // // User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'), // accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data. Gyroscope units are // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised. // //===================================================================================================== //---------------------------------------------------------------------------------------------------- // Header files #include "AHRS.h" #include  //---------------------------------------------------------------------------------------------------- // Definitions #define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer #define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases #define halfT 0.5f // half the sample period //--------------------------------------------------------------------------------------------------- // Variable definitions float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error //==================================================================================================== // Function //==================================================================================================== void AHRSupdate(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; 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 flux 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 = sqrt((hx*hx) + (hy*hy)); bz = hz; // estimated direction of gravity and flux (v and w) vx = 2*(q1q3 - q0q2); vy = 2*(q0q1 + q2q3); vz = q0q0 - q1q1 - q2q2 + q3q3; 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 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; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki; // adjusted gyroscope measurements gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; // integrate quaternion rate and normalise 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; } //==================================================================================================== // END OF CODE //====================================================================================================

补偿，得有观测者才能对测量者做纠正。
加计本来是观测者，对陀螺补偿。
对外力加速度来说，加计属于测量者，没有外力的观测者，无法补偿加计。
如果是震动，可以调小PI参数，靠长时间的数值平均，减小震动引起的加速度误差。
而机体转弯的离心力可以用 GPS地速×陀螺三维角速度，根据离心力公式算出三维离心加速度，去减掉转弯带来的加速度影响。
但是机体的纵向加速度就没法补偿了，不过一般情况下长时间单方向加速的情况较少，误差可以容忍。

如果要高精度的姿态测算，那除非再引入观测者，比如GPS的坐标定位信息。也就是所谓的GPS/INS组合导航，一般是卡尔曼算法。
举个例子，比如惯导中当前估算速度为匀速xx米/秒，移动方向为北向，所以估算1秒后坐标会向北移动XX米。
1秒后GPS给出信息，和惯导估算的相比，坐标向北多移动了YY米。，变成了XX+YY米。
那么应该是有外力加速度，导致机体北向速度变快，本周期平均值为YY-XX米/秒^2（只是举个例子，数值不做准）系统就会这外力加速度补偿到加计的测量值里去。

GPS/INS组合导航，系统就会运算出姿态和坐标定位信息，是融合过的，都非常准。卡尔曼算法很难，我不懂，只知道个大概原理。

一言蔽之，这些算法都是以测量者的数值为基础，用观测者观察到的其他方面的数值，用一个转换算法去推断测量者的数值误差，然后纠正之。而这就是卡尔曼算法的流程。

这里误差没说清楚，不是指向量差。这个叉积误差是指将 带有误差的加计向量 转动到与 重力向量重合，需要 绕什么轴，转多少角度。
逆向推理一下，这个叉积在机体三轴上的投影，就是加计和重力之间的角度误差。
也就是说，如果陀螺按这个叉积误差的轴，转动叉积误差的角度（也就是转动三轴投影的角度）那就能把加计和重力向量的误差消除掉。（具体可看向量叉积的定义）
如果完全按叉积误差转过去，那就是完全信任加计。如果一点也不转，那就是完全信任陀螺。
那么把这个叉积的三轴乘以X%，加到陀螺的积分角度上去，就是这个x%互补系数的互补算法了。

ps：实际上叉积的length是两向量夹角的正弦，而且必须在±90度以内，并不完全与误差角度成线性正比。
如果转成三轴夹角，按欧拉角的转动顺序分解到三轴上去，会很麻烦。
这里的叉积算法把sin误差当成角度误差，并无视欧拉角的转动顺序，在误差较小的时候，并不会有影响。
因为这个修正对误差来说，是收敛的，正常情况下误差只会越来越小。
为了解释方便，所以上面就把叉积等同于角度误差了。

仔细看了一会，上面的磁力计修正流程已经想通了，看看大家想没想通吧。

陀螺的零点漂移可以靠加计和磁力计来补偿，不过加计和磁力计的零点漂移、椭圆修正就没法动态补偿了。因为没有观测者。
只能在初始化的时候修正，但是温度的影响就不太好办。
gps/ins是王道，可惜高阶卡尔曼实在不好弄，谁有心得分享一下吧。

不过一般玩玩，这些算法够了。

谢谢分享，很好的资料

向大神学习了。