扩展卡尔曼滤波器磁力计偏航漂移

Posted

技术标签:

【中文标题】扩展卡尔曼滤波器磁力计偏航漂移【英文标题】:Extended Kalman Filter Magnetometer Yaw drifting 【发布时间】:2011-08-13 07:07:28 【问题描述】:

我正在使用扩展卡尔曼滤波器来融合加速度计、陀螺仪和磁力计数据。我使用加速度计来校正俯仰和滚动数据,并使用磁力计来校正偏航。俯仰和滚动运行良好,但即使我使用了磁力计,我也有非常严重的偏航漂移。我用来在 EKF 中融合磁力计数据的代码是:

(m 是磁力计测量值,a 是加速度计测量值)

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) 传感器噪声和设备行为的细微差别,例如磁力计通常会打破 KF 假设

(2)什么是状态转换模型,即俯仰/偏航的变化与磁场变化的关系是什么

【讨论】:

以上是关于扩展卡尔曼滤波器磁力计偏航漂移的主要内容,如果未能解决你的问题,请参考以下文章

数据融合基于扩展卡尔曼滤波器实现三维数据融合matlab源码

GPS +加速度计的C语言中的任何卡尔曼滤波器实现?

卡尔曼滤波器使用旋转矩阵平滑加速度计信号

卡尔曼滤波 KF | 扩展卡尔曼滤波 EKF (思路流程和计算公式)

卡尔曼滤波 KF | 扩展卡尔曼滤波 EKF (思路流程和计算公式)

卡尔曼滤波 KF | 扩展卡尔曼滤波 EKF (思路流程和计算公式)