void RTFusionRTQF::newIMUData(RTIMU_DATA& data, const RTIMUSettings *settings)
{
if (m_debug) {
HAL_INFO("\n------\n");
HAL_INFO2("IMU update delta time: %f, sample %d\n", m_timeDelta, m_sampleNumber++);
}
m_sampleNumber++;
if (m_enableGyro)
m_gyro = data.gyro;
else
m_gyro = RTVector3();
m_accel = data.accel;
m_compass = data.compass;
m_compassValid = data.compassValid;
if (m_firstTime) {
m_lastFusionTime = data.timestamp;
calculatePose(m_accel, m_compass, settings->m_compassAdjDeclination);
m_Fk.fill(0);
// initialize the poses
m_stateQ.fromEuler(m_measuredPose);
m_fusionQPose = m_stateQ;
m_fusionPose = m_measuredPose;
m_firstTime = false;
} else {
m_timeDelta = (RTFLOAT)(data.timestamp - m_lastFusionTime) / (RTFLOAT)1000000;
m_lastFusionTime = data.timestamp;
if (m_timeDelta <= 0)
return;
calculatePose(data.accel, data.compass, settings->m_compassAdjDeclination);
predict();
update();
m_stateQ.toEuler(m_fusionPose);
m_fusionQPose = m_stateQ;
if (m_debug) {
HAL_INFO(RTMath::displayRadians("Measured pose", m_measuredPose));
HAL_INFO(RTMath::displayRadians("RTQF pose", m_fusionPose));
HAL_INFO(RTMath::displayRadians("Measured quat", m_measuredPose));
HAL_INFO(RTMath::display("RTQF quat", m_stateQ));
HAL_INFO(RTMath::display("Error quat", m_stateQError));
}
}
data.fusionPoseValid = true;
data.fusionQPoseValid = true;
data.fusionPose = m_fusionPose;
data.fusionQPose = m_fusionQPose;
}
void RTFusionKalman4::newIMUData(RTIMU_DATA& data, const RTIMUSettings *settings)
{
if (m_enableGyro)
m_gyro = data.gyro;
else
m_gyro = RTVector3();
m_accel = data.accel;
m_compass = data.compass;
m_compassValid = data.compassValid;
if (m_firstTime) {
m_lastFusionTime = data.timestamp;
calculatePose(m_accel, m_compass, settings->m_compassAdjDeclination);
m_Fk.fill(0);
// init covariance matrix to something
m_Pkk.fill(0);
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
m_Pkk.setVal(i,j, 0.5);
// initialize the observation model Hk
// Note: since the model is the state vector, this is an identity matrix so it won't be used
// initialize the poses
m_stateQ.fromEuler(m_measuredPose);
m_fusionQPose = m_stateQ;
m_fusionPose = m_measuredPose;
m_firstTime = false;
} else {
m_timeDelta = (RTFLOAT)(data.timestamp - m_lastFusionTime) / (RTFLOAT)1000000;
m_lastFusionTime = data.timestamp;
if (m_timeDelta <= 0)
return;
if (m_debug) {
HAL_INFO("\n------\n");
HAL_INFO1("IMU update delta time: %f\n", m_timeDelta);
}
calculatePose(data.accel, data.compass, settings->m_compassAdjDeclination);
predict();
update();
m_stateQ.toEuler(m_fusionPose);
m_fusionQPose = m_stateQ;
if (m_debug | settings->m_fusionDebug) {
HAL_INFO(RTMath::displayRadians("Measured pose", m_measuredPose));
HAL_INFO(RTMath::displayRadians("Kalman pose", m_fusionPose));
HAL_INFO(RTMath::displayRadians("Measured quat", m_measuredPose));
HAL_INFO(RTMath::display("Kalman quat", m_stateQ));
HAL_INFO(RTMath::display("Error quat", m_stateQError));
}
}
data.fusionPoseValid = true;
data.fusionQPoseValid = true;
data.fusionPose = m_fusionPose;
data.fusionQPose = m_fusionQPose;
}
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 RTFusionAHRS::newIMUData(RTIMU_DATA& data, const RTIMUSettings *settings)
{
if (m_debug) {
HAL_INFO("\n------\n");
HAL_INFO1("IMU update delta time: %f\n", m_timeDelta);
}
m_gyro = data.gyro;
m_accel = data.accel;
m_compass = data.compass;
m_compassValid = data.compassValid;
if (m_firstTime) {
// adjust for compass declination, compute the correction data only at beginning
m_sin_theta_half = sin(-settings->m_compassAdjDeclination/2.0f);
m_cos_theta_half = cos(-settings->m_compassAdjDeclination/2.0f);
m_lastFusionTime = data.timestamp;
calculatePose(m_accel, m_compass, settings->m_compassAdjDeclination);
// initialize the poses
m_stateQ.fromEuler(m_measuredPose);
m_fusionQPose = m_stateQ;
m_fusionPose = m_measuredPose;
m_firstTime = false;
} else { // not first time
m_timeDelta = (RTFLOAT)(data.timestamp - m_lastFusionTime) / (RTFLOAT)1000000;
m_lastFusionTime = data.timestamp;
if (m_timeDelta <= 0)
return;
calculatePose(data.accel, data.compass, settings->m_compassAdjDeclination);
// =================================================
// AHRS
//
// Previous Q Pose; short name local variables for readability
float q1 = m_stateQ.scalar();
float q2 = m_stateQ.x();
float q3 = m_stateQ.y();
float q4 = m_stateQ.z();
float ax, ay, az, mx, my, mz, gx, gy, gz; // accelerometer, magnetometer, gyroscope
float gerrx, gerry, gerrz; // gyro bias error
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
float _2q1mx,_2q1my, _2q1mz,_2q2mx;
float _4bx, _4bz;
float _2q1, _2q2, _2q3, _2q4;
float q1q1, q1q2, q1q3, q1q4, q2q2, q2q3, q2q4, q3q4, q3q3, q4q4;
float _4q1, _4q2, _4q3, _8q2, _8q3;
float _2q3q4, _2q1q3;
if (m_enableCompass) {
mx = m_compass.x();
my = m_compass.y();
mz = m_compass.z();
} else {
mx = 0.0f;
my = 0.0f;
mz = 0.0f;
}
if (m_enableAccel) {
ax = m_accel.x();
ay = m_accel.y();
az = m_accel.z();
} else {
ax = 0.0f;
ay = 0.0f;
az = 0.0f; }
if (m_enableGyro) {
gx=m_gyro.x();
gy=m_gyro.y();
gz=m_gyro.z();
} else { return; } // We need to have valid gyroscope data
////////////////////////////////////////////////////////////////////////////
// Regular Algorithm
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz); // s
qDot2 = 0.5f * ( q1 * gx + q3 * gz - q4 * gy); // x
qDot3 = 0.5f * ( q1 * gy - q2 * gz + q4 * gx); // y
qDot4 = 0.5f * ( q1 * gz + q2 * gy - q3 * gx); // z
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Use this algorithm if accelerometer is valid
// If accelerometer is not valid, update pose based on previous qDot
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0) return; // handle NaN
ax /= norm;
ay /= norm;
az /= norm;
// Auxiliary variables to avoid repeated arithmetic
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q4 = 2.0f * q4;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
q4q4 = q4 * q4;
if(((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
// If magnetometer is invalid
// Auxiliary variables to avoid repeated arithmetic
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_4q3 = 4.0f * q3;
_8q2 = 8.0f * q2;
_8q3 = 8.0f * q3;
// Gradient decent algorithm corrective step
s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay;
s2 = _4q2 * q4q4 - _2q4 * ax + 4.0f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az;
s4 = 4.0f * q2q2 * q4 - _2q2 * ax + 4.0f * q3q3 * q4 - _2q3 * ay;
} else {
// Valid magnetometer available use this code
// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0) return; // handle NaN
mx /= norm;
my /= norm;
mz /= norm;
// Auxiliary variables to avoid repeated arithmetic
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q1q4 = q1 * q4;
q2q3 = q2 * q3;
q2q4 = q2 * q4;
q3q4 = q3 * q4;
_2q1q3 = 2.0f * q1q3;
_2q3q4 = 2.0f * q3q4;
_2q1mx = 2.0f * q1 * mx;
_2q1my = 2.0f * q1 * my;
_2q1mz = 2.0f * q1 * mz;
_2q2mx = 2.0f * q2 * mx;
// Reference direction of Earth's magnetic field
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
} // end valid magnetometer
norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
if (norm == 0.0) return; // handle NaN
s1 /= norm;
s2 /= norm;
s3 /= norm;
s4 /= norm;
// Compute estimated gyroscope biases
gerrx = _2q1 * s2 - _2q2 * s1 - _2q3 * s4 + _2q4 * s3;
gerry = _2q1 * s3 + _2q2 * s4 - _2q3 * s1 - _2q4 * s2;
gerrz = _2q1 * s4 - _2q2 * s3 + _2q3 * s2 - _2q4 * s1;
// Compute and remove gyroscope biases
m_gbiasx += gerrx * m_timeDelta * m_zeta;
m_gbiasy += gerry * m_timeDelta * m_zeta;
m_gbiasz += gerrz * m_timeDelta * m_zeta;
gx -= m_gbiasx;
gy -= m_gbiasy;
gz -= m_gbiasz;
// Apply feedback step
qDot1 -= m_beta * s1;
qDot2 -= m_beta * s2;
qDot3 -= m_beta * s3;
qDot4 -= m_beta * s4;
} // end if valid accelerometer
// Integrate to yield quaternion
q1 += qDot1 * m_timeDelta;
q2 += qDot2 * m_timeDelta;
q3 += qDot3 * m_timeDelta;
q4 += qDot4 * m_timeDelta;
// normalise quaternion
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
if (norm == 0.0) return; // handle NaN
q1 /= norm;
q2 /= norm;
q3 /= norm;
q4 /= norm;
m_stateQ.setScalar(q1);
m_stateQ.setX(q2);
m_stateQ.setY(q3);
m_stateQ.setZ(q4);
// Rotate Quaternion by Magnetic Declination
// m_stateQ = q_declination * m_statqQ;
/*
SAGE
N.<c,d,q1,q2,q3,q4,cos_theta_half, sin_theta_half> = QQ[]
H.<i,j,k> = QuaternionAlgebra(c,d)
q = q1 + q2 * i + q3 * j + q4 * k
// here rotation is around gravity vector by theta
mag_declination = cos_theta_half + sin_theta_half * (0 * i + 0 * j+ 1*k)
q * mag_declination
s : -q4*sin_theta_half + q1*cos_theta_half
x : q3*sin_theta_half + q2*cos_theta_half
y : -q2*sin_theta_half + q3*cos_theta_half
z : q4*cos_theta_half + q1*sin_theta_half
*/
m_stateQdec.setScalar(q1*m_cos_theta_half - q4*m_sin_theta_half);
m_stateQdec.setX(q3*m_sin_theta_half + q2*m_cos_theta_half);
m_stateQdec.setY(q3*m_cos_theta_half - q2*m_sin_theta_half);
m_stateQdec.setZ(q4*m_cos_theta_half + q1*m_sin_theta_half);
} // end not first time
// =================================================
if (m_enableCompass || m_enableAccel) {
m_stateQError = m_measuredQPose - m_stateQ;
} else {
m_stateQError = RTQuaternion();
}
m_stateQdec.toEuler(m_fusionPose);
m_fusionQPose = m_stateQdec;
if (m_debug | settings->m_fusionDebug) {
HAL_INFO(RTMath::displayRadians("Measured pose", m_measuredPose));
HAL_INFO(RTMath::displayRadians("AHRS pose", m_fusionPose));
HAL_INFO(RTMath::displayRadians("Measured quat", m_measuredPose));
HAL_INFO(RTMath::display("AHRS quat", m_stateQ));
HAL_INFO(RTMath::display("Error quat", m_stateQError));
HAL_INFO3("AHRS Gyro Bias: %+f, %+f, %+f\n", m_gbiasx, m_gbiasy, m_gbiasz);
}
data.fusionPoseValid = true;
data.fusionQPoseValid = true;
data.fusionPose = m_fusionPose;
data.fusionQPose = m_fusionQPose;
} //
