Quaternion interpolate(float t, Quaternion a, Quaternion b)
{
float w = acos(dotQuaternion(normQuaternion(a), normQuaternion(b)));
float s = sin(w);
return addQuaternion(scaleQuaternion(sin((1.0f - t) * w) / s, a),
scaleQuaternion(sin(t * w) / s, b));
}
void ComplementaryFilter::updateImpl(
double gx, double gy, double gz,
double ax, double ay, double az,
double dt)
{
if (!initialized_)
{
// First time - ignore prediction:
getMeasurement(ax, ay, az,
q0_, q1_, q2_, q3_);
initialized_ = true;
return;
}
if(dt <= 0.0)
{
UERROR("dt=%f <=0.0, orientation will not be updated!", dt);
return;
}
// Bias estimation.
if (do_bias_estimation_)
updateBiases(ax, ay, az, gx, gy, gz);
// Prediction.
double q0_pred, q1_pred, q2_pred, q3_pred;
getPrediction(gx, gy, gz, dt,
q0_pred, q1_pred, q2_pred, q3_pred);
// Correction (from acc):
// q_ = q_pred * [(1-gain) * qI + gain * dq_acc]
// where qI = identity quaternion
double dq0_acc, dq1_acc, dq2_acc, dq3_acc;
getAccCorrection(ax, ay, az,
q0_pred, q1_pred, q2_pred, q3_pred,
dq0_acc, dq1_acc, dq2_acc, dq3_acc);
double gain;
if (do_adaptive_gain_)
{
gain = getAdaptiveGain(gain_acc_, ax, ay, az);
}
else
{
gain = gain_acc_;
}
scaleQuaternion(gain, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred,
dq0_acc, dq1_acc, dq2_acc, dq3_acc,
q0_, q1_, q2_, q3_);
normalizeQuaternion(q0_, q1_, q2_, q3_);
}