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);
}
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);
}
