void scaleQuaternion(
double gain,
double& dq0, double& dq1, double& dq2, double& dq3)
{
if (dq0 < 0.0)//0.9
{
// Slerp (Spherical linear interpolation):
double angle = acos(dq0);
double A = sin(angle*(1.0 - gain))/sin(angle);
double B = sin(angle * gain)/sin(angle);
dq0 = A + B * dq0;
dq1 = B * dq1;
dq2 = B * dq2;
dq3 = B * dq3;
}
else
{
// Lerp (Linear interpolation):
dq0 = (1.0 - gain) + gain * dq0;
dq1 = gain * dq1;
dq2 = gain * dq2;
dq3 = gain * dq3;
}
normalizeQuaternion(dq0, dq1, dq2, dq3);
}
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_);
}
__inline void cleanQuaternion(double *quaternion) {
if ((quaternion[0] * quaternion[0] + quaternion[3] * quaternion[3]) < NUM_TOL_SQ) {
quaternion[0] = 1e-5;
quaternion[3] = -1e-5;
}
if ((quaternion[1] * quaternion[1] + quaternion[2] * quaternion[2]) < NUM_TOL_SQ) {
quaternion[1] = 1e-5;
quaternion[2] = -1e-5;
}
/* normalize */
normalizeQuaternion(quaternion);
}
void TaitBryanToQuaternion(float v[3], float q[4]){
float cosX2 = cosf(v[VEC3_X] / 2.0f);
float sinX2 = sinf(v[VEC3_X] / 2.0f);
float cosY2 = cosf(v[VEC3_Y] / 2.0f);
float sinY2 = sinf(v[VEC3_Y] / 2.0f);
float cosZ2 = cosf(v[VEC3_Z] / 2.0f);
float sinZ2 = sinf(v[VEC3_Z] / 2.0f);
q[QUAT_W] = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2;
q[QUAT_X] = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2;
q[QUAT_Y] = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2;
q[QUAT_Z] = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2;
normalizeQuaternion(q);
}
void ComplementaryFilter::getPrediction(
double wx, double wy, double wz, double dt,
double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const
{
double wx_unb = wx - wx_bias_;
double wy_unb = wy - wy_bias_;
double wz_unb = wz - wz_bias_;
q0_pred = q0_ + 0.5*dt*( wx_unb*q1_ + wy_unb*q2_ + wz_unb*q3_);
q1_pred = q1_ + 0.5*dt*(-wx_unb*q0_ - wy_unb*q3_ + wz_unb*q2_);
q2_pred = q2_ + 0.5*dt*( wx_unb*q3_ - wy_unb*q0_ - wz_unb*q1_);
q3_pred = q3_ + 0.5*dt*(-wx_unb*q2_ + wy_unb*q1_ - wz_unb*q0_);
normalizeQuaternion(q0_pred, q1_pred, q2_pred, q3_pred);
}
void gradientDescent(double *wPrimeWPrimeTranspose, int iFrame, int jFrame, double * wPairwiseProd, double *errArray, double *quaternionArray, char *isDone, int nFrame, int nPoint, double *RTransposeR, double *dRTransposeR, double *quaternionTmp, double *gradient) {
int i, ii, j, k;
double errMin, errTmp, errMinOld;
double *quaternionMin;
int nItr;
double lambda;
double normGradSq;
int nItrLine;
/* update the wPrimeWPrimeTranspose matrix */
updateW(wPairwiseProd, iFrame, jFrame, nFrame, wPrimeWPrimeTranspose);
/* Figure out the best guess to start from */
quaternionMin = quaternionArray + 4 * (iFrame * nFrame + jFrame);
/* Go over all the neighbors around the current location to find the best one */
if (isDone[jFrame * nFrame + iFrame])
errMin = errArray[jFrame * nFrame + iFrame];
else
errMin = -1;
for (i = iFrame - 1; i <= iFrame + 1; ++i)
for (j = jFrame - 1; j <= jFrame + 1; ++j)
if ((i >= 0) && (i < nFrame) && (j >= 0) && (j < nFrame)
&& isDone[j * nFrame + i]) {
/* and check which one already gives the smallest value */
copyQuaternion(quaternionArray + 4 * (i * nFrame + j), quaternionTmp, 1);
errTmp = reconstrPairVal(quaternionTmp, wPrimeWPrimeTranspose, RTransposeR);
/* Keep the best pick */
if ((errTmp < errMin) || (errMin < 0)) {
errMin = errTmp;
copyQuaternion(quaternionTmp, quaternionMin, 0);
}
}
/* Start from that optimal value and perform gradient descent */
lambda=1.0;
/* fprintf(stderr,"Start error %f\n", errMin); */
for (nItr = 0; nItr < 40; ++nItr) {
errMin = reconstrPairValGrad(quaternionMin, wPrimeWPrimeTranspose, gradient, RTransposeR, dRTransposeR);
/* stop if the gradient is too small or the error too */
normGradSq = normSq(4, gradient);
if ((normGradSq < NUM_TOL_SQ) || (errMin < NUM_TOL ))
break;
/* Perform line search */
errMinOld=errMin;
for (nItrLine = 0; nItrLine < 20; ++nItrLine) {
/* get the value for the current quaternion */
for (k = 0; k < 4; ++k)
quaternionTmp[k] = quaternionMin[k] - lambda * gradient[k];
cleanQuaternion(quaternionTmp);
errTmp = reconstrPairVal(quaternionTmp, wPrimeWPrimeTranspose, RTransposeR);
/* check if the error is better than what we had before */
if (errTmp < errMin) {
errMin = errTmp;
copyQuaternion(quaternionTmp, quaternionMin, 0);
break;
} else
lambda /= 2;
}
/* stop if the error does not change much, < 0.1 percent */
if ((errMinOld-errMin)<0.001*errMinOld)
break;
}
/* fprintf(stderr,"%i ", nItr);
*update if nothing before or if the current result is better than before
*/
if ((!isDone[iFrame * nFrame + jFrame]) || ((isDone[iFrame * nFrame + jFrame]) && (errMin <errArray[iFrame * nFrame + jFrame]))) {
errArray[iFrame * nFrame + jFrame] = errMin;
errArray[jFrame * nFrame + iFrame] = errMin;
normalizeQuaternion(quaternionMin);
copyQuaternion(quaternionMin, quaternionArray + 4 * (jFrame * nFrame + iFrame), 0);
isDone[iFrame * nFrame + jFrame] = 1;
isDone[jFrame * nFrame + iFrame] = 1;
}
}
