四元數初始化
q0-3為四元數四個值,用最上面公式根據加計計算出來的歐拉角來初始化
--------------------------------------------------
四元數微分方程
四元數更新算法，一階龍庫法，同樣4個量（入、P1-3）也為四元數的四個值,即上面的q0-3。
Wx,y,z是三個陀螺的這個周期的角速度，比如歐拉角微分方程中的0.01弧度/秒，T為更新周期,比如上面的0.02秒。
再來一張，另外一本書上的，仔細看和上面是一樣的delta角度，就是上面的角速度*周期，單位為弧度
--------------------------------------------------
四元數微分方程更新后的規(guī)范化
每個周期更新完四元數，需要對四元數做規(guī)范化處理。因為四元數本來就定義為四維單位向量。
求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空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
