// 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
orientation_fusion_madgwick_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;
// 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))
{
orientation_fusion_imu_update(
delta_time,
filter_space,
filter_packet,
fusion_state);
return;
}
MadgwickMARGState *marg_state = &fusion_state->fusion_state.madgwick_marg_state;
// Current orientation from earth frame to sensor frame
const Eigen::Quaternionf SEq = fusion_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 = filter_space->getMagnetometerCalibrationDirection();
// Get the direction of the gravitational fields in the identity pose
Eigen::Vector3f k_identity_g_direction = filter_space->getGravityCalibrationDirection();
// 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
Eigen::Quaternionf omega_bias= marg_state->omega_bias;
omega_bias = Eigen::Quaternionf(omega_bias.coeffs() + omega_err.coeffs()*zeta*delta_time);
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
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
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::Quaternionf &new_first_derivative = SEqDot_est;
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;
}
}
// 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
orientation_fusion_imu_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;
// Current orientation from earth frame to sensor frame
const Eigen::Quaternionf SEq = fusion_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 = filter_space->getGravityCalibrationDirection();
// 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
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::Quaternionf &new_first_derivative = SEqDot_omega;
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;
}
}
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);
}
}
// -- 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);
}
}
// -- 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);
}
}
