static void _psmove_orientation_fusion_complementary_marg_update( PSMoveOrientation *orientation_state, float delta_t, const glm::vec3 ¤t_omega, const glm::vec3 ¤t_g, const glm::vec3 ¤t_m) { // Get the direction of the magnetic fields in the identity pose PSMove_3AxisVector identity_m= psmove_orientation_get_magnetometer_calibration_direction(orientation_state); glm::vec3 k_identity_m_direction = glm::vec3(identity_m.x, identity_m.y, identity_m.z); // Get the direction of the gravitational field in the identity pose PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state); glm::vec3 k_identity_g_direction = glm::vec3(identity_g.x, identity_g.y, identity_g.z); // Angular Rotation (AR) Update //----------------------------- // Compute the rate of change of the orientation purely from the gyroscope // q_dot = 0.5*q*omega glm::quat q_current= orientation_state->quaternion; glm::quat q_omega = glm::quat(0.f, current_omega.x, current_omega.y, current_omega.z); glm::quat q_derivative = (q_current*0.5f) * q_omega; // Integrate the rate of change to get a new orientation // q_new= q + q_dot*dT glm::quat q_step = q_derivative * delta_t; glm::quat ar_orientation = q_current + q_step; // Make sure the resulting quaternion is normalized ar_orientation= glm::normalize(ar_orientation); // Magnetic/Gravity (MG) Update //----------------------------- const glm::vec3* mg_from[2] = { &k_identity_g_direction, &k_identity_m_direction }; const glm::vec3* mg_to[2] = { ¤t_g, ¤t_m }; glm::quat mg_orientation; // Always attempt to align with the identity_mg, even if we don't get within the alignment tolerance. // More often then not we'll be better off moving forward with what we have and trying to refine // the alignment next frame. psmove_alignment_quaternion_between_vector_frames( mg_from, mg_to, 0.1f, q_current, mg_orientation); // Blending Update //---------------- // The final rotation is a blend between the integrated orientation and absolute rotation from the earth-frame float mg_wight = orientation_state->fusion_state.complementary_marg_state.mg_weight; orientation_state->quaternion= psmove_quaternion_normalized_lerp(ar_orientation, mg_orientation, mg_wight); // Update the blend weight orientation_state->fusion_state.complementary_marg_state.mg_weight = lerp_clampf(mg_wight, k_base_earth_frame_align_weight, 0.9f); }
static void _psmove_orientation_fusion_complementary_marg_update( PSMoveOrientation *orientation_state, float delta_t, const Eigen::Vector3f ¤t_omega, const Eigen::Vector3f ¤t_g, const Eigen::Vector3f ¤t_m) { // TODO: Following variable is unused PSMoveMadgwickMARGState *marg_state = &orientation_state->fusion_state.madgwick_marg_state; // Get the direction of the magnetic fields in the identity pose PSMove_3AxisVector identity_m= psmove_orientation_get_magnetometer_calibration_direction(orientation_state); Eigen::Vector3f k_identity_m_direction = Eigen::Vector3f(identity_m.x, identity_m.y, identity_m.z); // Get the direction of the gravitational field in the identity pose PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state); Eigen::Vector3f k_identity_g_direction = Eigen::Vector3f(identity_g.x, identity_g.y, identity_g.z); // Angular Rotation (AR) Update //----------------------------- // Compute the rate of change of the orientation purely from the gyroscope // q_dot = 0.5*q*omega Eigen::Quaternionf q_omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z()); Eigen::Quaternionf q_derivative = Eigen::Quaternionf(orientation_state->quaternion.coeffs()*0.5f) * q_omega; // Integrate the rate of change to get a new orientation // q_new= q + q_dot*dT Eigen::Quaternionf q_step = Eigen::Quaternionf(q_derivative.coeffs() * delta_t); Eigen::Quaternionf ar_orientation = Eigen::Quaternionf(orientation_state->quaternion.coeffs() + q_step.coeffs()); // Make sure the resulting quaternion is normalized ar_orientation.normalize(); // Magnetic/Gravity (MG) Update //----------------------------- const Eigen::Vector3f* mg_from[2] = { &k_identity_g_direction, &k_identity_m_direction }; const Eigen::Vector3f* mg_to[2] = { ¤t_g, ¤t_m }; Eigen::Quaternionf mg_orientation; bool mg_align_success = psmove_alignment_quaternion_between_vector_frames( mg_from, mg_to, 0.1f, orientation_state->quaternion, mg_orientation); // Blending Update //---------------- if (mg_align_success) { // The final rotation is a blend between the integrated orientation and absolute rotation from the earth-frame float mg_wight = orientation_state->fusion_state.complementary_marg_state.mg_weight; orientation_state->quaternion = psmove_quaternion_normalized_lerp(ar_orientation, mg_orientation, mg_wight); // Update the blend weight orientation_state->fusion_state.complementary_marg_state.mg_weight = lerp_clampf(mg_wight, k_base_earth_frame_align_weight, 0.9f); } else { orientation_state->quaternion = ar_orientation; } }
void OrientationFilterComplementaryMARG::update(const float delta_time, const PoseFilterPacket &packet) { const Eigen::Vector3f ¤t_omega= packet.imu_gyroscope_rad_per_sec; Eigen::Vector3f current_g= packet.imu_accelerometer_g_units; eigen_vector3f_normalize_with_default(current_g, Eigen::Vector3f::Zero()); Eigen::Vector3f current_m= packet.imu_magnetometer_unit; eigen_vector3f_normalize_with_default(current_m, Eigen::Vector3f::Zero()); // Get the direction of the magnetic fields in the identity pose. Eigen::Vector3f k_identity_m_direction = m_constants.magnetometer_calibration_direction; // Get the direction of the gravitational fields in the identity pose Eigen::Vector3f k_identity_g_direction = m_constants.gravity_calibration_direction; // Angular Rotation (AR) Update //----------------------------- // Compute the rate of change of the orientation purely from the gyroscope // q_dot = 0.5*q*omega Eigen::Quaternionf q_current= m_state->orientation; Eigen::Quaternionf q_omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z()); Eigen::Quaternionf q_derivative = Eigen::Quaternionf(q_current.coeffs()*0.5f) * q_omega; // Integrate the rate of change to get a new orientation // q_new= q + q_dot*dT Eigen::Quaternionf q_step = Eigen::Quaternionf(q_derivative.coeffs() * delta_time); Eigen::Quaternionf ar_orientation = Eigen::Quaternionf(q_current.coeffs() + q_step.coeffs()); // Make sure the resulting quaternion is normalized ar_orientation.normalize(); // Magnetic/Gravity (MG) Update //----------------------------- const Eigen::Vector3f* mg_from[2] = { &k_identity_g_direction, &k_identity_m_direction }; const Eigen::Vector3f* mg_to[2] = { ¤t_g, ¤t_m }; Eigen::Quaternionf mg_orientation; // Always attempt to align with the identity_mg, even if we don't get within the alignment tolerance. // More often then not we'll be better off moving forward with what we have and trying to refine // the alignment next frame. eigen_alignment_quaternion_between_vector_frames( mg_from, mg_to, 0.1f, q_current, mg_orientation); // Blending Update //---------------- // Save the new quaternion and first derivative back into the orientation state // Derive the second derivative { // The final rotation is a blend between the integrated orientation and absolute rotation from the earth-frame const Eigen::Quaternionf new_orientation = eigen_quaternion_normalized_lerp(ar_orientation, mg_orientation, mg_weight); const Eigen::Vector3f &new_angular_velocity= current_omega; const Eigen::Vector3f new_angular_acceleration = (current_omega - m_state->angular_velocity) / delta_time; m_state->apply_state(new_orientation, new_angular_velocity, new_angular_acceleration); } // Update the blend weight // -- Exponential blend the MG weight from 1 down to k_base_earth_frame_align_weight mg_weight = lerp_clampf(mg_weight, k_base_earth_frame_align_weight, 0.9f); }
static void orientation_fusion_complementary_marg_update( const float delta_time, const OrientationFilterSpace *filter_space, const OrientationFilterPacket *filter_packet, OrientationSensorFusionState *fusion_state) { const Eigen::Vector3f ¤t_omega= filter_packet->gyroscope; const Eigen::Vector3f ¤t_g= filter_packet->normalized_accelerometer; const Eigen::Vector3f ¤t_m= filter_packet->normalized_magnetometer; // Get the direction of the magnetic fields in the identity pose. Eigen::Vector3f k_identity_m_direction = filter_space->getMagnetometerCalibrationDirection(); // Get the direction of the gravitational fields in the identity pose Eigen::Vector3f k_identity_g_direction = filter_space->getGravityCalibrationDirection(); // Angular Rotation (AR) Update //----------------------------- // Compute the rate of change of the orientation purely from the gyroscope // q_dot = 0.5*q*omega Eigen::Quaternionf q_current= fusion_state->orientation; Eigen::Quaternionf q_omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z()); Eigen::Quaternionf q_derivative = Eigen::Quaternionf(q_current.coeffs()*0.5f) * q_omega; // Integrate the rate of change to get a new orientation // q_new= q + q_dot*dT Eigen::Quaternionf q_step = Eigen::Quaternionf(q_derivative.coeffs() * delta_time); Eigen::Quaternionf ar_orientation = Eigen::Quaternionf(q_current.coeffs() + q_step.coeffs()); // Make sure the resulting quaternion is normalized ar_orientation.normalize(); // Magnetic/Gravity (MG) Update //----------------------------- const Eigen::Vector3f* mg_from[2] = { &k_identity_g_direction, &k_identity_m_direction }; const Eigen::Vector3f* mg_to[2] = { ¤t_g, ¤t_m }; Eigen::Quaternionf mg_orientation; // Always attempt to align with the identity_mg, even if we don't get within the alignment tolerance. // More often then not we'll be better off moving forward with what we have and trying to refine // the alignment next frame. eigen_alignment_quaternion_between_vector_frames( mg_from, mg_to, 0.1f, q_current, mg_orientation); // Blending Update //---------------- float mg_wight = fusion_state->fusion_state.complementary_marg_state.mg_weight; // Save the new quaternion and first derivative back into the orientation state // Derive the second derivative { // The final rotation is a blend between the integrated orientation and absolute rotation from the earth-frame const Eigen::Quaternionf new_orientation = eigen_quaternion_normalized_lerp(ar_orientation, mg_orientation, mg_wight); const Eigen::Quaternionf &new_first_derivative = q_derivative; const Eigen::Quaternionf new_second_derivative = Eigen::Quaternionf( (new_first_derivative.coeffs() - fusion_state->orientation_first_derivative.coeffs()) / delta_time); fusion_state->orientation = new_orientation; fusion_state->orientation_second_derivative = new_first_derivative; fusion_state->orientation_first_derivative = new_second_derivative; } // Update the blend weight fusion_state->fusion_state.complementary_marg_state.mg_weight = lerp_clampf(mg_wight, k_base_earth_frame_align_weight, 0.9f); }
// -- OrientationFilterComplementaryOpticalARG -- void OrientationFilterComplementaryOpticalARG::update(const float delta_time, const PoseFilterPacket &packet) { if (packet.tracking_projection_area_px_sqr <= k_real_epsilon) { OrientationFilterMadgwickARG::update(delta_time, packet); return; } const Eigen::Vector3f ¤t_omega= packet.imu_gyroscope_rad_per_sec; Eigen::Vector3f current_g= packet.imu_accelerometer_g_units; eigen_vector3f_normalize_with_default(current_g, Eigen::Vector3f::Zero()); // Current orientation from earth frame to sensor frame const Eigen::Quaternionf SEq = m_state->orientation; Eigen::Quaternionf SEq_new = SEq; // Compute the quaternion derivative measured by gyroscopes // Eqn 12) q_dot = 0.5*q*omega Eigen::Quaternionf omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z()); Eigen::Quaternionf SEqDot_omega = Eigen::Quaternionf(SEq.coeffs() * 0.5f) *omega; if (!current_g.isApprox(Eigen::Vector3f::Zero(), k_normal_epsilon)) { // Get the direction of the gravitational fields in the identity pose Eigen::Vector3f k_identity_g_direction = m_constants.gravity_calibration_direction; // Eqn 15) Applied to the gravity vector // Fill in the 3x1 objective function matrix f(SEq, Sa) =|f_g| Eigen::Matrix<float, 3, 1> f_g; eigen_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL); // Eqn 21) Applied to the gravity vector // Fill in the 4x3 objective function Jacobian matrix: J_gb(SEq)= [J_g] Eigen::Matrix<float, 4, 3> J_g; eigen_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g); // Eqn 34) gradient_F= J_g(SEq)*f(SEq, Sa) // Compute the gradient of the objective function Eigen::Matrix<float, 4, 1> gradient_f = J_g * f_g; Eigen::Quaternionf SEqHatDot = Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0)); // normalize the gradient eigen_quaternion_normalize_with_default(SEqHatDot, *k_eigen_quaternion_zero); // Compute the estimated quaternion rate of change // Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot const float beta= sqrtf(3.0f / 4.0f) * fmaxf(fmaxf(m_constants.gyro_variance.x(), m_constants.gyro_variance.y()), m_constants.gyro_variance.z()); Eigen::Quaternionf SEqDot_est = Eigen::Quaternionf(SEqDot_omega.coeffs() - SEqHatDot.coeffs()*beta); // Compute then integrate the estimated quaternion rate // Eqn 42) SEq_new = SEq + SEqDot_est*delta_t SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_est.coeffs()*delta_time); } else { SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_omega.coeffs()*delta_time); } // Make sure the net quaternion is a pure rotation quaternion SEq_new.normalize(); // Save the new quaternion and first derivative back into the orientation state // Derive the second derivative { // The final rotation is a blend between the integrated orientation and absolute optical orientation const float max_variance_fraction = safe_divide_with_default( m_constants.orientation_variance_curve.evaluate(packet.tracking_projection_area_px_sqr), m_constants.orientation_variance_curve.MaxValue, 1.f); float optical_weight= lerp_clampf(1.f - max_variance_fraction, 0, k_max_optical_orientation_weight); static float g_weight_override= -1.f; if (g_weight_override >= 0.f) { optical_weight= g_weight_override; } const Eigen::EulerAnglesf optical_euler_angles = eigen_quaternionf_to_euler_angles(packet.optical_orientation); const Eigen::EulerAnglesf SEeuler_new= eigen_quaternionf_to_euler_angles(packet.optical_orientation); // Blend in the yaw from the optical orientation const float blended_heading_radians= wrap_lerpf( SEeuler_new.get_heading_radians(), optical_euler_angles.get_heading_radians(), optical_weight, -k_real_pi, k_real_pi); const Eigen::EulerAnglesf new_euler_angles( SEeuler_new.get_bank_radians(), blended_heading_radians, SEeuler_new.get_attitude_radians()); const Eigen::Quaternionf new_orientation = eigen_euler_angles_to_quaternionf(new_euler_angles); const Eigen::Vector3f &new_angular_velocity= current_omega; const Eigen::Vector3f new_angular_acceleration = (current_omega - m_state->angular_velocity) / delta_time; m_state->apply_state(new_orientation, new_angular_velocity, new_angular_acceleration); } }