Vector4d rigidBodyDynamics::quaternionDivision(Vector4d& q1, Vector4d& q2) const {
Vector4d q2inv;
q2inv << -q2(0) , -q2(1) , -q2(2) , q2(3);
Vector4d result = quaternionMultiplication(q1,q2inv);
return result;
}
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_);
}
void ComplementaryFilter::getMeasurement(
double ax, double ay, double az,
double mx, double my, double mz,
double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas)
{
// q_acc is the quaternion obtained from the acceleration vector representing
// the orientation of the Global frame wrt the Local frame with arbitrary yaw
// (intermediary frame). q3_acc is defined as 0.
double q0_acc, q1_acc, q2_acc, q3_acc;
// Normalize acceleration vector.
normalizeVector(ax, ay, az);
if (az >=0)
{
q0_acc = sqrt((az + 1) * 0.5);
q1_acc = -ay/(2.0 * q0_acc);
q2_acc = ax/(2.0 * q0_acc);
q3_acc = 0;
}
else
{
double X = sqrt((1 - az) * 0.5);
q0_acc = -ay/(2.0 * X);
q1_acc = X;
q2_acc = 0;
q3_acc = ax/(2.0 * X);
}
// [lx, ly, lz] is the magnetic field reading, rotated into the intermediary
// frame by the inverse of q_acc.
// l = R(q_acc)^-1 m
double lx = (q0_acc*q0_acc + q1_acc*q1_acc - q2_acc*q2_acc)*mx +
2.0 * (q1_acc*q2_acc)*my - 2.0 * (q0_acc*q2_acc)*mz;
double ly = 2.0 * (q1_acc*q2_acc)*mx + (q0_acc*q0_acc - q1_acc*q1_acc +
q2_acc*q2_acc)*my + 2.0 * (q0_acc*q1_acc)*mz;
// q_mag is the quaternion that rotates the Global frame (North West Up) into
// the intermediary frame. q1_mag and q2_mag are defined as 0.
double gamma = lx*lx + ly*ly;
double beta = sqrt(gamma + lx*sqrt(gamma));
double q0_mag = beta / (sqrt(2.0 * gamma));
double q3_mag = ly / (sqrt(2.0) * beta);
// The quaternion multiplication between q_acc and q_mag represents the
// quaternion, orientation of the Global frame wrt the local frame.
// q = q_acc times q_mag
quaternionMultiplication(q0_acc, q1_acc, q2_acc, q3_acc,
q0_mag, 0, 0, q3_mag,
q0_meas, q1_meas, q2_meas, q3_meas );
//q0_meas = q0_acc*q0_mag;
//q1_meas = q1_acc*q0_mag + q2_acc*q3_mag;
//q2_meas = q2_acc*q0_mag - q1_acc*q3_mag;
//q3_meas = q0_acc*q3_mag;
}
Vector4d rigidBodyDynamics::addQuaternionError(Vector3d& mrp, Vector4d& qref) const{
Vector4d qnew, dq;
dq = mrp2quaternion(mrp);
Vector4d qnew1 = quaternionMultiplication(dq, qref);
if (qnew1.dot(qref) >= 0) {
return qnew1;
} else {
Vector4d qnew2 = -1 * qnew1;
return qnew2;
}
}
