void apply_state(
const Eigen::Quaternionf &new_orientation,
const Eigen::Vector3f &new_angular_velocity,
const Eigen::Vector3f &new_angular_acceleration)
{
if (eigen_quaternion_is_valid(new_orientation))
{
orientation = new_orientation;
}
else
{
SERVER_LOG_WARNING("OrientationFilter") << "Orientation is NaN!";
}
if (eigen_vector3f_is_valid(new_angular_velocity))
{
angular_velocity= new_angular_velocity;
}
else
{
SERVER_LOG_WARNING("OrientationFilter") << "Angular Velocity is NaN!";
}
if (eigen_vector3f_is_valid(new_angular_acceleration))
{
angular_acceleration= new_angular_acceleration;
}
else
{
SERVER_LOG_WARNING("OrientationFilter") << "Angular Acceleration is NaN!";
}
// state is valid now that we have had an update
bIsValid= true;
}
void OrientationFilter::update(
const float delta_time,
const OrientationSensorPacket &sensorPacket)
{
OrientationFilterPacket filterPacket;
m_FilterSpace.convertSensorPacketToFilterPacket(sensorPacket, filterPacket);
const Eigen::Quaternionf orientation_backup= m_FusionState->orientation;
const Eigen::Vector3f first_derivative_backup = m_FusionState->angular_velocity;
const Eigen::Vector3f second_derivative_backup = m_FusionState->angular_acceleration;
switch(m_FusionState->fusion_type)
{
case FusionTypeNone:
break;
case FusionTypePassThru:
{
const Eigen::Quaternionf &new_orientation= filterPacket.orientation;
const Eigen::Quaternionf orientation_derivative= Eigen::Quaternionf((new_orientation.coeffs() - m_FusionState->orientation.coeffs()) / delta_time);
const Eigen::Vector3f angular_velocity = quaternion_derivative_to_angular_velocity(new_orientation, orientation_derivative);
const Eigen::Vector3f angular_accelertion = (angular_velocity - m_FusionState->angular_velocity) / delta_time;
m_FusionState->orientation = new_orientation;
m_FusionState->angular_velocity = angular_velocity;
m_FusionState->angular_acceleration = angular_accelertion;
}
break;
case FusionTypeMadgwickIMU:
orientation_fusion_imu_update(delta_time, &m_FilterSpace, &filterPacket, m_FusionState);
break;
case FusionTypeMadgwickMARG:
orientation_fusion_madgwick_marg_update(delta_time, &m_FilterSpace, &filterPacket, m_FusionState);
break;
case FusionTypeComplementaryMARG:
orientation_fusion_complementary_marg_update(delta_time, &m_FilterSpace, &filterPacket, m_FusionState);
break;
}
if (!eigen_quaternion_is_valid(m_FusionState->orientation))
{
SERVER_LOG_WARNING("OrientationFilter") << "Orientation is NaN!";
m_FusionState->orientation = orientation_backup;
}
if (!eigen_vector3f_is_valid(m_FusionState->angular_velocity))
{
SERVER_LOG_WARNING("OrientationFilter") << "Angular Velocity is NaN!";
m_FusionState->angular_velocity = first_derivative_backup;
}
if (!eigen_vector3f_is_valid(m_FusionState->angular_acceleration))
{
SERVER_LOG_WARNING("OrientationFilter") << "Angular Acceleration is NaN!";
m_FusionState->angular_acceleration = second_derivative_backup;
}
}
