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; } }
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::Quaternionf first_derivative_backup = m_FusionState->orientation_first_derivative; const Eigen::Quaternionf second_derivative_backup = m_FusionState->orientation_second_derivative; switch(m_FusionState->fusion_type) { case FusionTypeNone: break; case FusionTypePassThru: { const Eigen::Quaternionf &new_orientation= filterPacket.orientation; const Eigen::Quaternionf new_first_derivative= Eigen::Quaternionf( (new_orientation.coeffs() - m_FusionState->orientation.coeffs()) / delta_time); const Eigen::Quaternionf new_second_derivative = Eigen::Quaternionf( (new_first_derivative.coeffs() - m_FusionState->orientation_first_derivative.coeffs()) / delta_time); m_FusionState->orientation = new_orientation; m_FusionState->orientation_first_derivative = new_first_derivative; m_FusionState->orientation_second_derivative = new_second_derivative; } 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_quaternion_is_valid(m_FusionState->orientation_first_derivative)) { SERVER_LOG_WARNING("OrientationFilter") << "Orientation first derivative is NaN!"; m_FusionState->orientation_first_derivative = first_derivative_backup; } if (!eigen_quaternion_is_valid(m_FusionState->orientation_second_derivative)) { SERVER_LOG_WARNING("OrientationFilter") << "Orientation second derivative is NaN!"; m_FusionState->orientation_second_derivative = second_derivative_backup; } }