磁力计会受重力影响？
新人请教磁力计各轴读书会受重力影响么？我的lsm303d读磁力计数据时垂直地面的轴读书最大，大于南北轴好多，无论xyz那个轴面相地面都是会变很大，这里的数是绝对值最大，新人只用过303不知道是否正常，别的磁力计也这样？prow 发表于 2014-11-16 14:01
{:lol:} 什么感谢，原来是这样啊，白天手机看不到图，现在才看到，谢谢 prow 发表于 2014-11-16 14:01
三楼打错子了，不好意思，是十分感谢{:lol:} 手机是需要画8字的，具体的传感器没用过 8字只是修正圆心的吧，对地磁倾角方向的垂直向量是没用的，只能靠倾斜补偿了，看到的几个姿态解算算法貌似都没有倾斜补偿 本帖最后由 eonk2005 于 2014-11-20 11:53 编辑
自己结下贴吧，研究了几种算法，第一种是ST集成传感器方案实现电子罗盘功能里的补偿方法（http://www.eeworld.com.cn/gykz/2011/0408/article_5352.html），直接用的三角函数，速度最慢，用的时候要注意参考文章中图片的各轴方向按自己的方向修改。
第二种是MadgwickAHRSupdate算法中用四元数旋转体坐标到世界坐标得到水平状态下的X,Y值，参与以后的修正补偿
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
各轴方向是前，左，上为正方向（北西上）
第三种是taulabs的，将起点（家点）状态的水平状态下的世界坐标系各轴数值按当前姿态旋转到体坐标系，这样于当前体坐标系下的磁力向量叉乘获得航向角偏差，修正陀螺漂移。方向是北东下为正
float bmag = 1.0f;
float brot;
float Rbe;
const float Be = {1.0f, 0.0f, 0.0f};
// Get rotation to bring earth magnetic field into body frame
Quaternion2R(cf_q, Rbe);
if (homeLocation.Set == HOMELOCATION_SET_TRUE) {
rot_mult(Rbe, homeLocation.Be, brot, false);家点BE中保存的是初始水平状态下的磁力计向量值
bmag = sqrtf(brot * brot + brot * brot + brot * brot);
brot /= bmag;
brot /= bmag;
brot /= bmag;
} else {
rot_mult(Rbe, Be, brot, false);
}
mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
mag.x /= mag_len;
mag.y /= mag_len;
mag.z /= mag_len;
// Only compute if neither vector is null
if (bmag < 1 || mag_len < 1)
mag_err = mag_err = mag_err = 0;
else
CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);求出初始水平磁力向量按当前角度旋转后与磁力计测量值的差值
//** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** 世界坐标系到体坐标系转换，四元数到旋转矩阵转换
void Quaternion2R(float q, float Rbe)
{
float q0s = q * q, q1s = q * q, q2s = q * q, q3s = q * q;
Rbe = q0s + q1s - q2s - q3s;
Rbe = 2 * (q * q + q * q);
Rbe = 2 * (q * q - q * q);
Rbe = 2 * (q * q - q * q);
Rbe = q0s - q1s + q2s - q3s;
Rbe = 2 * (q * q + q * q);
Rbe = 2 * (q * q + q * q);
Rbe = 2 * (q * q - q * q);
Rbe = q0s - q1s - q2s + q3s;
}
// ****** Vector Cross Product ********向量叉乘求向量差
void CrossProduct(const float v1, const float v2, float result)
{
result = v1*v2 - v2*v1;
result = v2*v1 - v1*v2;
result = v1*v2 - v2*v1;
}
说的不对的大家指正{:lol:}，后两种姿态算法这里没有贴完整代码，实际效果差不多，感觉taulabs的更灵活些，因为它的加速度计修正和磁力计修正系数是分开的设置的，毕竟现实中陀螺仪，加计，磁计的ODR速度是差很多的，不过taulabs这种倾角补偿每次都是用起点状态的磁力向量旋转得当前向量的方式和当前向量做差后觉得误差比MadgwickAHRSupdate的要大，也有可能是我代码还没读透吧。
页:
[1]