void vrpn_Tracker_WiimoteHead::_update_gravity_moving_avg() { // Moving average of last three gravity vectors /// @todo replace/supplement gravity moving average with Kalman filter q_vec_type movingAvg = Q_NULL_VECTOR; q_vec_copy(movingAvg, d_vGrav); q_vec_add(movingAvg, movingAvg, d_vGravPenultimate); q_vec_add(movingAvg, movingAvg, d_vGravAntepenultimate); q_vec_scale(movingAvg, 0.33333, movingAvg); // reset gravity transform MAKE_IDENTITY_QUAT(d_gravityXform.quat); MAKE_NULL_VEC(d_gravityXform.xyz); q_vec_type regulargravity = Q_NULL_VECTOR; regulargravity[2] = 1; q_from_two_vecs(d_gravityXform.quat, movingAvg, regulargravity); d_gravDirty = false; }
void vrpn_IMU_SimpleCombiner::update_matrix_based_on_values(double time_interval) { //================================================================== // Adjust the orientation based on the rotational velocity, which is // measured in the rotated coordinate system. We need to rotate the // difference vector back to the canonical orientation, apply the // orientation change there, and then rotate back. // Be sure to scale by the time value. q_type forward, inverse; q_copy(forward, d_quat); q_invert(inverse, forward); // Remember that Euler angles in Quatlib have rotation around Z in // the first term. Compute the time-scaled delta transform in // canonical space. q_type delta; q_from_euler(delta, time_interval * d_rotational_vel.values[Q_Z], time_interval * d_rotational_vel.values[Q_Y], time_interval * d_rotational_vel.values[Q_X]); // Bring the delta back into canonical space q_type canonical; q_mult(canonical, delta, inverse); q_mult(canonical, forward, canonical); q_mult(d_quat, canonical, d_quat); //================================================================== // To the extent that the acceleration vector's magnitude is equal // to the expected gravity, rotate the orientation so that the vector // points downward. This is measured in the rotated coordinate system, // so we need to rotate back to canonical orientation and apply // the difference there and then rotate back. The rate of rotation // should be as specified in the gravity-rotation-rate parameter so // we don't swing the head around too quickly but only slowly re-adjust. double accel = sqrt( d_acceleration.values[0] * d_acceleration.values[0] + d_acceleration.values[1] * d_acceleration.values[1] + d_acceleration.values[2] * d_acceleration.values[2] ); double diff = fabs(accel - 9.80665); // Only adjust if we're close enough to the expected gravity // @todo In a more complicated IMU tracker, base this on state and // error estimates from a Kalman or other filter. double scale = 1.0 - diff; if (scale > 0) { // Get a new forward and inverse matrix from the current, just- // rotated matrix. q_copy(forward, d_quat); // Change how fast we adjust based on how close we are to the // expected value of gravity. Then further scale this by the // amount of time since the last estimate. double gravity_scale = scale * d_gravity_restore_rate * time_interval; // Rotate the gravity vector by the estimated transform. // We expect this direction to match the global down (-Y) vector. q_vec_type gravity_global; q_vec_set(gravity_global, d_acceleration.values[0], d_acceleration.values[1], d_acceleration.values[2]); q_vec_normalize(gravity_global, gravity_global); q_xform(gravity_global, forward, gravity_global); //printf(" XXX Gravity: %lg, %lg, %lg\n", gravity_global[0], gravity_global[1], gravity_global[2]); // Determine the rotation needed to take gravity and rotate // it into the direction of -Y. q_vec_type neg_y; q_vec_set(neg_y, 0, -1, 0); q_type rot; q_from_two_vecs(rot, gravity_global, neg_y); // Scale the rotation by the fraction of the orientation we // should remove based on the time that has passed, how well our // gravity vector matches expected, and the specified rate of // correction. static q_type identity = { 0, 0, 0, 1 }; q_type scaled_rot; q_slerp(scaled_rot, identity, rot, gravity_scale); //printf("XXX Scaling gravity vector by %lg\n", gravity_scale); // Rotate by this angle. q_mult(d_quat, scaled_rot, d_quat); //================================================================== // If we are getting compass data, and to the extent that the // acceleration vector's magnitude is equal to the expected gravity, // compute the cross product of the cross product to find the // direction of north perpendicular to down. This is measured in // the rotated coordinate system, so we need to rotate back to the // canonical orientation and do the comparison there. // The fraction of rotation should be as specified in the // magnetometer-rotation-rate parameter so we don't swing the head // around too quickly but only slowly re-adjust. if (d_magnetometer.ana != NULL) { // Get a new forward and inverse matrix from the current, just- // rotated matrix. q_copy(forward, d_quat); // Find the North vector that is perpendicular to gravity by // clearing its Y axis to zero and renormalizing. q_vec_type magnetometer; q_vec_set(magnetometer, d_magnetometer.values[0], d_magnetometer.values[1], d_magnetometer.values[2]); q_vec_type magnetometer_global; q_xform(magnetometer_global, forward, magnetometer); magnetometer_global[Q_Y] = 0; q_vec_type north_global; q_vec_normalize(north_global, magnetometer_global); //printf(" XXX north_global: %lg, %lg, %lg\n", north_global[0], north_global[1], north_global[2]); // Determine the rotation needed to take north and rotate it // into the direction of negative Z. q_vec_type neg_z; q_vec_set(neg_z, 0, 0, -1); q_from_two_vecs(rot, north_global, neg_z); // Change how fast we adjust based on how close we are to the // expected value of gravity. Then further scale this by the // amount of time since the last estimate. double north_rate = scale * d_north_restore_rate * time_interval; // Scale the rotation by the fraction of the orientation we // should remove based on the time that has passed, how well our // gravity vector matches expected, and the specified rate of // correction. static q_type identity = { 0, 0, 0, 1 }; q_slerp(scaled_rot, identity, rot, north_rate); // Rotate by this angle. q_mult(d_quat, scaled_rot, d_quat); } } //================================================================== // Compute and store the velocity information // This will be in the rotated coordinate system, so we need to // rotate back to the identity orientation before storing. // Convert from radians/second into a quaternion rotation as // rotated in a hundredth of a second and set the rotational // velocity dt to a hundredth of a second so that we don't // risk wrapping. // Remember that Euler angles in Quatlib have rotation around Z in // the first term. Compute the time-scaled delta transform in // canonical space. q_from_euler(delta, 1e-2 * d_rotational_vel.values[Q_Z], 1e-2 * d_rotational_vel.values[Q_Y], 1e-2 * d_rotational_vel.values[Q_X]); // Bring the delta back into canonical space q_mult(canonical, delta, inverse); q_mult(vel_quat, forward, canonical); vel_quat_dt = 1e-2; }