51 do_bias_estimation_(true),
52 do_adaptive_gain_(false),
55 q0_(1), q1_(0), q2_(0), q3_(0),
56 wx_prev_(0), wy_prev_(0), wz_prev_(0),
57 wx_bias_(0), wy_bias_(0), wz_bias_(0) { }
83 if (gain >= 0 && gain <= 1.0)
93 if (gain >= 0 && gain <= 1.0)
119 if (bias_alpha >= 0 && bias_alpha <= 1.0)
134 double q0,
double q1,
double q2,
double q3)
157 double wx,
double wy,
double wz,
174 double q0_pred, q1_pred, q2_pred, q3_pred;
176 q0_pred, q1_pred, q2_pred, q3_pred);
181 double dq0_acc, dq1_acc, dq2_acc, dq3_acc;
183 q0_pred, q1_pred, q2_pred, q3_pred,
184 dq0_acc, dq1_acc, dq2_acc, dq3_acc);
201 dq0_acc, dq1_acc, dq2_acc, dq3_acc,
208 double wx,
double wy,
double wz,
209 double mx,
double my,
double mz,
227 double q0_pred, q1_pred, q2_pred, q3_pred;
229 q0_pred, q1_pred, q2_pred, q3_pred);
234 double dq0_acc, dq1_acc, dq2_acc, dq3_acc;
236 q0_pred, q1_pred, q2_pred, q3_pred,
237 dq0_acc, dq1_acc, dq2_acc, dq3_acc);
243 double q0_temp, q1_temp, q2_temp, q3_temp;
245 dq0_acc, dq1_acc, dq2_acc, dq3_acc,
246 q0_temp, q1_temp, q2_temp, q3_temp);
251 double dq0_mag, dq1_mag, dq2_mag, dq3_mag;
253 q0_temp, q1_temp, q2_temp, q3_temp,
254 dq0_mag, dq1_mag, dq2_mag, dq3_mag);
259 dq0_mag, dq1_mag, dq2_mag, dq3_mag,
266 double wx,
double wy,
double wz)
const 268 double acc_magnitude = sqrt(ax*ax + ay*ay + az*az);
286 double wx,
double wy,
double wz)
303 double wx,
double wy,
double wz,
double dt,
304 double& q0_pred,
double& q1_pred,
double& q2_pred,
double& q3_pred)
const 310 q0_pred =
q0_ + 0.5*dt*( wx_unb*
q1_ + wy_unb*
q2_ + wz_unb*
q3_);
311 q1_pred =
q1_ + 0.5*dt*(-wx_unb*
q0_ - wy_unb*
q3_ + wz_unb*
q2_);
312 q2_pred =
q2_ + 0.5*dt*( wx_unb*
q3_ - wy_unb*
q0_ - wz_unb*
q1_);
313 q3_pred =
q3_ + 0.5*dt*(-wx_unb*
q2_ + wy_unb*
q1_ - wz_unb*
q0_);
319 double ax,
double ay,
double az,
320 double mx,
double my,
double mz,
321 double& q0_meas,
double& q1_meas,
double& q2_meas,
double& q3_meas)
326 double q0_acc, q1_acc, q2_acc, q3_acc;
332 q0_acc = sqrt((az + 1) * 0.5);
333 q1_acc = -ay/(2.0 * q0_acc);
334 q2_acc = ax/(2.0 * q0_acc);
339 double X = sqrt((1 - az) * 0.5);
340 q0_acc = -ay/(2.0 * X);
343 q3_acc = ax/(2.0 * X);
349 double lx = (q0_acc*q0_acc + q1_acc*q1_acc - q2_acc*q2_acc)*mx +
350 2.0 * (q1_acc*q2_acc)*my - 2.0 * (q0_acc*q2_acc)*mz;
351 double ly = 2.0 * (q1_acc*q2_acc)*mx + (q0_acc*q0_acc - q1_acc*q1_acc +
352 q2_acc*q2_acc)*my + 2.0 * (q0_acc*q1_acc)*mz;
356 double gamma = lx*lx + ly*ly;
357 double beta = sqrt(gamma + lx*sqrt(gamma));
358 double q0_mag = beta / (sqrt(2.0 * gamma));
359 double q3_mag = ly / (sqrt(2.0) * beta);
365 q0_mag, 0, 0, q3_mag,
366 q0_meas, q1_meas, q2_meas, q3_meas );
375 double ax,
double ay,
double az,
376 double& q0_meas,
double& q1_meas,
double& q2_meas,
double& q3_meas)
387 q0_meas = sqrt((az + 1) * 0.5);
388 q1_meas = -ay/(2.0 * q0_meas);
389 q2_meas = ax/(2.0 * q0_meas);
394 double X = sqrt((1 - az) * 0.5);
395 q0_meas = -ay/(2.0 * X);
398 q3_meas = ax/(2.0 * X);
403 double ax,
double ay,
double az,
404 double p0,
double p1,
double p2,
double p3,
405 double& dq0,
double& dq1,
double& dq2,
double& dq3)
418 dq0 = sqrt((gz + 1) * 0.5);
419 dq1 = -gy/(2.0 * dq0);
420 dq2 = gx/(2.0 * dq0);
425 double mx,
double my,
double mz,
426 double p0,
double p1,
double p2,
double p3,
427 double& dq0,
double& dq1,
double& dq2,
double& dq3)
438 double gamma = lx*lx + ly*ly;
439 double beta = sqrt(gamma + lx*sqrt(gamma));
440 dq0 = beta / (sqrt(2.0 * gamma));
443 dq3 = ly / (sqrt(2.0) * beta);
447 double& q0,
double& q1,
double& q2,
double& q3)
const 455 double a_mag = sqrt(ax*ax + ay*ay + az*az);
460 double m = 1.0/(error1 - error2);
461 double b = 1.0 - m*error1;
464 else if (error < error2)
465 factor = m*error + b;
474 double norm = sqrt(x*x + y*y + z*z);
483 double norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
491 double q0,
double q1,
double q2,
double q3,
492 double& q0_inv,
double& q1_inv,
double& q2_inv,
double& q3_inv)
503 double& dq0,
double& dq1,
double& dq2,
double& dq3)
508 double angle = acos(dq0);
509 double A = sin(angle*(1.0 - gain))/sin(angle);
510 double B = sin(angle * gain)/sin(angle);
519 dq0 = (1.0 - gain) + gain * dq0;
529 double p0,
double p1,
double p2,
double p3,
530 double q0,
double q1,
double q2,
double q3,
531 double& r0,
double& r1,
double& r2,
double& r3)
534 r0 = p0*q0 - p1*q1 - p2*q2 - p3*q3;
535 r1 = p0*q1 + p1*q0 + p2*q3 - p3*q2;
536 r2 = p0*q2 - p1*q3 + p2*q0 + p3*q1;
537 r3 = p0*q3 + p1*q2 - p2*q1 + p3*q0;
541 double x,
double y,
double z,
542 double q0,
double q1,
double q2,
double q3,
543 double& vx,
double& vy,
double& vz)
545 vx = (q0*q0 + q1*q1 - q2*q2 - q3*q3)*x + 2*(q1*q2 - q0*q3)*y + 2*(q1*q3 + q0*q2)*z;
546 vy = 2*(q1*q2 + q0*q3)*x + (q0*q0 - q1*q1 + q2*q2 - q3*q3)*y + 2*(q2*q3 - q0*q1)*z;
547 vz = 2*(q1*q3 - q0*q2)*x + 2*(q2*q3 + q0*q1)*y + (q0*q0 - q1*q1 - q2*q2 + q3*q3)*z;
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)