4

I am using extended kalman filter to fuse accelerometer, gyro, and magnetometer data. I use accelerometer to correct pitch and roll data, and magnetometer to correct yaw. The pitch and roll are working well, but i have a very severe yaw drifting even though I implemented the magnetometer. The code I'm using to fuse magnetometer data in the EKF is:

(m being the magnetometer measurements and a being the accelerometer measurements)

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 not completely sure about how to fuse the magnetometer data. If you know what is wrong with the code or how I could fix it, please let me know!

Thanks a lot!

thomas
  • 5,637
  • 2
  • 24
  • 35
Mita
  • 49
  • 1
  • 2

1 Answers1

1

This is an overloaded question... to implement something like that you'd need to first understand at least:

(1) nuances of sensor noise and behavior on device, for example the magnetometer will typically break KF assumptions

(2) what is the state transition model, i.e. what is the relationship between changes in pitch/yaw and changes in magnetic field

peter karasev
  • 2,578
  • 1
  • 28
  • 38