拡張カルマンフィルター磁力計ヨードリフト

Question

私は、加速度計、ジャイロ、および磁力計のデータを融合するために、拡張カルマンフィルタを使用しています。私は加速度計を使ってピッチとロールのデータを補正し、磁力計はヨーを補正します。ピッチとロールはうまくいきますが、私は磁力計を実装しても非常に厳しいヨーヨーがあります。私はEKFに磁力計データを融合するために使用しているコードは：

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; 

Iは、ヒューズする方法について完全に分からない（Mは、磁力計の測定とである加速度計の測定値です）磁力計データ。コードの何が間違っているのか、それを修正する方法が分かっている場合は、教えてください！

ありがとうございます！

Answer 1

これは、オーバーロードされた質問...あなたが最初に理解する必要があるだろうというようなものを実装することです少なくとも、

は が

（1）デバイス上のセンサノイズおよび行動のニュアンスは、例えば磁力計は、一般的に解除されますKF推定値

（2）状態遷移モデルとは何か、つまりピッチ/ヨーの変化と磁場の変化の関係は何ですか