4
我使用擴展卡爾曼濾波器來融合加速度計，陀螺儀和磁力計數據。我使用加速計來校正俯仰和滾動數據，並使用磁力計來校正偏航。俯仰和滾動運行良好，但即使實施了磁力計，我也有非常嚴重的偏航漂移。我使用的EKF融合磁強計數據的代碼是：擴展卡爾曼濾波磁強計偏航漂移
（M是磁力計測量和被加速度計測量）
m_max.x = +540; m_max.y = +500; m_max.z = 180;
m_min.x = -520; m_min.y = -570; m_min.z = -770;
m.x = (m.x - m_min.x)/(m_max.x - m_min.x) * 2 - 1.0;
m.y = (m.y - m_min.y)/(m_max.y - m_min.y) * 2 - 1.0;
m.z = (m.z - m_min.z)/(m_max.z - m_min.z) * 2 - 1.0;
vector temp_a = a;
// normalize
vector_normalize(&temp_a);
//vector_normalize(&m);
// compute E and N
vector E;
vector N;
vector_cross(&m,&temp_a,&E);
vector_normalize(&E);
vector_cross(&temp_a,&E,&N);
// q is the state quaternion matrix
Xog = [1-2(q2*q2+q3*q3);
2(q1*q2+q0*q3)];
Xogmag = [N;E];
// yaw error
Ey = Xogmag - Xog;
// yaw observation matrix
Hy = [0, 0, -4*q2, -4*q3, 0, 0, 0;
w*q3, 2*q2, 2*q1, 2*q0, 0, 0, 0];
// yaw estimation error covariance matrix
Py - Hy * P * (Hy') + Ry
// yaw kalman gain
Ky = P * (Hy') * inv(Py);
// update the state
X = X + Ky * Ey;
// update system state covariance matrix
P = P - Ky * Hy * P;
我不能完全確定如何融合磁力計數據。如果您知道代碼有什麼問題或我如何解決問題，請告訴我！
非常感謝！