TEST_F(QuaternionTest, sperical_linear_interpolation_between_from_ant_to_with_t_equal_one_return_to)
{
auto from = create_random_quaternion();
auto to = create_random_quaternion();
quaternion_normalize(from);
quaternion_normalize(to);
const auto res = quaternion_slerp(from, to, 1.0);
EXPECT_EQ(to.w(), res.w());
EXPECT_EQ(to.x(), res.x());
EXPECT_EQ(to.y(), res.y());
EXPECT_EQ(to.z(), res.z());
}
TEST_F(QuaternionTest, normalizing_quaternion_gives_a_unit_quaternion)
{
auto quat = create_random_quaternion();
quaternion_normalize(quat);
EXPECT_FLOAT_EQ(1, quaternion_norm(quat));
}
TEST_F(QuaternionTest, sperical_linear_interpolation_makes_correct_quaternion)
{
auto from = create_random_quaternion();
auto to = create_random_quaternion();
quaternion_normalize(from);
quaternion_normalize(to);
const auto t =(rand() % 400) / 400.0;
const auto res = quaternion_slerp(from, to, t);
const auto correct = slerp(from, to, t);
EXPECT_EQ(correct.w(), res.w());
EXPECT_EQ(correct.x(), res.x());
EXPECT_EQ(correct.y(), res.y());
EXPECT_EQ(correct.z(), res.z());
}
static char * test_quaternion_axis_anglev3()
{
quaternion q, q1;
float c;
float s;
float angle = HYP_TAU / 4.0f;
vector3 axis;
quaternion_set_from_axis_anglev3(&q, HYP_VECTOR3_UNIT_X, angle);
vector3_set(&axis, HYP_VECTOR3_UNIT_X);
c = HYP_COS(angle / 2.0f);
s = HYP_SIN(angle / 2.0f);
q1.x = axis.x * s;
q1.y = axis.y * s;
q1.z = axis.z * s;
q1.w = c;
quaternion_normalize(&q1);
test_assert(quaternion_equals(&q, &q1));
return 0;
}
/**
* @ingroup experimental
* @brief This code is suspect. Treats two quaternions sort of like 2 vector4's and then computes the cross-product between them.
*
*/
HYPAPI void quaternion_axis_between_EXP(const quaternion *self, const quaternion *qT, quaternion *qR)
{
quaternion axis;
axis = quaternion_cross_product_EXP(self, qT);
quaternion_set(qR, &axis);
quaternion_normalize(qR);
}
/**
* @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;
}
/**
* @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;
}
void
quaternion_togl(Quaternion *quat)
{
if (APPROX(fabs(quat->w), 1)) { return; }
if (quat->w > 1) { quaternion_normalize(quat); }
/* get the angle, but turn us around 180 degrees */
/* printf ("togl: setting rotation %f %f %f %f\n",quat->w,quat->x,quat->y,quat->z);*/
FW_GL_ROTATE_RADIANS(2.0 * acos(quat->w), quat->x, quat->y, quat->z);
}
/**
* @ingroup quaternion
* @brief initializes the quaternion with random values, then normalizes it
*/
HYPAPI quaternion * _quaternion_set_random(quaternion *self)
{
self->x = HYP_RANDOM_FLOAT;
self->y = HYP_RANDOM_FLOAT;
self->z = HYP_RANDOM_FLOAT;
self->w = HYP_RANDOM_FLOAT;
quaternion_normalize(self);
return self;
}
void
quaternion_inverse(Quaternion *ret, const Quaternion *quat)
{
/* double n = norm(quat); */
quaternion_set(ret, quat);
quaternion_conjugate(ret);
/* unit quaternion, so take conjugate */
quaternion_normalize(ret);
/* printf("Quaternion inverse: ret = {%f, %f, %f, %f}, quat = {%f, %f, %f, %f}\n",
ret->w, ret->x, ret->y, ret->z, quat->w, quat->x, quat->y, quat->z); */
}
/**
* @ingroup quaternion
* @brief Sets the values in the quaternion, in place, based on the axis and angle.
*
* @param self the quaternion that will become initialized with the values of the axis and angle
* @param x the x axis
* @param y the y axis
* @param z the z axis
* @param angle the angle is in radians
*
* q = cos(a/2) + i ( x * sin(a/2)) + j (y * sin(a/2)) + k ( z * sin(a/2))
*
*/
HYPAPI quaternion * quaternion_set_from_axis_anglef3(quaternion *self, float x, float y, float z, float angle)
{
float s = HYP_SIN(angle / 2.0f);
float c = HYP_COS(angle / 2.0f);
self->x = x * s;
self->y = y * s;
self->z = z * s;
self->w = c;
/* reduce rounding errors caused by sin/cos */
return quaternion_normalize(self);
}
/** [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;
}
void
vrmlrot_to_quaternion(Quaternion *quat, const double x, const double y, const double z, const double a)
{
double s;
double scale = sqrt((x * x) + (y * y) + (z * z));
/* no rotation - use (multiplication ???) identity quaternion */
if (APPROX(scale, 0)) {
quat->w = 1;
quat->x = 0;
quat->y = 0;
quat->z = 0;
} else {
s = sin(a/2);
/* normalize rotation axis to convert VRML rotation to quaternion */
quat->w = cos(a / 2);
quat->x = s * (x / scale);
quat->y = s * (y / scale);
quat->z = s * (z / scale);
quaternion_normalize(quat);
}
}
/**
* @ingroup quaternion
* @brief Normalized LERP. This calls lerp and then normalizes the quaternion (pulls it back on the manifold)
*
* @param percent This refers to how far along we want to go toward end. 1.0 means go to the end. 0.0 means start. 1.1 is nonsense. Negative is nonsense.
* @param start The quaternion representing the starting orientation
* @param end The quaternion representing the final or ending orientation
* @param qR The resulting new orientation.
*
*/
HYPAPI quaternion * quaternion_nlerp(const quaternion *start, const quaternion *end, float percent, quaternion *qR)
{
quaternion_lerp(start, end, percent, qR);
quaternion_normalize(qR);
return qR;
}
/* now with quaternions AH */
void rotate(system_t *system, molecule_t *molecule, pbc_t *pbc, double scale) {
atom_t *atom_ptr;
double u1, u2, u3;
double com[3];
int i, ii, n;
double *new_coord_array;
struct quaternion position_vector, rnd_rotation, rnd_rotation_conjugate, answer;
// create a random rotation
// http://planning.cs.uiuc.edu/node198.html
u1 = get_rand(system);
u2 = get_rand(system);
u3 = get_rand(system);
// simple linear interpolation for adjusting the scale
quaternion_construct_xyzw(&rnd_rotation, scale*sqrt(1-u1)*sin(2*M_PI*u2), scale*sqrt(1-u1)*cos(2*M_PI*u2), scale*sqrt(u1)*sin(2*M_PI*u3), 1-scale+scale*sqrt(u1)*cos(2*M_PI*u3)); /* make a random quaternion */
quaternion_normalize(&rnd_rotation);
quaternion_conjugate(&rnd_rotation, &rnd_rotation_conjugate); /* needed to transform coordinates */
/* count the number of atoms in a molecule, and allocate new coords array */
for (atom_ptr = molecule->atoms, n = 0; atom_ptr; atom_ptr = atom_ptr->next)
++n;
new_coord_array = calloc(n * 3, sizeof(double));
memnullcheck(new_coord_array, n * 3 * sizeof(double), __LINE__ - 1, __FILE__);
/* save the com coordinate */
com[0] = molecule->com[0];
com[1] = molecule->com[1];
com[2] = molecule->com[2];
/* translate the molecule to the origin */
for (atom_ptr = molecule->atoms; atom_ptr; atom_ptr = atom_ptr->next) {
atom_ptr->pos[0] -= com[0];
atom_ptr->pos[1] -= com[1];
atom_ptr->pos[2] -= com[2];
}
/* quaternion multiply */
for (atom_ptr = molecule->atoms, i = 0; atom_ptr; atom_ptr = atom_ptr->next, i++) {
ii = i * 3;
//position_vector = position
quaternion_construct_xyzw(&position_vector, atom_ptr->pos[0], atom_ptr->pos[1], atom_ptr->pos[2], 0.);
//answer = rnd_rotation*(position*rnd_rotation_conjugate)
quaternion_multiplication(&position_vector, &rnd_rotation_conjugate, &answer);
quaternion_multiplication(&rnd_rotation, &answer, &answer);
//set the new coords
new_coord_array[ii + 0] = answer.x;
new_coord_array[ii + 1] = answer.y;
new_coord_array[ii + 2] = answer.z;
}
/* set the new coordinates and then translate back from the origin */
for (atom_ptr = molecule->atoms, i = 0; atom_ptr; atom_ptr = atom_ptr->next, i++) {
ii = i * 3;
atom_ptr->pos[0] = new_coord_array[ii + 0];
atom_ptr->pos[1] = new_coord_array[ii + 1];
atom_ptr->pos[2] = new_coord_array[ii + 2];
atom_ptr->pos[0] += com[0];
atom_ptr->pos[1] += com[1];
atom_ptr->pos[2] += com[2];
}
/* free our temporary array */
free(new_coord_array);
}
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);
}
