/** * @ingroup experimental * @brief this is an opinionated method (opinionated about what axis is yaw, pitch, roll and what is left/right/up/down * * @param self the quaternion * @param ax the x axis * @param ay the y axis * @param az the z axis * */ HYPAPI quaternion * quaternion_set_from_euler_anglesf3_EXP(quaternion *self, float ax, float ay, float az) { vector3 vx; vector3 vy; vector3 vz; quaternion qx; quaternion qy; quaternion qz; vector3_setf3(&vx, 1, 0, 0); vector3_setf3(&vy, 0, 1, 0); vector3_setf3(&vz, 0, 0, 1); quaternion_set_from_axis_anglev3(&qx, &vx, ax); quaternion_set_from_axis_anglev3(&qy, &vy, ay); quaternion_set_from_axis_anglev3(&qz, &vz, az); /* self = qx * qy * qz */ quaternion_multiply(&qx, &qy); quaternion_multiply(&qx, &qz); quaternion_set(self, &qx); quaternion_normalize(self); return self; }
void quaternion_from_euler(vector4d *q, double rx, double ry, double rz) { vector3d vx = {1, 0, 0}, vy = {0, 1, 0}, vz = {0, 0, 1}; vector4d qx, qy, qz, qt; quaternion_from_axisangle(&qx, &vx, rx); quaternion_from_axisangle(&qy, &vy, ry); quaternion_from_axisangle(&qz, &vz, rz); quaternion_multiply(&qt, &qx, &qy); quaternion_multiply(q, &qt, &qz); }
rs2_pose pose_multiply(const rs2_pose& ref2_in_ref1, const rs2_pose& ref3_in_ref2) { rs2_pose ref3_in_ref1; ref3_in_ref1.rotation = quaternion_multiply(ref2_in_ref1.rotation, ref3_in_ref2.rotation); ref3_in_ref1.translation = vector_addition(quaternion_rotate_vector(ref2_in_ref1.rotation, ref3_in_ref2.translation), ref2_in_ref1.translation); return ref3_in_ref1; }
/** * @ingroup experimental * @brief rotate a quaternion by a quaternion (basically, multiply and then normalize) * * @param self the quaternion being rotated * @param qT the other quaternion * */ HYPAPI quaternion * quaternion_rotate_by_quaternion_EXP(quaternion *self, const quaternion *qT) { /* self = self * qT */ quaternion_multiply(self, qT); quaternion_normalize(self); return self; }
/* * Rotate vector v by unit quaternion q: * * v' = q q_v q^-1, where q_v = [0, v] */ void quaternion_rotation(struct point_XYZ *ret, const Quaternion *quat, const struct point_XYZ *v) { Quaternion q_v, q_i, q_r1, q_r2; q_v.w = 0.0; q_v.x = v->x; q_v.y = v->y; q_v.z = v->z; quaternion_inverse(&q_i, quat); quaternion_multiply(&q_r1, &q_v, &q_i); quaternion_multiply(&q_r2, quat, &q_r1); ret->x = q_r2.x; ret->y = q_r2.y; ret->z = q_r2.z; /* printf("Quaternion rotation: ret = {%f, %f, %f}, quat = {%f, %f, %f, %f}, v = {%f, %f, %f}\n", ret->x, ret->y, ret->z, quat->w, quat->x, quat->y, quat->z, v->x, v->y, v->z); */ }
static char * test_quaternion_multiply() { quaternion qA, qB; quaternion_set_from_axis_anglev3(&qA, HYP_VECTOR3_UNIT_X, HYP_TAU / 8.0f); quaternion_set_from_axis_anglev3(&qB, HYP_VECTOR3_UNIT_X, HYP_TAU / 4.0f); /* qA squared */ quaternion_multiply(&qA, &qA); test_assert(quaternion_equals(&qA, &qB)); return 0; }
/** [quaternion inverse example] */ static char * test_quaternion_inverse() { quaternion qA; quaternion qInverse; quaternion qIdentity; quaternion_set_from_axis_anglef3(&qA, 1.0f, 1.0f, 1.0f, HYP_TAU / 4.0f); quaternion_set(&qInverse, &qA); quaternion_inverse(&qInverse); quaternion_multiply(&qA, &qInverse); quaternion_normalize(&qA); quaternion_identity(&qIdentity); test_assert(quaternion_equals(&qA, &qIdentity)); return 0; }
/** * @ingroup vector3 * @brief Reflect a point by the quaternion. Returns the reflected point. (through origin) * * \f$self= qT * self * qT\f$ * * @param qT the quaternion * @param self the starting point that is rotated by qT * */ HYPAPI vector3 * vector3_reflect_by_quaternion(vector3 *self, const quaternion *qT) { quaternion q; quaternion_set(&q, qT); quaternion_multiplyv3(&q, self); quaternion_multiply(&q, qT); /* this seems to be necessary */ quaternion_normalize(&q); self->x = q.x; self->y = q.y; self->z = q.z; return self; }
static char * test_quaternion_multiply_identity() { quaternion qA, qB, q; qA.x = 1; qA.y = 2; qA.z = 3; qA.w = 4; quaternion_identity(&qB); quaternion_set(&q, &qA); quaternion_multiply(&q, &qB); test_assert(quaternion_equals(&q, &qA)); return 0; }
/** * @ingroup vector3 * @brief Rotate a point by the quaternion. Returns the rotated point. * * \f$self= qT * self * conjugate(qT)\f$ * * @param self the starting point * @param qT the quaternion * */ HYPAPI vector3 * vector3_rotate_by_quaternion(vector3 *self, const quaternion *qT) { quaternion qinverse; quaternion q; /* make the conjugate */ quaternion_set(&qinverse, qT); quaternion_conjugate(&qinverse); quaternion_set(&q, qT); quaternion_multiplyv3(&q, self); quaternion_multiply(&q, &qinverse); self->x = q.x; self->y = q.y; self->z = q.z; return self; }
rs2_vector quaternion_rotate_vector(const rs2_quaternion& q, const rs2_vector& v) { rs2_quaternion v_as_quaternion = { v.x, v.y, v.z, 0 }; rs2_quaternion rotated_v = quaternion_multiply(quaternion_multiply(q, v_as_quaternion), quaternion_conjugate(q)); return rs2_vector { rotated_v.x, rotated_v.y, rotated_v.z }; }
void Estimator::run_estimator(const vector_t& accel, const vector_t& gyro, const uint64_t& imu_time) { _current_state.now_us = imu_time; if (last_time == 0 || _current_state.now_us <= last_time) { last_time = _current_state.now_us; return; } float dt = (_current_state.now_us - last_time) * 1e-6f; last_time = _current_state.now_us; // Crank up the gains for the first few seconds for quick convergence if (imu_time < (uint64_t)params->get_param_int(Params::PARAM_INIT_TIME)*1000) { kp = params->get_param_float(Params::PARAM_FILTER_KP)*10.0f; ki = params->get_param_float(Params::PARAM_FILTER_KI)*10.0f; } else { kp = params->get_param_float(Params::PARAM_FILTER_KP); ki = params->get_param_float(Params::PARAM_FILTER_KI); } // Run LPF to reject a lot of noise run_LPF(accel, gyro); // add in accelerometer float a_sqrd_norm = _accel_LPF.x*_accel_LPF.x + _accel_LPF.y*_accel_LPF.y + _accel_LPF.z*_accel_LPF.z; if (use_acc && a_sqrd_norm < 1.15f*1.15f*9.80665f*9.80665f && a_sqrd_norm > 0.85f*0.85f*9.80665f*9.80665f) { // Get error estimated by accelerometer measurement vector_t a = vector_normalize(_accel_LPF); // Get the quaternion from accelerometer (low-frequency measure q) // (Not in either paper) quaternion_t q_acc_inv = quaternion_inverse(quat_from_two_vectors(a, g)); // Get the error quaternion between observer and low-freq q // Below Eq. 45 Mahoney Paper q_tilde = quaternion_multiply(q_acc_inv, q_hat); // Correction Term of Eq. 47a and 47b Mahoney Paper // w_acc = 2*s_tilde*v_tilde w_acc.x = -2.0f*q_tilde.w*q_tilde.x; w_acc.y = -2.0f*q_tilde.w*q_tilde.y; w_acc.z = 0.0f; // Don't correct z, because it's unobservable from the accelerometer // integrate biases from accelerometer feedback // (eq 47b Mahoney Paper, using correction term w_acc found above) b.x -= ki*w_acc.x*dt; b.y -= ki*w_acc.y*dt; b.z = 0.0; // Don't integrate z bias, because it's unobservable } else { w_acc.x = 0.0f; w_acc.y = 0.0f; w_acc.z = 0.0f; } // Pull out Gyro measurements if (quad_int) { // Quadratic Integration (Eq. 14 Casey Paper) // this integration step adds 12 us on the STM32F10x chips wbar = vector_add(vector_add(scalar_multiply(-1.0f/12.0f,w2), scalar_multiply(8.0f/12.0f,w1)), scalar_multiply(5.0f/12.0f,_gyro_LPF)); w2 = w1; w1 = _gyro_LPF; } else { wbar = _gyro_LPF; } // Build the composite omega vector for kinematic propagation // This the stuff inside the p function in eq. 47a - Mahoney Paper wfinal = vector_add(vector_sub(wbar, b), scalar_multiply(kp, w_acc)); // Propagate Dynamics (only if we've moved) float sqrd_norm_w = sqrd_norm(wfinal); if (sqrd_norm_w > 0.0f) { float p = wfinal.x; float q = wfinal.y; float r = wfinal.z; if (mat_exp) { // Matrix Exponential Approximation (From Attitude Representation and Kinematic // Propagation for Low-Cost UAVs by Robert T. Casey) // (Eq. 12 Casey Paper) // This adds 90 us on STM32F10x chips float norm_w = sqrtf(sqrd_norm_w); quaternion_t qhat_np1; float t1 = cosf((norm_w*dt)/2.0f); float t2 = 1.0f/norm_w * sinf((norm_w*dt)/2.0f); qhat_np1.w = t1*q_hat.w + t2*( - p*q_hat.x - q*q_hat.y - r*q_hat.z); qhat_np1.x = t1*q_hat.x + t2*(p*q_hat.w + r*q_hat.y - q*q_hat.z); qhat_np1.y = t1*q_hat.y + t2*(q*q_hat.w - r*q_hat.x + p*q_hat.z); qhat_np1.z = t1*q_hat.z + t2*(r*q_hat.w + q*q_hat.x - p*q_hat.y); q_hat = quaternion_normalize(qhat_np1); } else { // Euler Integration // (Eq. 47a Mahoney Paper), but this is pretty straight-forward quaternion_t qdot = {0.5f * ( - p*q_hat.x - q*q_hat.y - r*q_hat.z), 0.5f * ( p*q_hat.w + r*q_hat.y - q*q_hat.z), 0.5f * ( q*q_hat.w - r*q_hat.x + p*q_hat.z), 0.5f * ( r*q_hat.w + q*q_hat.x - p*q_hat.y) }; q_hat.w += qdot.w*dt; q_hat.x += qdot.x*dt; q_hat.y += qdot.y*dt; q_hat.z += qdot.z*dt; q_hat = quaternion_normalize(q_hat); } } // Save attitude estimate _current_state.q = q_hat; // Extract Euler Angles for controller euler_from_quat(_current_state.q, &_current_state.euler.x, &_current_state.euler.y, &_current_state.euler.z); // Save off adjust gyro measurements with estimated biases for control _current_state.omega = vector_sub(_gyro_LPF, b); }