|
|
四元數(shù)初始化
q0-3為四元數(shù)四個(gè)值,用最上面公式根據(jù)加計(jì)計(jì)算出來(lái)的歐拉角來(lái)初始化
--------------------------------------------------
四元數(shù)微分方程
四元數(shù)更新算法，一階龍庫(kù)法，同樣4個(gè)量（入、P1-3）也為四元數(shù)的四個(gè)值,即上面的q0-3。
Wx,y,z是三個(gè)陀螺的這個(gè)周期的角速度，比如歐拉角微分方程中的0.01弧度/秒，T為更新周期,比如上面的0.02秒。
再來(lái)一張，另外一本書上的，仔細(xì)看和上面是一樣的delta角度，就是上面的角速度*周期，單位為弧度
--------------------------------------------------
四元數(shù)微分方程更新后的規(guī)范化
每個(gè)周期更新完四元數(shù)，需要對(duì)四元數(shù)做規(guī)范化處理。因?yàn)樗脑獢?shù)本來(lái)就定義為四維單位向量。
求q0-3的平方和，再開根號(hào)算出的向量長(zhǎng)度length。然后每個(gè)q0-3除這個(gè)length。
四元數(shù)轉(zhuǎn)歐拉角公式
把四元數(shù)轉(zhuǎn)成了方向余弦矩陣中的幾個(gè)元素，再用這幾個(gè)元素轉(zhuǎn)成了歐拉角
先從四元數(shù)q0-3轉(zhuǎn)成方向余弦矩陣：
再?gòu)姆较蛴嘞揖仃囖D(zhuǎn)成歐拉角
代碼：
//更新方向余弦矩陣
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);
}
----------------------------------------------------
|
|
