// 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; }
bool psmove_alignment_quaternion_between_vector_frames( const Eigen::Vector3f* from[2], const Eigen::Vector3f* to[2], const float tolerance, const Eigen::Quaternionf &initial_q, Eigen::Quaternionf &out_q) { bool success = true; Eigen::Quaternionf previous_q = initial_q; Eigen::Quaternionf q = initial_q; //const float tolerance_squared = tolerance*tolerance; //TODO: This variable is unused, but it should be. Need to re-test with this added since the below test should be: error_squared > tolerance_squared const int k_max_iterations = 32; float previous_error_squared = k_real_max; float error_squared = k_real_max; float squared_error_delta = k_real_max; float gamma = 0.5f; bool backtracked = false; for (int iteration = 0; iteration < k_max_iterations && // Haven't exceeded iteration limit error_squared > tolerance && // Aren't within tolerance of the destination squared_error_delta > k_normal_epsilon && // Haven't reached a minima gamma > k_normal_epsilon; // Haven't reduced our step size to zero iteration++) { // Fill in the 6x1 objective function matrix |f_0| // |f_1| float error_squared0, error_squared1; Eigen::Matrix<float, 3, 1> f_0; psmove_alignment_compute_objective_vector(q, *from[0], *to[0], f_0, &error_squared0); Eigen::Matrix<float, 3, 1> f_1; psmove_alignment_compute_objective_vector(q, *from[1], *to[1], f_1, &error_squared1); Eigen::Matrix<float, 6, 1> f; f.block<3, 1>(0, 0) = f_0; f.block<3, 1>(3, 0) = f_1; error_squared = error_squared0 + error_squared1; // Make sure this new step hasn't made the error worse if (error_squared <= previous_error_squared) { // We won't have a valid error derivative if we had to back track squared_error_delta = !backtracked ? fabsf(error_squared - previous_error_squared) : squared_error_delta; backtracked = false; // This is a good step. // Remember it in case the next one makes things worse previous_error_squared = error_squared; previous_q = q; // Fill in the 4x6 objective function Jacobian matrix: [J_0|J_1] Eigen::Matrix<float, 4, 3> J_0; psmove_alignment_compute_objective_jacobian(q, *from[0], J_0); Eigen::Matrix<float, 4, 3> J_1; psmove_alignment_compute_objective_jacobian(q, *from[1], J_1); Eigen::Matrix<float, 4, 6> J; J.block<4, 3>(0, 0) = J_0; J.block<4, 3>(0, 3) = J_1; // Compute the gradient of the objective function Eigen::Matrix<float, 4, 1> gradient_f = J*f; Eigen::Quaternionf gradient_q = Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0)); // The gradient points toward the maximum, so we subtract it off to head toward the minimum. // The step scale 'gamma' is just a guess. q = Eigen::Quaternionf(q.coeffs() - gradient_q.coeffs()*gamma); //q-= gradient_q*gamma; q.normalize(); } else { // The step made the error worse. // Return to the previous orientation and half our step size. q = previous_q; gamma /= 2.f; backtracked = true; } } if (error_squared > tolerance) { // Make sure we didn't fail to converge on the goal success = false; } out_q= q; return success; }
bool psmove_alignment_quaternion_between_vector_frames( const glm::vec3* from[2], const glm::vec3* to[2], const float tolerance, const glm::quat &initial_q, glm::quat &out_q) { bool success = true; glm::quat previous_q = initial_q; glm::quat q = initial_q; //const float tolerance_squared = tolerance*tolerance; //TODO: This variable is unused, but it should be. Need to re-test with this added since the below test should be: error_squared > tolerance_squared const int k_max_iterations = 32; float previous_error_squared = k_real_max; float error_squared = k_real_max; float squared_error_delta = k_real_max; float gamma = 0.5f; bool backtracked = false; for (int iteration = 0; iteration < k_max_iterations && // Haven't exceeded iteration limit error_squared > tolerance && // Aren't within tolerance of the destination squared_error_delta > k_normal_epsilon && // Haven't reached a minima gamma > k_normal_epsilon; // Haven't reduced our step size to zero iteration++) { // Compute the two 3x1 halves of the 6x1 objective function matrix |f_0| // |f_1| float error_squared0, error_squared1; glm::vec3 f_0; psmove_alignment_compute_objective_vector(q, *from[0], *to[0], f_0, &error_squared0); glm::vec3 f_1; psmove_alignment_compute_objective_vector(q, *from[1], *to[1], f_1, &error_squared1); error_squared = error_squared0 + error_squared1; // Make sure this new step hasn't made the error worse if (error_squared <= previous_error_squared) { // We won't have a valid error derivative if we had to back track squared_error_delta = !backtracked ? fabsf(error_squared - previous_error_squared) : squared_error_delta; backtracked = false; // This is a good step. // Remember it in case the next one makes things worse previous_error_squared = error_squared; previous_q = q; // Compute the two 4x3 halves of the 4x6 objective function Jacobian matrix: [J_0|J_1] glm::mat3x4 J_0; psmove_alignment_compute_objective_jacobian(q, *from[0], J_0); glm::mat3x4 J_1; psmove_alignment_compute_objective_jacobian(q, *from[1], J_1); // Compute the gradient of the objective function glm::vec4 gradient_f = J_0*f_0 + J_1*f_1; glm::quat gradient_q(gradient_f[0], gradient_f[1], gradient_f[2], gradient_f[3]); // The gradient points toward the maximum, so we subtract it off to head toward the minimum. // The step scale 'gamma' is just a guess. q = q + gradient_q*(-gamma); //q-= gradient_q*gamma; q = glm::normalize(q); } else { // The step made the error worse. // Return to the previous orientation and half our step size. q = previous_q; gamma /= 2.f; backtracked = true; } } if (error_squared > tolerance) { // Make sure we didn't fail to converge on the goal success = false; } out_q= q; return success; }
// 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; }