ranger fusion bug

[garlinplus]

when ranger is selected as primary height sensor.EKF get time aligned ranger data from ring buffer as followed: void Ekf::controlFusionModes() { // Store the status to enable change detection _control_status_prev.value = _control_status.value;

// monitor the tilt alignment
if (!_control_status.flags.tilt_align) {
    // whilst we are aligning the tilt, monitor the variances
    const Vector3f angle_err_var_vec = calcRotVecVariances();

    // Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
    // and declare the tilt alignment complete
    if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(math::radians(3.0f))) {
        _control_status.flags.tilt_align = true;
        _control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState()); // TODO: is this needed?

        // send alignment status message to the console
        const char* height_source = nullptr;
        if (_control_status.flags.baro_hgt) {
            height_source = "baro";

        } else if (_control_status.flags.ev_hgt) {
            height_source = "ev";

        } else if (_control_status.flags.gps_hgt) {
            height_source = "gps";

        } else if (_control_status.flags.rng_hgt) {
            height_source = "rng";

        } else {
            height_source = "unknown";

        }
        if(height_source){
            ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
                (unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
        }
    }
}

// check for intermittent data (before pop_first_older_than)
const baroSample &baro_init = _baro_buffer.get_newest();
_baro_hgt_faulty = !isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL);

const gpsSample &gps_init = _gps_buffer.get_newest();
_gps_hgt_intermittent = !isRecent(gps_init.time_us, 2 * GPS_MAX_INTERVAL);

// check for arrival of new sensor data at the fusion time horizon
_time_prev_gps_us = _gps_sample_delayed.time_us;
_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);

if (_mag_data_ready) {
    // if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
    // this is useful if there is a lot of interference on the sensor measurement.
    if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) &&_NED_origin_initialised) {
        const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
        _mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred);
        _control_status.flags.synthetic_mag_z = true;
    } else {
        _control_status.flags.synthetic_mag_z = false;
    }
}

_delta_time_baro_us = _baro_sample_delayed.time_us;
_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);

// if we have a new baro sample save the delta time between this sample and the last sample which is
// used below for baro offset calculations
if (_baro_data_ready) {
    _delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us;
}

**{
// Get range data from buffer and check validity
const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
_range_sensor.setDataReadiness(is_rng_data_ready);
_range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth);

// update range sensor angle parameters in case they have changed
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
}

if (_range_sensor.isDataHealthy()) {
    // correct the range data for position offset relative to the IMU
    Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
    Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
    _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
}**

// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
// This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow,
// in this case we need to empty the buffer
if (!_flow_data_ready || !_control_status.flags.opt_flow) {
    _flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
               && (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
}

// check if we should fuse flow data for terrain estimation
if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) {
    // only fuse flow for terrain if range data hasn't been fused for 5 seconds
    _flow_for_terrain_data_ready = isTimedOut(_time_last_hagl_fuse, (uint64_t)5E6);
    // only fuse flow for terrain if the main filter is not fusing flow and we are using gps
    _flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps);
}

_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
_tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);

// check for height sensor timeouts and reset and change sensor if necessary
controlHeightSensorTimeouts();

// control use of observations for aiding
controlMagFusion();
controlOpticalFlowFusion();
controlGpsFusion();
controlAirDataFusion();
controlBetaFusion();
controlDragFusion();
controlHeightFusion();

// Additional data odoemtery data from an external estimator can be fused.
controlExternalVisionFusion();

// Additional horizontal velocity data from an auxiliary sensor can be fused
controlAuxVelFusion();

// Fake position measurement for constraining drift when no other velocity or position measurements
controlFakePosFusion();

// check if we are no longer fusing measurements that directly constrain velocity drift
update_deadreckoning_status();

}

