void OrientationFilterSpace::convertSensorPacketToFilterPacket( const OrientationSensorPacket &sensorPacket, OrientationFilterPacket &outFilterPacket) const { outFilterPacket.orientation = sensorPacket.orientation; outFilterPacket.gyroscope= m_SensorTransform * sensorPacket.gyroscope; outFilterPacket.normalized_accelerometer= m_SensorTransform * sensorPacket.accelerometer; outFilterPacket.normalized_magnetometer= m_SensorTransform * sensorPacket.magnetometer; eigen_vector3f_normalize_with_default(outFilterPacket.normalized_accelerometer, Eigen::Vector3f()); eigen_vector3f_normalize_with_default(outFilterPacket.normalized_magnetometer, Eigen::Vector3f()); }
void AppStage_MagnetometerCalibration::update() { bool bControllerDataUpdatedThisFrame= false; if (m_isControllerStreamActive && m_controllerView->GetOutputSequenceNum() != m_lastControllerSeqNum) { const PSMoveRawSensorData &sensorData= m_controllerView->GetPSMoveView().GetRawSensorData(); m_lastMagnetometer= sensorData.Magnetometer; m_lastAccelerometer= sensorData.Accelerometer; m_lastControllerSeqNum = m_controllerView->GetOutputSequenceNum(); bControllerDataUpdatedThisFrame= true; } switch (m_menuState) { case eCalibrationMenuState::waitingForStreamStartResponse: { if (bControllerDataUpdatedThisFrame) { if (m_controllerView->GetPSMoveView().GetHasValidHardwareCalibration()) { m_sampleCount = 0; m_samplePercentage = 0; m_minSampleExtent= *k_psmove_int_vector3_zero; m_maxSampleExtent= *k_psmove_int_vector3_zero; m_led_color_r= 255; m_led_color_g= 0; m_led_color_b= 0; if (m_bBypassCalibration) { m_app->getOrbitCamera()->resetOrientation(); m_menuState= AppStage_MagnetometerCalibration::complete; } else { m_menuState= AppStage_MagnetometerCalibration::measureBExtents; } } else { m_menuState= AppStage_MagnetometerCalibration::failedBadCalibration; } } } break; case eCalibrationMenuState::failedStreamStart: case eCalibrationMenuState::failedBadCalibration: { } break; case eCalibrationMenuState::measureBExtents: { if (bControllerDataUpdatedThisFrame && m_sampleCount < k_max_magnetometer_samples) { // Grow the measurement extents bounding box expandMagnetometerBounds(m_lastMagnetometer, m_minSampleExtent, m_maxSampleExtent); // Make sure this sample isn't too close to another sample bool bTooClose= false; for (int sampleIndex= m_sampleCount-1; sampleIndex >= 0; --sampleIndex) { const PSMoveIntVector3 diff= m_lastMagnetometer - m_magnetometerIntSamples[sampleIndex]; const int distanceSquared= diff.lengthSquared(); if (distanceSquared < k_min_sample_distance_sq) { bTooClose= true; break; } } // Display the last N samples if (!bTooClose) { // Store the new sample m_magnetometerIntSamples[m_sampleCount]= m_lastMagnetometer; m_alignedSamples->magnetometerEigenSamples[m_sampleCount] = psmove_int_vector3_to_eigen_vector3(m_lastMagnetometer); ++m_sampleCount; // Compute a best fit ellipsoid for the sample points switch (m_ellipseFitMethod) { case _ellipse_fit_method_box: eigen_alignment_fit_bounding_box_ellipsoid( m_alignedSamples->magnetometerEigenSamples, m_sampleCount, m_sampleFitEllipsoid); break; case _ellipse_fit_method_min_volume: eigen_alignment_fit_min_volume_ellipsoid( m_alignedSamples->magnetometerEigenSamples, m_sampleCount, 0.0001f, m_sampleFitEllipsoid); break; } // Update the extents progress based on min extent size int minRange = computeMagnetometerCalibrationMinRange(m_minSampleExtent, m_maxSampleExtent); if (minRange > 0) { m_samplePercentage = std::min( std::min((100 * m_sampleCount) / k_sample_count_target, 100), std::min((100 * minRange) / k_sample_range_target, 100)); int led_color_r = (255 * (100 - m_samplePercentage)) / 100; int led_color_g = (255 * m_samplePercentage) / 100; int led_color_b = 0; // Send request to change led color, don't care about callback if (led_color_r != m_led_color_r || led_color_g != m_led_color_g || led_color_b != m_led_color_b) { m_led_color_r = led_color_r; m_led_color_g = led_color_g; m_led_color_b = led_color_b; m_controllerView->GetPSMoveViewMutable().SetLEDOverride(m_led_color_r, m_led_color_g, m_led_color_b); } } } } } break; case eCalibrationMenuState::waitForGravityAlignment: { if (m_controllerView->GetPSMoveView().GetIsStableAndAlignedWithGravity()) { std::chrono::time_point<std::chrono::high_resolution_clock> now = std::chrono::high_resolution_clock::now(); if (m_bIsStable) { std::chrono::duration<double, std::milli> stableDuration = now - m_stableStartTime; if (stableDuration.count() >= k_stabilize_wait_time_ms) { m_identityPoseMVectorSum= *k_psmove_int_vector3_zero; m_identityPoseSampleCount = 0; m_menuState= AppStage_MagnetometerCalibration::measureBDirection; } } else { m_bIsStable= true; m_stableStartTime= now; } } else { if (m_bIsStable) { m_bIsStable= false; } } } break; case eCalibrationMenuState::measureBDirection: { if (m_controllerView->GetPSMoveView().GetIsStableAndAlignedWithGravity()) { if (bControllerDataUpdatedThisFrame) { m_identityPoseMVectorSum = m_identityPoseMVectorSum + m_lastMagnetometer; ++m_identityPoseSampleCount; if (m_identityPoseSampleCount > k_desired_magnetometer_sample_count) { float N= static_cast<float>(m_identityPoseSampleCount); // The average magnetometer direction was recorded while the controller // was in the cradle pose PSMoveFloatVector3 identityPoseAvg= m_identityPoseMVectorSum.castToFloatVector3().unsafe_divide(N); // Project the averaged magnetometer sample into the space of the ellipse // And then normalize it (any deviation from unit length is error) Eigen::Vector3f projected_sample = eigen_alignment_project_point_on_ellipsoid_basis( psmove_float_vector3_to_eigen_vector3(identityPoseAvg), m_sampleFitEllipsoid); eigen_vector3f_normalize_with_default(projected_sample, Eigen::Vector3f(0.f, 1.f, 0.f)); // Tell the psmove service about the new magnetometer settings { RequestPtr request(new PSMoveProtocol::Request()); request->set_type(PSMoveProtocol::Request_RequestType_SET_MAGNETOMETER_CALIBRATION); PSMoveProtocol::Request_RequestSetMagnetometerCalibration *calibration= request->mutable_set_magnetometer_calibration_request(); calibration->set_controller_id(m_controllerView->GetControllerID()); write_calibration_parameter(m_sampleFitEllipsoid.center, calibration->mutable_ellipse_center()); write_calibration_parameter(m_sampleFitEllipsoid.extents, calibration->mutable_ellipse_extents()); write_calibration_parameter(m_sampleFitEllipsoid.basis.col(0), calibration->mutable_ellipse_basis_x()); write_calibration_parameter(m_sampleFitEllipsoid.basis.col(1), calibration->mutable_ellipse_basis_y()); write_calibration_parameter(m_sampleFitEllipsoid.basis.col(2), calibration->mutable_ellipse_basis_z()); calibration->set_ellipse_fit_error(m_sampleFitEllipsoid.error); write_calibration_parameter(projected_sample, calibration->mutable_magnetometer_identity()); ClientPSMoveAPI::register_callback( ClientPSMoveAPI::send_opaque_request(&request), AppStage_MagnetometerCalibration::handle_set_magnetometer_calibration, this); } // Wait for the response m_menuState= AppStage_MagnetometerCalibration::waitForSetCalibrationResponse; } } } else { m_bIsStable= false; m_menuState= AppStage_MagnetometerCalibration::waitForGravityAlignment; } } break; case eCalibrationMenuState::waitForSetCalibrationResponse: { } break; case eCalibrationMenuState::failedSetCalibration: { } break; case eCalibrationMenuState::complete: { } break; case eCalibrationMenuState::pendingExit: { } break; default: assert(0 && "unreachable"); } }
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); }
// -- 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); } }
void OrientationFilterMadgwickMARG::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()); // 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)) { OrientationFilterMadgwickARG::update(delta_time, packet); return; } // Current orientation from earth frame to sensor frame const Eigen::Quaternionf SEq = m_state->orientation; // 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). 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; // 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; eigen_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL); Eigen::Matrix<float, 3, 1> f_m; eigen_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; eigen_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g); Eigen::Matrix<float, 4, 3> J_m; eigen_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 eigen_quaternion_normalize_with_default(SEqHatDot, *k_eigen_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 const float zeta= sqrtf(3.0f / 4.0f) * fmaxf(fmaxf(m_constants.gyro_variance.x(), m_constants.gyro_variance.y()), m_constants.gyro_variance.z()); Eigen::Quaternionf omega_bias(0.f, m_omega_bias_x, m_omega_bias_y, m_omega_bias_z); omega_bias = Eigen::Quaternionf(omega_bias.coeffs() + omega_err.coeffs()*zeta*delta_time); m_omega_bias_x= omega_bias.x(); m_omega_bias_y= omega_bias.y(); m_omega_bias_z= omega_bias.z(); // 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() - 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 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 Eigen::Quaternionf SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_est.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 { const Eigen::Quaternionf &new_orientation = SEq_new; const Eigen::Vector3f new_angular_velocity(corrected_omega.x(), corrected_omega.y(), corrected_omega.z()); const Eigen::Vector3f new_angular_acceleration = (new_angular_velocity - m_state->angular_velocity) / delta_time; m_state->apply_state(new_orientation, new_angular_velocity, new_angular_acceleration); } }
// -- OrientationFilterMadgwickARG -- // 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 void OrientationFilterMadgwickARG::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()); // 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 { const Eigen::Quaternionf &new_orientation = SEq_new; 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); } }