教授提到,當我們使用IMU獲得3D方向時,可以使用互補濾波器將錨點的結果和陀螺儀的結果結合起來
參考程式碼如下:% 四元數微分方程的4階龍格庫塔法% q0:4*1% gyro:陀螺儀資料% T:更新週期function[ q ]=Quaternion_RungeKutta4(q0,gyro,T)q0=Norm_Quaternion(q
如果帶入四元數形式的旋轉矩陣,則有我們用來表示重力方向向量:相應程式如下:// 估計重力方向向量halfvx=q1*q3-q0*q2
Gyro誤差模型:其中G為gyro真值,Mg為交軸誤差(交軸誤差指IMU三軸不正交產生的誤差),Sg為scale誤差,Ug為IMU gyro測量值,Bg為gyro bias, 與acc相比,gyro的誤差模型中多了一項Hg*A,A是加速度真
% 驗證一下,M_gyro_inv_ * M_gyro_inv應該是單位矩陣fprintf(‘M_gyro_inv_ * M_gyro=\r\n’)disp(M_gyro_inv_*M_gyro)fprintf(‘M_gyro_inv_ =
——————————————————————————手機要想得到compass的值需要由三個sensor來確定,accel, gyro, magnetic