As above bold code,EKF run with 8ms,And my ranger sensor with 20hz.So there may be not sensor data. But in followed function: void Ekf::controlHeightFusion() { checkRangeAidSuitability(); const bool do_range_aid = (_params.range_aid == 1) && isRangeAidSuitable();

bool fuse_height = false;

switch (_params.vdist_sensor_type) {
default:
    ECL_ERR("Invalid hgt mode: %d", _params.vdist_sensor_type);

// FALLTHROUGH
case VDIST_SENSOR_BARO:
    if (do_range_aid && _range_sensor.isDataHealthy()) {
        setControlRangeHeight();
        fuse_height = true;

        // we have just switched to using range finder, calculate height sensor offset such that current
        // measurement matches our current height estimate
        if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
            _hgt_sensor_offset = _terrain_vpos;
        }

    } else if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) {
        startBaroHgtFusion();
        fuse_height = true;

    } else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) {
        // switch to gps if there was a reset to gps
        fuse_height = true;

        // we have just switched to using gps height, calculate height sensor offset such that current
        // measurement matches our current height estimate
        if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
            _hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
        }
    }

    break;

case VDIST_SENSOR_RANGE:
    if (_range_sensor.isDataHealthy()) {
        setControlRangeHeight();
        fuse_height = true;

        if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
            // we have just switched to using range finder, calculate height sensor offset such that current
            // measurement matches our current height estimate
            // use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
            if (_control_status.flags.in_air && isTerrainEstimateValid()) {
                _hgt_sensor_offset = _terrain_vpos;

            } else if (_control_status.flags.in_air) {
                _hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);

            } else {
                _hgt_sensor_offset = _params.rng_gnd_clearance;
            }
        }

    } else if (_baro_data_ready && !_baro_hgt_faulty) {
        startBaroHgtFusion();
        fuse_height = true;
    }

    break;

case VDIST_SENSOR_GPS:

    // NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
    // to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
    // Do switching between GPS and rangefinder if using range finder as a height source when close
    // to ground and moving slowly. Also handle switch back from emergency Baro sensor when GPS recovers.
    if (!_control_status_prev.flags.rng_hgt && do_range_aid && _range_sensor.isDataHealthy()) {
        setControlRangeHeight();

        // we have just switched to using range finder, calculate height sensor offset such that current
        // measurement matches our current height estimate
        _hgt_sensor_offset = _terrain_vpos;

    } else if (_control_status_prev.flags.rng_hgt && !do_range_aid) {
        // must stop using range finder so find another sensor now
        if (!_gps_hgt_intermittent && _gps_checks_passed) {
            // GPS quality OK
            startGpsHgtFusion();
        } else if (!_baro_hgt_faulty) {
            // Use baro as a fallback
            startBaroHgtFusion();
        }
    } else if (_control_status.flags.baro_hgt && !do_range_aid && !_gps_hgt_intermittent && _gps_checks_passed) {
        // In baro fallback mode and GPS has recovered so start using it
        startGpsHgtFusion();
    }
    if (_control_status.flags.gps_hgt && _gps_data_ready) {
        fuse_height = true;
    } else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
        fuse_height = true;
    } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
        fuse_height = true;
    }
    break;

case VDIST_SENSOR_EV:

    // don't start using EV data unless data is arriving frequently
    if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
        fuse_height = true;
        setControlEVHeight();
        resetHeight();
    }

    if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
        // switch to baro if there was a reset to baro
        fuse_height = true;
    }

    // determine if we should use the vertical position observation
    if (_control_status.flags.ev_hgt) {
        fuse_height = true;
    }

    break;
}

updateBaroHgtOffset();

if (isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL) && _control_status.flags.rng_hgt
    && (!_range_sensor.isDataHealthy())) {

    // If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
    // and are on the ground, then synthesise a measurement at the expected on ground value
    if (!_control_status.flags.in_air) {
        _range_sensor.setRange(_params.rng_gnd_clearance);
        _range_sensor.setDataReadiness(true);
        _range_sensor.setValidity(true); // bypass the checks
    }

    fuse_height = true;
}

