Мое требование - выполнить гравитационную компенсацию для данных акселерометра IMU. Я следую этой теме, чтобы сделать то же самое. Я использую Qt, и недавно я наткнулся на класс QQuaternion . Однако я не уверен, что полностью понял класс QQuaternion, поэтому я с нетерпением жду, когда кто-нибудь прокомментирует мои действия.
Используемый мной IMU дает кватернион (q0,q1,q2,q3, где q0 - скалярный компонент). Мне нужно сделать следующие шаги, чтобы компенсировать гравитацию:
Шаг 1: Мне нужно оценить вектор ускорения из локальной системы отсчета тела (значения, непосредственно измеренные IMU) к локальной системе координат Земли
QQuaternion quat(q0,q1,q2,q3); // quaternion from IMU
QVector3D acc(a0,a1,a2); // acceleration vector
QVector3D rotatedAcc = quat.rotatedVector(acc);// ----> is this is the correct way of doing the step 1 mentioned above?
Шаг 2: Поскольку мы знаем, что гравитация - это вектор u = (0,0, g) в локальной системе координат Земли (где g ≈ 9,81 м / с ^ 2, при условии, что измерения IMU также выражены в м / с ^ 2), мы можем затем вычесть этот вектор из повернутого сверху ускорения
QVector3D g(0,0,1); // Gravity vector
QVector3D gCompensatedAcc = rotated-g; // G compensated acc vector
Шаг 3: Однако gCompensatedAcc отсутствует в системе отсчета локального тела ИДУ (поскольку на шаге 1 я повернул ее в систему отсчета локальной Земли). Таким образом, чтобы преобразовать его обратно, умножив его на обратный кватернион .
QQuaternion quatinv= quat.inverted(); // inverse quaternion
QVector3D gCompensatedAccLocal = quatinv.rotatedVector(gCompensatedAcc); //
, когда я делаю вышеуказанные шаги, z acceleration выглядит скомпенсированным правильно (независимо от того, как я ориентируюсь, ускорение ноль, если устройство все еще). Но x и y не кажутся компенсированными. Может кто-то заметить проблему здесь?
Большое спасибо заранее