c2-utopia/lib/RTIMULib/RTFusionRTQF.cpp

235 lines
7.3 KiB
C++

////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib-Arduino
//
// Copyright (c) 2014-2015, richards-tech
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
// Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#ifndef RTARDULINK_MODE
#include "RTFusionRTQF.h"
#ifdef USE_SLERP
// The slerp power valule controls the influence of the measured state to correct the predicted state
// 0 = measured state ignored (just gyros), 1 = measured state overrides predicted state.
// In between 0 and 1 mixes the two conditions
#define RTQF_SLERP_POWER (RTFLOAT)0.02;
#else
// The QVALUE affects the gyro response.
#define RTQF_QVALUE (RTFLOAT)0.001
// The RVALUE controls the influence of the accels and compass.
// The bigger the value, the more sluggish the response.
#define RTQF_RVALUE (RTFLOAT)0.0005
#endif
RTFusionRTQF::RTFusionRTQF()
{
#ifdef USE_SLERP
m_slerpPower = RTQF_SLERP_POWER;
#else
m_Q = RTQF_QVALUE;
m_R = RTQF_RVALUE;
#endif
m_enableGyro = true;
m_enableAccel = true;
m_enableCompass = true;
reset();
}
RTFusionRTQF::~RTFusionRTQF()
{
}
void RTFusionRTQF::reset()
{
m_firstTime = true;
m_fusionPose = RTVector3();
m_fusionQPose.fromEuler(m_fusionPose);
m_measuredPose = RTVector3();
m_measuredQPose.fromEuler(m_measuredPose);
}
void RTFusionRTQF::newIMUData(const RTVector3& gyro, const RTVector3& accel, const RTVector3& compass, unsigned long timestamp)
{
RTVector3 fusionGyro;
if (m_firstTime) {
m_lastFusionTime = timestamp;
calculatePose(accel, compass);
// initialize the poses
m_fusionQPose.fromEuler(m_measuredPose);
m_fusionPose = m_measuredPose;
m_firstTime = false;
} else {
m_timeDelta = (RTFLOAT)(timestamp - m_lastFusionTime) / (RTFLOAT)1000;
m_lastFusionTime = timestamp;
if (m_timeDelta <= 0)
return;
calculatePose(accel, compass);
// predict();
RTFLOAT x2, y2, z2;
RTFLOAT qs, qx, qy,qz;
qs = m_fusionQPose.scalar();
qx = m_fusionQPose.x();
qy = m_fusionQPose.y();
qz = m_fusionQPose.z();
if (m_enableGyro)
fusionGyro = gyro;
else
fusionGyro = RTVector3();
x2 = fusionGyro.x() / (RTFLOAT)2.0;
y2 = fusionGyro.y() / (RTFLOAT)2.0;
z2 = fusionGyro.z() / (RTFLOAT)2.0;
// Predict new state
m_fusionQPose.setScalar(qs + (-x2 * qx - y2 * qy - z2 * qz) * m_timeDelta);
m_fusionQPose.setX(qx + (x2 * qs + z2 * qy - y2 * qz) * m_timeDelta);
m_fusionQPose.setY(qy + (y2 * qs - z2 * qx + x2 * qz) * m_timeDelta);
m_fusionQPose.setZ(qz + (z2 * qs + y2 * qx - x2 * qy) * m_timeDelta);
// update();
#ifdef USE_SLERP
if (m_enableCompass || m_enableAccel) {
// calculate rotation delta
m_rotationDelta = m_fusionQPose.conjugate() * m_measuredQPose;
m_rotationDelta.normalize();
// take it to the power (0 to 1) to give the desired amount of correction
RTFLOAT theta = acos(m_rotationDelta.scalar());
RTFLOAT sinPowerTheta = sin(theta * m_slerpPower);
RTFLOAT cosPowerTheta = cos(theta * m_slerpPower);
m_rotationUnitVector.setX(m_rotationDelta.x());
m_rotationUnitVector.setY(m_rotationDelta.y());
m_rotationUnitVector.setZ(m_rotationDelta.z());
m_rotationUnitVector.normalize();
m_rotationPower.setScalar(cosPowerTheta);
m_rotationPower.setX(sinPowerTheta * m_rotationUnitVector.x());
m_rotationPower.setY(sinPowerTheta * m_rotationUnitVector.y());
m_rotationPower.setZ(sinPowerTheta * m_rotationUnitVector.z());
m_rotationPower.normalize();
// multiple this by predicted value to get result
m_fusionQPose *= m_rotationPower;
}
#else
if (m_enableCompass || m_enableAccel) {
m_stateQError = m_measuredQPose - m_fusionQPose;
} else {
m_stateQError = RTQuaternion();
}
// make new state estimate
RTFLOAT qt = m_Q * m_timeDelta;
m_fusionQPose += m_stateQError * (qt / (qt + m_R));
#endif
m_fusionQPose.normalize();
m_fusionQPose.toEuler(m_fusionPose);
}
}
void RTFusionRTQF::calculatePose(const RTVector3& accel, const RTVector3& mag)
{
RTQuaternion m;
RTQuaternion q;
bool compassValid = (mag.x() != 0) || (mag.y() != 0) || (mag.z() != 0);
if (m_enableAccel) {
accel.accelToEuler(m_measuredPose);
} else {
m_measuredPose = m_fusionPose;
}
if (m_enableCompass && compassValid) {
RTFLOAT cosX2 = cos(m_measuredPose.x() / 2.0f);
RTFLOAT sinX2 = sin(m_measuredPose.x() / 2.0f);
RTFLOAT cosY2 = cos(m_measuredPose.y() / 2.0f);
RTFLOAT sinY2 = sin(m_measuredPose.y() / 2.0f);
q.setScalar(cosX2 * cosY2);
q.setX(sinX2 * cosY2);
q.setY(cosX2 * sinY2);
q.setZ( - sinX2 * sinY2);
// normalize();
m.setScalar(0);
m.setX(mag.x());
m.setY(mag.y());
m.setZ(mag.z());
m = q * m * q.conjugate();
m_measuredPose.setZ(-atan2(m.y(), m.x()));
} else {
m_measuredPose.setZ(m_fusionPose.z());
}
m_measuredQPose.fromEuler(m_measuredPose);
// check for quaternion aliasing. If the quaternion has the wrong sign
// the kalman filter will be very unhappy.
int maxIndex = -1;
RTFLOAT maxVal = -1000;
for (int i = 0; i < 4; i++) {
if (fabs(m_measuredQPose.data(i)) > maxVal) {
maxVal = fabs(m_measuredQPose.data(i));
maxIndex = i;
}
}
// if the biggest component has a different sign in the measured and kalman poses,
// change the sign of the measured pose to match.
if (((m_measuredQPose.data(maxIndex) < 0) && (m_fusionQPose.data(maxIndex) > 0)) ||
((m_measuredQPose.data(maxIndex) > 0) && (m_fusionQPose.data(maxIndex) < 0))) {
m_measuredQPose.setScalar(-m_measuredQPose.scalar());
m_measuredQPose.setX(-m_measuredQPose.x());
m_measuredQPose.setY(-m_measuredQPose.y());
m_measuredQPose.setZ(-m_measuredQPose.z());
m_measuredQPose.toEuler(m_measuredPose);
}
}
#endif // #ifndef RTARDULINK_MODE