terrain_estimator.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name ECL nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file terrain_estimator.cpp
 * Function for fusing rangefinder measurements to estimate terrain vertical position/
 *
 * @author Paul Riseborough <p_riseborough@live.com.au>
 *
 */

#include "ekf.h"
#include <ecl.h>
#include <mathlib/mathlib.h>

bool Ekf::initHagl()
{
    bool initialized = false;
    // get most recent range measurement from buffer
    const rangeSample &latest_measurement = _range_buffer.get_newest();

    if (!_control_status.flags.in_air) {
        // if on ground, do not trust the range sensor, but assume a ground clearance
        _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
        // use the ground clearance value as our uncertainty
        _terrain_var = sq(_params.rng_gnd_clearance);
        initialized = true;

    } else if (_rng_hgt_valid
           && (_time_last_imu - latest_measurement.time_us) < (uint64_t)2e5
           && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
        // if we have a fresh measurement, use it to initialise the terrain estimator
        _terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2;
        // initialise state variance to variance of measurement
        _terrain_var = sq(_params.range_noise);
        // success
        initialized = true;

    } else if (_flow_for_terrain_data_ready) {
        // initialise terrain vertical position to origin as this is the best guess we have
        _terrain_vpos = fmaxf(0.0f,  _state.pos(2));
        _terrain_var = 100.0f;
        initialized = true;

    } else {
        // no information - cannot initialise
        initialized = false;
    }

    if (initialized) {
        // has initialized with valid data
        _time_last_hagl_fuse = _time_last_imu;
    }

    return initialized;
}

void Ekf::runTerrainEstimator()
{
    // Perform initialisation check and
    // on ground, continuously reset the terrain estimator
    if (!_terrain_initialised || !_control_status.flags.in_air) {
        _terrain_initialised = initHagl();

    } else {

        // predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle

        // process noise due to errors in vehicle height estimate
        _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);

        // process noise due to terrain gradient
        _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient)
                * (sq(_state.vel(0)) + sq(_state.vel(1)));

        // limit the variance to prevent it becoming badly conditioned
        _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);

        // Fuse range finder data if available
        if (_range_data_ready && _rng_hgt_valid) {
            fuseHagl();

            // update range sensor angle parameters in case they have changed
            // we do this here to avoid doing those calculations at a high rate
            _sin_tilt_rng = sinf(_params.rng_sens_pitch);
            _cos_tilt_rng = cosf(_params.rng_sens_pitch);
        }

        if (_flow_for_terrain_data_ready) {
            fuseFlowForTerrain();
            _flow_for_terrain_data_ready = false;
        }

        // constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
        if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
            _terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
        }
    }

    updateTerrainValidity();
}

void Ekf::fuseHagl()
{
    // If the vehicle is excessively tilted, do not try to fuse range finder observations
    if (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
        // get a height above ground measurement from the range finder assuming a flat earth
        float meas_hagl = _range_sample_delayed.rng * _R_rng_to_earth_2_2;

        // predict the hagl from the vehicle position and terrain height
        float pred_hagl = _terrain_vpos - _state.pos(2);

        // calculate the innovation
        _hagl_innov = pred_hagl - meas_hagl;

        // calculate the observation variance adding the variance of the vehicles own height uncertainty
        float obs_variance = fmaxf(P[9][9] * _params.vehicle_variance_scaler, 0.0f)
                     + sq(_params.range_noise)
                     + sq(_params.range_noise_scaler * _range_sample_delayed.rng);

        // calculate the innovation variance - limiting it to prevent a badly conditioned fusion
        _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);

        // perform an innovation consistency check and only fuse data if it passes
        float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
        _terr_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);

        if (_terr_test_ratio <= 1.0f) {
            // calculate the Kalman gain
            float gain = _terrain_var / _hagl_innov_var;
            // correct the state
            _terrain_vpos -= gain * _hagl_innov;
            // correct the variance
            _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
            // record last successful fusion event
            _time_last_hagl_fuse = _time_last_imu;
            _innov_check_fail_status.flags.reject_hagl = false;

        } else {
            // If we have been rejecting range data for too long, reset to measurement
            if ((_time_last_imu - _time_last_hagl_fuse) > (uint64_t)10E6) {
                _terrain_vpos = _state.pos(2) + meas_hagl;
                _terrain_var = obs_variance;

            } else {
                _innov_check_fail_status.flags.reject_hagl = true;
            }
        }

    } else {
        _innov_check_fail_status.flags.reject_hagl = true;
    }
}

