C++ (Cpp) orientation_fusion_madgwick_marg_update Examples

Example #1

File: OrientationFilter.cpp Project: Figuration/PSMoveService

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

Example #2

File: OrientationFilter.cpp Project: hani910/PSMoveService

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