// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper: // "An efficient orientation filter for inertial and inertial/magnetic sensor arrays" // https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf static void _psmove_orientation_fusion_imu_update( PSMoveOrientation *orientation_state, float delta_t, const Eigen::Vector3f ¤t_omega, const Eigen::Vector3f ¤t_g) { // Current orientation from earth frame to sensor frame Eigen::Quaternionf SEq = orientation_state->quaternion; 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 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); // 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; psmove_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; psmove_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 psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero); // Compute the estimated quaternion rate of change // Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot 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_t); } else { SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_omega.coeffs()*delta_t); } // Make sure the net quaternion is a pure rotation quaternion SEq_new.normalize(); // Save the new quaternion back into the orientation state orientation_state->quaternion = SEq_new; }
// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper: // "An efficient orientation filter for inertial and inertial/magnetic sensor arrays" // https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf static void _psmove_orientation_fusion_imu_update( PSMoveOrientation *orientation_state, float delta_t, const glm::vec3 ¤t_omega, const glm::vec3 ¤t_g) { // Current orientation from earth frame to sensor frame glm::quat SEq = orientation_state->quaternion; glm::quat SEq_new = SEq; // Compute the quaternion derivative measured by gyroscopes // Eqn 12) q_dot = 0.5*q*omega glm::quat omega = glm::quat(0.f, current_omega.x, current_omega.y, current_omega.z); glm::quat SEqDot_omega = (SEq * 0.5f) *omega; if (is_nearly_equal(glm::dot(current_g, current_g), 0.f, k_normal_epsilon*k_normal_epsilon)) { // Get the direction of the gravitational fields 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); // Eqn 15) Applied to the gravity vector // Fill in the 3x1 objective function matrix f(SEq, Sa) =|f_g| glm::vec3 f_g; psmove_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] glm::mat3x4 J_g; psmove_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 glm::vec4 gradient_f = J_g * f_g; glm::quat SEqHatDot = glm::quat(gradient_f[0], gradient_f[1], gradient_f[2], gradient_f[3]); // normalize the gradient psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero); // Compute the estimated quaternion rate of change // Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot glm::quat SEqDot_est = SEqDot_omega + SEqHatDot*(-beta); // Compute then integrate the estimated quaternion rate // Eqn 42) SEq_new = SEq + SEqDot_est*delta_t SEq_new = SEq + SEqDot_est*delta_t; } else { SEq_new = SEq + SEqDot_omega*delta_t; } // Make sure the net quaternion is a pure rotation quaternion SEq_new= glm::normalize(SEq_new); // Save the new quaternion back into the orientation state orientation_state->quaternion= SEq_new; }
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; } }
// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper: // "An efficient orientation filter for inertial and inertial/magnetic sensor arrays" // https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf static void _psmove_orientation_fusion_madgwick_marg_update( PSMoveOrientation *orientation_state, float delta_t, const Eigen::Vector3f ¤t_omega, const Eigen::Vector3f ¤t_g, const Eigen::Vector3f ¤t_m) { // If there isn't a valid magnetometer or accelerometer vector, fall back to the IMU style update if (current_g.isZero(k_normal_epsilon) || current_m.isZero(k_normal_epsilon)) { _psmove_orientation_fusion_imu_update( orientation_state, delta_t, current_omega, current_g); return; } PSMoveMadgwickMARGState *marg_state = &orientation_state->fusion_state.madgwick_marg_state; // Current orientation from earth frame to sensor frame Eigen::Quaternionf SEq = orientation_state->quaternion; // Get the direction of the magnetic fields in the identity pose. // NOTE: In the original paper we converge on this vector over time automatically (See Eqn 45 & 46) // but since we've already done the work in calibration to get this vector, let's just use it. // This also removes the last assumption in this function about what // the orientation of the identity-pose is (handled by the sensor transform). 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 fields 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); // Eqn 15) Applied to the gravity and magnetometer vectors // Fill in the 6x1 objective function matrix f(SEq, Sa, Eb, Sm) =|f_g| // |f_b| Eigen::Matrix<float, 3, 1> f_g; psmove_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL); Eigen::Matrix<float, 3, 1> f_m; psmove_alignment_compute_objective_vector(SEq, k_identity_m_direction, current_m, f_m, NULL); Eigen::Matrix<float, 6, 1> f_gb; f_gb.block<3, 1>(0, 0) = f_g; f_gb.block<3, 1>(3, 0) = f_m; // Eqn 21) Applied to the gravity and magnetometer vectors // Fill in the 4x6 objective function Jacobian matrix: J_gb(SEq, Eb)= [J_g|J_b] Eigen::Matrix<float, 4, 3> J_g; psmove_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g); Eigen::Matrix<float, 4, 3> J_m; psmove_alignment_compute_objective_jacobian(SEq, k_identity_m_direction, J_m); Eigen::Matrix<float, 4, 6> J_gb; J_gb.block<4, 3>(0, 0) = J_g; J_gb.block<4, 3>(0, 3) = J_m; // Eqn 34) gradient_F= J_gb(SEq, Eb)*f(SEq, Sa, Eb, Sm) // Compute the gradient of the objective function Eigen::Matrix<float, 4, 1> gradient_f = J_gb*f_gb; Eigen::Quaternionf SEqHatDot = Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0)); // normalize the gradient to estimate direction of the gyroscope error psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero); // Eqn 47) omega_err= 2*SEq*SEqHatDot // compute angular estimated direction of the gyroscope error Eigen::Quaternionf omega_err = Eigen::Quaternionf(SEq.coeffs()*2.f) * SEqHatDot; // Eqn 48) net_omega_bias+= zeta*omega_err // Compute the net accumulated gyroscope bias marg_state->omega_bias = Eigen::Quaternionf(marg_state->omega_bias.coeffs() + omega_err.coeffs()*zeta*delta_t); marg_state->omega_bias.w() = 0.f; // no bias should accumulate on the w-component // Eqn 49) omega_corrected = omega - net_omega_bias Eigen::Quaternionf omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z()); Eigen::Quaternionf corrected_omega = Eigen::Quaternionf(omega.coeffs() - marg_state->omega_bias.coeffs()); // Compute the rate of change of the orientation purely from the gyroscope // Eqn 12) q_dot = 0.5*q*omega Eigen::Quaternionf SEqDot_omega = Eigen::Quaternionf(SEq.coeffs() * 0.5f) * corrected_omega; // Compute the estimated quaternion rate of change // Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot 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 Eigen::Quaternionf SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_est.coeffs()*delta_t); // Make sure the net quaternion is a pure rotation quaternion SEq_new.normalize(); // Save the new quaternion back into the orientation state orientation_state->quaternion = SEq_new; }
// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper: // "An efficient orientation filter for inertial and inertial/magnetic sensor arrays" // https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf static void _psmove_orientation_fusion_madgwick_marg_update( PSMoveOrientation *orientation_state, float delta_t, const glm::vec3 ¤t_omega, const glm::vec3 ¤t_g, const glm::vec3 ¤t_m) { // If there isn't a valid magnetometer or accelerometer vector, fall back to the IMU style update if (is_nearly_equal(glm::dot(current_g, current_g), 0.f, k_normal_epsilon*k_normal_epsilon) || is_nearly_equal(glm::dot(current_m, current_m), 0.f, k_normal_epsilon*k_normal_epsilon)) { _psmove_orientation_fusion_imu_update( orientation_state, delta_t, current_omega, current_g); return; } PSMoveMadgwickMARGState *marg_state = &orientation_state->fusion_state.madgwick_marg_state; // Current orientation from earth frame to sensor frame glm::quat SEq = orientation_state->quaternion; // Get the direction of the magnetic fields in the identity pose. // NOTE: In the original paper we converge on this vector over time automatically (See Eqn 45 & 46) // but since we've already done the work in calibration to get this vector, let's just use it. // This also removes the last assumption in this function about what // the orientation of the identity-pose is (handled by the sensor transform). 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 fields 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); // Eqn 15) Applied to the gravity and magnetometer vectors // Fill in the 6x1 objective function matrix f(SEq, Sa, Eb, Sm) =|f_g| // |f_b| glm::vec3 f_g; psmove_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL); glm::vec3 f_m; psmove_alignment_compute_objective_vector(SEq, k_identity_m_direction, current_m, f_m, NULL); // Eqn 21) Applied to the gravity and magnetometer vectors // Fill in the 4x6 objective function Jacobian matrix: J_gb(SEq, Eb)= [J_g|J_b] glm::mat3x4 J_g; psmove_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g); glm::mat3x4 J_m; psmove_alignment_compute_objective_jacobian(SEq, k_identity_m_direction, J_m); // Eqn 34) gradient_F= J_gb(SEq, Eb)*f(SEq, Sa, Eb, Sm) // Compute the gradient of the objective function glm::vec4 gradient_f = J_g*f_g + J_m*f_m; glm::quat SEqHatDot = glm::quat(gradient_f[0], gradient_f[1], gradient_f[2], gradient_f[3]); // normalize the gradient to estimate direction of the gyroscope error psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero); // Eqn 47) omega_err= 2*SEq*SEqHatDot // compute angular estimated direction of the gyroscope error glm::quat omega_err = (SEq*2.f) * SEqHatDot; // Eqn 48) net_omega_bias+= zeta*omega_err // Compute the net accumulated gyroscope bias glm::quat omega_bias= marg_state->omega_bias; omega_bias = omega_bias + omega_err*zeta*delta_t; omega_bias.w = 0.f; // no bias should accumulate on the w-component marg_state->omega_bias= omega_bias; // Eqn 49) omega_corrected = omega - net_omega_bias glm::quat omega = glm::quat(0.f, current_omega.x, current_omega.y, current_omega.z); glm::quat corrected_omega = omega + (-omega_bias); // Compute the rate of change of the orientation purely from the gyroscope // Eqn 12) q_dot = 0.5*q*omega glm::quat SEqDot_omega = (SEq * 0.5f) * corrected_omega; // Compute the estimated quaternion rate of change // Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot glm::quat SEqDot_est = SEqDot_omega + SEqHatDot*(-beta); // Compute then integrate the estimated quaternion rate // Eqn 42) SEq_new = SEq + SEqDot_est*delta_t glm::quat SEq_new = SEq + SEqDot_est*delta_t; // Make sure the net quaternion is a pure rotation quaternion SEq_new= glm::normalize(SEq_new); // Save the new quaternion back into the orientation state orientation_state->quaternion= SEq_new; }