void Ekf::fuseFlowForTerrain()
{
    // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
    // correct for gyro bias errors in the data used to do the motion compensation
    // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
    Vector2f opt_flow_rate;
    opt_flow_rate(0) = _flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0);
    opt_flow_rate(1) = _flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1);

    // get latest estimated orientation
    float q0 = _state.quat_nominal(0);
    float q1 = _state.quat_nominal(1);
    float q2 = _state.quat_nominal(2);
    float q3 = _state.quat_nominal(3);

    // calculate the optical flow observation variance
    float R_LOS = calcOptFlowMeasVar();

    // get rotation matrix from earth to body
    Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);

    // calculate the sensor position relative to the IMU
    Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;

    // calculate the velocity of the sensor relative to the imu in body frame
    // Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign
    Vector3f vel_rel_imu_body = cross_product(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt, pos_offset_body);

    // calculate the velocity of the sensor in the earth frame
    Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;

    // rotate into body frame
    Vector3f vel_body = earth_to_body * vel_rel_earth;

    float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

    // constrain terrain to minimum allowed value and predict height above ground
    _terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
    float pred_hagl = _terrain_vpos - _state.pos(2);

    // Calculate observation matrix for flow around the vehicle x axis
    float Hx = vel_body(1) * t0 / (pred_hagl * pred_hagl);

    // Constrain terrain variance to be non-negative
    _terrain_var = fmaxf(_terrain_var, 0.0f);

    // Cacluate innovation variance
    _flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS;

    // calculate the kalman gain for the flow x measurement
    float Kx = _terrain_var * Hx / _flow_innov_var[0];

    // calculate prediced optical flow about x axis
    float pred_flow_x = vel_body(1) * earth_to_body(2, 2) / pred_hagl;

    // calculate flow innovation (x axis)
    _flow_innov[0] = pred_flow_x - opt_flow_rate(0);

    // calculate correction term for terrain variance
    float KxHxP =  Kx * Hx * _terrain_var;

    // innovation consistency check
    float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
    float flow_test_ratio = sq(_flow_innov[0]) / (sq(gate_size) * _flow_innov_var[0]);

    // do not perform measurement update if badly conditioned
    if (flow_test_ratio <= 1.0f) {
        _terrain_vpos += Kx * _flow_innov[0];
        // guard against negative variance
        _terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
        _time_last_of_fuse = _time_last_imu;
    }

    // Calculate observation matrix for flow around the vehicle y axis
    float Hy = -vel_body(0) * t0 / (pred_hagl * pred_hagl);

    // Calculuate innovation variance
    _flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS;

    // calculate the kalman gain for the flow y measurement
    float Ky = _terrain_var * Hy / _flow_innov_var[1];

    // calculate prediced optical flow about y axis
    float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) / pred_hagl;

    // calculate flow innovation (y axis)
    _flow_innov[1] = pred_flow_y - opt_flow_rate(1);

    // calculate correction term for terrain variance
    float KyHyP =  Ky * Hy * _terrain_var;

    // innovation consistency check
    flow_test_ratio = sq(_flow_innov[1]) / (sq(gate_size) * _flow_innov_var[1]);

    if (flow_test_ratio <= 1.0f) {
        _terrain_vpos += Ky * _flow_innov[1];
        // guard against negative variance
        _terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
        _time_last_of_fuse = _time_last_imu;
    }
}

bool Ekf::isTerrainEstimateValid() const
{
    return _hagl_valid;
}

void Ekf::updateTerrainValidity()
{
    // we have been fusing range finder measurements in the last 5 seconds
    bool recent_range_fusion = (_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e6;

    // we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
    // this can only be the case if the main filter does not fuse optical flow
    bool recent_flow_for_terrain_fusion = ((_time_last_imu - _time_last_of_fuse) < (uint64_t)5e6)
                          && !_control_status.flags.opt_flow;

    _hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));
}

// get the estimated vertical position of the terrain relative to the NED origin
void Ekf::getTerrainVertPos(float *ret)
{
    memcpy(ret, &_terrain_vpos, sizeof(float));
}

void Ekf::getHaglInnov(float *hagl_innov)
{
    memcpy(hagl_innov, &_hagl_innov, sizeof(_hagl_innov));
}


void Ekf::getHaglInnovVar(float *hagl_innov_var)
{
    memcpy(hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var));
}