void
psmove_orientation_update(PSMoveOrientation *orientation_state)
{
psmove_return_if_fail(orientation_state != NULL);
int frame_half;
long now = psmove_util_get_ticks();
if (now - orientation_state->sample_freq_measure_start >= 1000)
{
float measured = ((float)orientation_state->sample_freq_measure_count) /
((float)(now-orientation_state->sample_freq_measure_start))*1000.f;
psmove_DEBUG("Measured sample_freq: %f\n", measured);
orientation_state->sample_freq = measured;
orientation_state->sample_freq_measure_start = now;
orientation_state->sample_freq_measure_count = 0;
}
/* We get 2 measurements per call to psmove_poll() */
orientation_state->sample_freq_measure_count += 2;
Eigen::Quaternionf quaternion_backup = orientation_state->quaternion;
float deltaT = 1.f / fmax(orientation_state->sample_freq, SAMPLE_FREQUENCY); // time delta = 1/frequency
for (frame_half=0; frame_half<2; frame_half++)
{
switch (orientation_state->fusion_type)
{
case OrientationFusion_None:
break;
case OrientationFusion_MadgwickIMU:
{
// Get the sensor data transformed by the sensor_transform
PSMove_3AxisVector a=
psmove_orientation_get_accelerometer_normalized_vector(orientation_state, (enum PSMove_Frame)(frame_half));
PSMove_3AxisVector omega=
psmove_orientation_get_gyroscope_vector(orientation_state, (enum PSMove_Frame)(frame_half));
// Apply the filter
_psmove_orientation_fusion_imu_update(
orientation_state,
deltaT,
/* Gyroscope */
Eigen::Vector3f(omega.x, omega.y, omega.z),
/* Accelerometer */
Eigen::Vector3f(a.x, a.y, a.z));
}
break;
case OrientationFusion_MadgwickMARG:
{
PSMove_3AxisVector m = psmove_orientation_get_magnetometer_normalized_vector(orientation_state);
PSMove_3AxisVector a = psmove_orientation_get_accelerometer_normalized_vector(orientation_state, (enum PSMove_Frame)(frame_half));
PSMove_3AxisVector omega = psmove_orientation_get_gyroscope_vector(orientation_state, (enum PSMove_Frame)(frame_half));
_psmove_orientation_fusion_madgwick_marg_update(orientation_state, deltaT,
/* Gyroscope */
Eigen::Vector3f(omega.x, omega.y, omega.z),
/* Accelerometer */
Eigen::Vector3f(a.x, a.y, a.z),
/* Magnetometer */
Eigen::Vector3f(m.x, m.y, m.z));
}
break;
case OrientationFusion_ComplementaryMARG:
{
PSMove_3AxisVector m=
psmove_orientation_get_magnetometer_normalized_vector(orientation_state);
PSMove_3AxisVector a=
psmove_orientation_get_accelerometer_normalized_vector(orientation_state, (enum PSMove_Frame)(frame_half));
PSMove_3AxisVector omega=
psmove_orientation_get_gyroscope_vector(orientation_state, (enum PSMove_Frame)(frame_half));
// Apply the filter
_psmove_orientation_fusion_complementary_marg_update(
orientation_state,
deltaT,
/* Gyroscope */
Eigen::Vector3f(omega.x, omega.y, omega.z),
/* Accelerometer */
Eigen::Vector3f(a.x, a.y, a.z),
/* Magnetometer */
Eigen::Vector3f(m.x, m.y, m.z));
}
break;
}
if (!psmove_quaternion_is_valid(orientation_state->quaternion))
{
psmove_DEBUG("Orientation is NaN!");
orientation_state->quaternion = quaternion_backup;
}
}
}
// 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;
}
