PX4 / PX4-ECL

Estimation & Control Library for Guidance, Navigation and Control Applications
http://dev.px4.io
BSD 3-Clause "New" or "Revised" License
478 stars 508 forks source link

Attitude estimation doesn't start without baro sensor #876

Open lukegluke opened 4 years ago

lukegluke commented 4 years ago

For now baro data is essential to initialize height in Ekf::initialiseFilter() before switching to another source (GPS, etc.) https://github.com/PX4/ecl/blob/5356077a3244a9a29adfae4aeaaab900cd28e9e8/EKF/ekf.cpp#L193

But it looks not really adoptable because filters will never get initialized for rovers that could have no baro sensor at all so the attitude estimator will not work (I don't dig enough yet - does attitude estimator itself needs height data?).

bresch commented 4 years ago

A source of vertical position and/or velocity is required to estimate the Z accel bias but not having a baro shouldn't block the whole EKF. Same for the mag: https://github.com/PX4/ecl/issues/800

garlinplus commented 3 years ago

Actualy if you hasn't a baro sensor,you can make a fake baro always witch 0 height,And you can get a attitude.

lukegluke commented 3 years ago

Yes, this is a workaround, but not the solution.

By the way for SYS_HAS_BARO and SYS_HAS_MAG parameters description says: Disable this if the board has no magnetometer[barometer], such as the Omnibus F4 SD. I wonder how ekf2 works on Omnibus F4 SD then?

bresch commented 3 years ago

I wonder how ekf2 works on Omnibus F4 SD then?

It doesn't, those drones are usually racers and use the "Q estimator" instead.

bresch commented 3 years ago

This will be fixed by https://github.com/PX4/PX4-ECL/pull/931 <= edit: the baro part didn't get merged in that PR

priseborough commented 3 years ago

We should fake a constant baro measurement with a larger uncertainty and set validity of vz and z to false so control loops don't try to use the height estimate.

lukegluke commented 3 years ago

@priseborough, I don't know if it is a suitable solution in general case (I'm working with rovers), but I workaround this issue by using gps height instead of baro (so no initialization until gps is valid) in Ekf::initialiseFilter() here. https://github.com/PX4/PX4-ECL/blob/b3fed06fe822d08d19ab1d2c2f8daf7b7d21951c/EKF/ekf.cpp#L172

if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
    // accumulate enough height measurements to be confident in the quality of the data
    if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
        if (_baro_sample_delayed.time_us != 0) {
            if (_hgt_counter == 0) {
                _baro_hgt_offset = _baro_sample_delayed.hgt;

            } else {
                _baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
            }

            _hgt_counter++;
        }
    }

} else if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
    if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
        if (_gps_sample_delayed.time_us != 0) {
            if (_hgt_counter == 0) {
                _baro_hgt_offset = _gps_sample_delayed.hgt;

            } else {
                _baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _gps_sample_delayed.hgt;
            }

            _hgt_counter++;
        }
    }
}

Of course it would better to check SYS_HAS_BARO instead.