四元数初始化
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);
}
----------------------------------------------------
|
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