if (fuse_height) {
    if (_control_status.flags.baro_hgt) {
        Vector2f baro_hgt_innov_gate;
        Vector3f baro_hgt_obs_var;

        // vertical position innovation - baro measurement has opposite sign to earth z axis
        _baro_hgt_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset;
        // observation variance - user parameter defined
        baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
        // innovation gate size
        baro_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);

        // Compensate for positive static pressure transients (negative vertical position innovations)
        // caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
        float deadzone_start = 0.0f;
        float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;

        if (_control_status.flags.gnd_effect) {
            if (_baro_hgt_innov(2) < -deadzone_start) {
                if (_baro_hgt_innov(2) <= -deadzone_end) {
                    _baro_hgt_innov(2) += deadzone_end;

                } else {
                    _baro_hgt_innov(2) = -deadzone_start;
                }
            }
        }
        // fuse height information
        fuseVerticalPosition(_baro_hgt_innov,baro_hgt_innov_gate,
            baro_hgt_obs_var, _baro_hgt_innov_var,_baro_hgt_test_ratio);

    } else if (_control_status.flags.gps_hgt) {
        Vector2f gps_hgt_innov_gate;
        Vector3f gps_hgt_obs_var;
        // vertical position innovation - gps measurement has opposite sign to earth z axis
        _gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
        // observation variance - receiver defined and parameter limited
        // use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
        const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
        const float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
        gps_hgt_obs_var(2) = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
        // innovation gate size
        gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
        // fuse height information
        fuseVerticalPosition(_gps_pos_innov,gps_hgt_innov_gate,
            gps_hgt_obs_var, _gps_pos_innov_var,_gps_pos_test_ratio);

    } else if (_control_status.flags.rng_hgt) {
        Vector2f rng_hgt_innov_gate;
        Vector3f rng_hgt_obs_var;
        // use range finder with tilt correction
        _rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
                         _params.rng_gnd_clearance)) - _hgt_sensor_offset;
        // observation variance - user parameter defined
        rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
                       + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
        // innovation gate size
        rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
        // fuse height information
        fuseVerticalPosition(_rng_hgt_innov,rng_hgt_innov_gate,
            rng_hgt_obs_var, _rng_hgt_innov_var,_rng_hgt_test_ratio);

    } else if (_control_status.flags.ev_hgt) {
        Vector2f ev_hgt_innov_gate;
        Vector3f ev_hgt_obs_var;
        // calculate the innovation assuming the external vision observation is in local NED frame
        _ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
        // observation variance - defined externally
        ev_hgt_obs_var(2) = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
        // innovation gate size
        ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
        // fuse height information
        fuseVerticalPosition(_ev_pos_innov,ev_hgt_innov_gate,
            ev_hgt_obs_var, _ev_pos_innov_var,_ev_pos_test_ratio);
    }
}

}

In case VDIST_SENSOR_RANGE,if we do not get ranger data in ring buffer,height source will be changed from ranger fusion to baro fusion.The above changed logic is not correct. if (_range_sensor.isDataHealthy()) { setControlRangeHeight(); fuse_height = true;

        if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
            // we have just switched to using range finder, calculate height sensor offset such that current
            // measurement matches our current height estimate
            // use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
            if (_control_status.flags.in_air && isTerrainEstimateValid()) {
                _hgt_sensor_offset = _terrain_vpos;

            } else if (_control_status.flags.in_air) {
                _hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);

            } else {
                _hgt_sensor_offset = _params.rng_gnd_clearance;
            }
        }

    } else if (_baro_data_ready && !_baro_hgt_faulty) {
        startBaroHgtFusion();
        fuse_height = true;
    }

The check of ranger valid check should be constraint in a time region.

[garlinplus]

The case VDIST_SENSOR_RANGE may be modified as followed: if (_range_sensor.isDataHealthy()) { setControlRangeHeight(); fuse_height = true;

    if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
        // we have just switched to using range finder, calculate height sensor offset such that current
        // measurement matches our current height estimate
        // use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
        if (_control_status.flags.in_air && isTerrainEstimateValid()) {
            _hgt_sensor_offset = _terrain_vpos;

        } else if (_control_status.flags.in_air) {
            _hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);

        } else {
            _hgt_sensor_offset = _params.rng_gnd_clearance;
        }
    }

} else if (!_range_sensor.Healthy()&& _baro_data_ready && !_baro_hgt_faulty) {
    startBaroHgtFusion();
    fuse_height = true;
}