// 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就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的,而陀螺积分误差也是在机体坐标系,而且叉积的大小与陀螺积分误差成正比,正好拿来纠正陀螺。(你可以自己拿东西想象一下)由于陀螺是对机体直接积分,所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。