Example #1
0
// run a 9-state EKF used to calculate orientation
void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
{
    imuSampleTime_ms = hal.scheduler->millis();
    dtIMU = delta_time;

    // initialise variables and constants
    if (!FiltInit) {
        StartTime_ms =  imuSampleTime_ms;
        newDataMag = false;
        YawAligned = false;
        state.quat[0] = 1.0f;
        const float Sigma_velNED = 0.5f; // 1 sigma uncertainty in horizontal velocity components
        const float Sigma_dAngBias  = 0.01745f*dtIMU; // 1 Sigma uncertainty in delta angle bias
        const float Sigma_angErr = 1.0f; // 1 Sigma uncertainty in angular misalignment (rad)
        for (uint8_t i=0; i <= 2; i++) Cov[i][i] = sq(Sigma_angErr);
        for (uint8_t i=3; i <= 5; i++) Cov[i][i] = sq(Sigma_velNED);
        for (uint8_t i=6; i <= 8; i++) Cov[i][i] = sq(Sigma_dAngBias);
        FiltInit = true;
        hal.console->printf("SmallEKF Alignment Started\n");
    }

    // We are using the IMU data from the flight vehicle and setting joint angles to zero for the time being
    gSense.gPsi = joint_angles.z; // yaw
    gSense.gPhi = joint_angles.x; // roll
    gSense.gTheta = joint_angles.y; // pitch
    cosPhi = cosf(gSense.gPhi);
    cosTheta = cosf(gSense.gTheta);
    sinPhi = sinf(gSense.gPhi);
    sinTheta = sinf(gSense.gTheta);
    sinPsi = sinf(gSense.gPsi);
    cosPsi = cosf(gSense.gPsi);
    gSense.delAng = delta_angles;
    gSense.delVel = delta_velocity;

    // predict states
    predictStates();

    // predict the covariance
    predictCovariance();

    // fuse SmallEKF velocity data
    fuseVelocity(YawAligned);

    // Align the heading once there has been enough time for the filter to settle and the tilt corrections have dropped below a threshold
    if ((((imuSampleTime_ms - StartTime_ms) > 5000 && TiltCorrection < 1e-4f) || ((imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) {
        //calculate the initial heading using magnetometer, estimated tilt and declination
        alignHeading();
        YawAligned = true;
        hal.console->printf("SmallEKF Alignment Completed\n");
    }

    // Fuse magnetometer data if  we have new measurements and an aligned heading
    readMagData();
    if (newDataMag && YawAligned) {
        fuseCompass();
        newDataMag = false;
    }

}
Example #2
0
// run a 9-state EKF used to calculate orientation
void SoloGimbalEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
{
    imuSampleTime_ms = AP_HAL::millis();
    dtIMU = delta_time;

    // initialise variables and constants
    if (!FiltInit) {
        // Note: the start time is initialised to 0 in the constructor
        if (StartTime_ms == 0) {
            StartTime_ms = imuSampleTime_ms;
        }

        // Set data to pre-initialsation defaults
        FiltInit = false;
        newDataMag = false;
        YawAligned = false;
        memset(&state, 0, sizeof(state));
        state.quat[0] = 1.0f;

        bool main_ekf_healthy = false;
        nav_filter_status main_ekf_status;

        if (_ahrs.get_filter_status(main_ekf_status)) {
            if (main_ekf_status.flags.attitude) {
                main_ekf_healthy = true;
            }
        }

        // Wait for gimbal to stabilise to body fixed position for a few seconds before starting small EKF
        // Also wait for navigation EKF to be healthy beasue we are using the velocity output data
        // This prevents jerky gimbal motion from degrading the EKF initial state estimates
        if (imuSampleTime_ms - StartTime_ms < 5000 || !main_ekf_healthy) {
            return;
        }

        Quaternion ned_to_vehicle_quat;
        ned_to_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());

        Quaternion vehicle_to_gimbal_quat;
        vehicle_to_gimbal_quat.from_vector312(joint_angles.x,joint_angles.y,joint_angles.z);

        // calculate initial orientation
        state.quat = ned_to_vehicle_quat * vehicle_to_gimbal_quat;

        const float Sigma_velNED = 0.5f; // 1 sigma uncertainty in horizontal velocity components
        const float Sigma_dAngBias = 0.002f*dtIMU; // 1 Sigma uncertainty in delta angle bias (rad)
        const float Sigma_angErr = 0.1f; // 1 Sigma uncertainty in angular misalignment (rad)
        for (uint8_t i=0; i <= 2; i++) Cov[i][i] = sq(Sigma_angErr);
        for (uint8_t i=3; i <= 5; i++) Cov[i][i] = sq(Sigma_velNED);
        for (uint8_t i=6; i <= 8; i++) Cov[i][i] = sq(Sigma_dAngBias);
        FiltInit = true;
        hal.console->printf("\nSoloGimbalEKF Alignment Started\n");

        // Don't run the filter in this timestep becasue we have already used the delta velocity data to set an initial orientation
        return;
    }

    // We are using IMU data and joint angles from the gimbal
    gSense.gPsi = joint_angles.z; // yaw
    gSense.gPhi = joint_angles.x; // roll
    gSense.gTheta = joint_angles.y; // pitch
    cosPhi = cosf(gSense.gPhi);
    cosTheta = cosf(gSense.gTheta);
    sinPhi = sinf(gSense.gPhi);
    sinTheta = sinf(gSense.gTheta);
    sinPsi = sinf(gSense.gPsi);
    cosPsi = cosf(gSense.gPsi);
    gSense.delAng = delta_angles;
    gSense.delVel = delta_velocity;

    // predict states
    predictStates();

    // predict the covariance
    predictCovariance();

    // fuse SoloGimbalEKF velocity data
    fuseVelocity();

    
    // Align the heading once there has been enough time for the filter to settle and the tilt corrections have dropped below a threshold
    // Force it to align if too much time has lapsed
    if (((((imuSampleTime_ms - StartTime_ms) > 8000 && TiltCorrection < 1e-4f) || (imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) {
        //calculate the initial heading using magnetometer, estimated tilt and declination
        alignHeading();
        YawAligned = true;
        hal.console->printf("\nSoloGimbalEKF Alignment Completed\n");
    }

    // Fuse magnetometer data if  we have new measurements and an aligned heading
    
    readMagData();
    if (newDataMag && YawAligned) {
        fuseCompass();
        newDataMag = false;
    }
    
}