Расширенный магнитный фильтр Калмана - PullRequest
4 голосов
/ 13 августа 2011

Я использую расширенный фильтр Калмана для объединения данных акселерометра, гироскопа и магнитометра. Я использую акселерометр, чтобы исправить данные тангажа и крена, и магнитометр, чтобы исправить рыскание. Гудок и крен работают хорошо, но у меня очень сильный дрейф, хотя я применил магнитометр. Код, который я использую для объединения данных магнитометра в 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;

Я не совсем уверен в том, как объединить данные магнитометра. Если вы знаете, что не так с кодом или как я могу это исправить, пожалуйста, дайте мне знать!

Большое спасибо!

1 Ответ

1 голос
/ 21 декабря 2012

Это перегруженный вопрос ... для реализации чего-то подобного вам необходимо сначала понять хотя бы:

(1) нюансы шума датчика и поведения на устройстве, например, магнитометр, как правило, нарушает предположения KF

(2) что такое модель перехода состояний, то есть какова связь между изменениями высоты тона / рыскания и изменениями в магнитном поле

Добро пожаловать на сайт PullRequest, где вы можете задавать вопросы и получать ответы от других членов сообщества.
...