diff --git a/EKF/control.cpp b/EKF/control.cpp index 7a0d753393..dc180d1aca 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -49,12 +49,71 @@ void Ekf::controlFusionModes() // Get the magnetic declination calcMagDeclination(); + // Check for tilt convergence during initial alignment + // filter the tilt error vector using a 1 sec time constant LPF + float filt_coef = 1.0f * _imu_sample_delayed.delta_ang_dt; + _tilt_err_length_filt = filt_coef * _tilt_err_vec.norm() + (1.0f - filt_coef) * _tilt_err_length_filt; + + // Once the tilt error has reduced sufficiently, initialise the yaw and magnetic field states + if (_tilt_err_length_filt < 0.005f && !_control_status.flags.tilt_align) { + _control_status.flags.tilt_align = true; + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + } + // optical flow fusion mode selection logic - _control_status.flags.opt_flow = false; + // to start using optical flow data we need angular alignment complete, and fresh optical flow and height above terrain data + if ((_params.fusion_mode & MASK_USE_OF) && !_control_status.flags.opt_flow && _control_status.flags.tilt_align + && (_time_last_imu - _time_last_optflow) < 5e5 && (_time_last_imu - _time_last_hagl_fuse) < 5e5) { + // If the heading is not aligned, reset the yaw and magnetic field states + if (!_control_status.flags.yaw_align) { + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + } + + // If the heading is valid, start using optical flow aiding + if (_control_status.flags.yaw_align) { + // set the flag and reset the fusion timeout + _control_status.flags.opt_flow = true; + _time_last_of_fuse = _time_last_imu; + + // if we are not using GPS and are in air, then we need to reset the velocity to be consistent with the optical flow reading + if (!_control_status.flags.gps) { + // calculate the rotation matrix from body to earth frame + matrix::Dcm body_to_earth(_state.quat_nominal); + + // constrain height above ground to be above minimum possible + float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); + + // calculate absolute distance from focal point to centre of frame assuming a flat earth + float range = heightAboveGndEst / body_to_earth(2, 2); + + if (_in_air && (range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) { + // calculate X and Y body relative velocities from OF measurements + Vector3f vel_optflow_body; + vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt; + vel_optflow_body(1) = range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt; + vel_optflow_body(2) = 0.0f; + + // rotate from body to earth frame + Vector3f vel_optflow_earth; + vel_optflow_earth = body_to_earth * vel_optflow_body; + + // take x and Y components + _state.vel(0) = vel_optflow_earth(0); + _state.vel(1) = vel_optflow_earth(1); + + } else { + _state.vel.setZero(); + } + } + } + + } else if (!(_params.fusion_mode & MASK_USE_OF)) { + _control_status.flags.opt_flow = false; + } // GPS fusion mode selection logic - // To start using GPS we need tilt and yaw alignment completed, the local NED origin set and fresh GPS data - if (!_control_status.flags.gps) { + // To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data + if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { if (_control_status.flags.tilt_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) { // If the heading is not aligned, reset the yaw and magnetic field states @@ -67,13 +126,12 @@ void Ekf::controlFusionModes() resetPosition(); resetVelocity(); _control_status.flags.gps = true; + _time_last_gps = _time_last_imu; } } - } - // decide when to start using optical flow data - if (!_control_status.flags.opt_flow) { - // TODO optical flow start logic + } else if (!(_params.fusion_mode & MASK_USE_GPS)) { + _control_status.flags.gps = false; } // handle the case when we are relying on GPS fusion and lose it @@ -106,7 +164,15 @@ void Ekf::controlFusionModes() // handle the case when we are relying on optical flow fusion and lose it if (_control_status.flags.opt_flow && !_control_status.flags.gps) { - // TODO + // We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something + if ((_time_last_imu - _time_last_of_fuse > 5e6)) { + // Switch to the non-aiding mode, zero the veloity states + // and set the synthetic position to the current estimate + _control_status.flags.opt_flow = false; + _last_known_posNE(0) = _state.pos(0); + _last_known_posNE(1) = _state.pos(1); + _state.vel.setZero(); + } } // Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances @@ -177,6 +243,12 @@ void Ekf::controlFusionModes() _control_status.flags.mag_dec = false; } + // Control the soure of height measurements for the main filter + _control_status.flags.baro_hgt = true; + _control_status.flags.rng_hgt = false; + _control_status.flags.gps_hgt = false; + + // Placeholder for control of wind velocity states estimation // TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement if (false) { @@ -199,6 +271,7 @@ void Ekf::calculateVehicleStatus() // Transition to in-air occurs when armed and when altitude has increased sufficiently from the altitude at arming bool in_air = _control_status.flags.armed && (_state.pos(2) - _last_disarmed_posD) < -1.0f; + if (!_control_status.flags.in_air && in_air) { _control_status.flags.in_air = true; } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 5441c8a082..9565388ca4 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -36,7 +36,7 @@ * Core functions for ekf attitude and position estimator. * * @author Roman Bast - * + * @author Paul Riseborough */ #include "ekf.h" @@ -48,6 +48,8 @@ Ekf::Ekf(): _fuse_pos(false), _fuse_hor_vel(false), _fuse_vert_vel(false), + _fuse_flow(false), + _fuse_hagl_data(false), _time_last_fake_gps(0), _time_last_pos_fuse(0), _time_last_vel_fuse(0), @@ -56,6 +58,7 @@ Ekf::Ekf(): _last_disarmed_posD(0.0f), _heading_innov(0.0f), _heading_innov_var(0.0f), + _delta_time_of(0.0f), _mag_declination(0.0f), _gpsDriftVelN(0.0f), _gpsDriftVelE(0.0f), @@ -66,10 +69,14 @@ Ekf::Ekf(): _last_gps_fail_us(0), _last_gps_origin_time_us(0), _gps_alt_ref(0.0f), - _baro_counter(0), - _baro_sum(0.0f), + _hgt_counter(0), + _hgt_sum(0.0f), _mag_counter(0), - _baro_at_alignment(0.0f) + _hgt_at_alignment(0.0f), + _terrain_vpos(0.0f), + _hagl_innov(0.0f), + _hagl_innov_var(0.0f), + _time_last_hagl_fuse(0) { _control_status = {}; _control_status_prev = {}; @@ -89,6 +96,8 @@ Ekf::Ekf(): _q_down_sampled.setZero(); _mag_sum = {}; _delVel_sum = {}; + _flow_gyro_bias = {}; + _imu_del_ang_of = {}; } Ekf::~Ekf() @@ -137,93 +146,137 @@ bool Ekf::init(uint64_t timestamp) _mag_healthy = false; _filter_initialised = false; + _terrain_initialised = false; return ret; } bool Ekf::update() { - bool ret = false; // indicates if there has been an update - - if (!_filter_initialised) { - _filter_initialised = initialiseFilter(); + // Only run the filter if IMU data in the buffer has been updated + if (_imu_updated) { if (!_filter_initialised) { - return false; + _filter_initialised = initialiseFilter(); + + if (!_filter_initialised) { + return false; + } } - } - //printStates(); - //printStatesFast(); - // prediction - if (_imu_updated) { - ret = true; + // perform state and covariance prediction for the main filter predictState(); predictCovariance(); - } - // control logic - controlFusionModes(); + // perform state and variance prediction for the terrain estimator + if (!_terrain_initialised) { + _terrain_initialised = initHagl(); + + } else { + predictHagl(); + } - // measurement updates + // control logic + controlFusionModes(); - // Fuse magnetometer data using the selected fusion method and only if angular alignment is complete - if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { - if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) { - fuseMag(); + // measurement updates - if (_control_status.flags.mag_dec) { - fuseDeclination(); + // Fuse magnetometer data using the selected fuson method and only if angular alignment is complete + if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { + if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) { + fuseMag(); + + if (_control_status.flags.mag_dec) { + fuseDeclination(); + } + + } else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) { + fuseMag2D(); + + } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { + // fusion of a Euler yaw angle from either a 321 or 312 rotation sequence + fuseHeading(); + + } else { + // do no fusion at all } + } - } else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) { - fuseMag2D(); + // determine if we have height data that can be fused + if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) + && (_R_prev(2, 2) > 0.7071f)) { + // if we have range data we always try to estimate terrain nheight + _fuse_hagl_data = true; - } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { - // fusion of a Euler yaw angle from either a 321 or 312 rotation sequence - fuseHeading(); + // only use if for height in the main filter if specifically enabled + if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { + _fuse_height = true; + } - } else { - // do no fusion at all + } else if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed) + && _params.vdist_sensor_type == VDIST_SENSOR_BARO) { + _fuse_height = true; } - } - if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { - _fuse_height = true; - } + // If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it + // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift + // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz + if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) { + _fuse_pos = true; + _fuse_vert_vel = true; + _fuse_hor_vel = true; + _fuse_flow = false; + + } else if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed) + && _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5 + && (_R_prev(2, 2) > 0.7071f)) { + _fuse_pos = false; + _fuse_vert_vel = false; + _fuse_hor_vel = false; + _fuse_flow = true; + + } else if (!_control_status.flags.gps && !_control_status.flags.opt_flow + && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { + _fuse_pos = true; + _gps_sample_delayed.pos(0) = _last_known_posNE(0); + _gps_sample_delayed.pos(1) = _last_known_posNE(1); + _time_last_fake_gps = _time_last_imu; + } - // If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it - // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift - // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz - if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) { - _fuse_pos = true; - _fuse_vert_vel = true; - _fuse_hor_vel = true; - - } else if (!_control_status.flags.gps && !_control_status.flags.opt_flow - && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { - _fuse_pos = true; - _gps_sample_delayed.pos(0) = _last_known_posNE(0); - _gps_sample_delayed.pos(1) = _last_known_posNE(1); - _time_last_fake_gps = _time_last_imu; - } + // fuse available range finder data into a terrain height estimator if it has been initialised + if (_fuse_hagl_data && _terrain_initialised) { + fuseHagl(); + _fuse_hagl_data = false; + } - if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { - fuseVelPosHeight(); - _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; - } + // Fuse available NED velocity and position data into the main filter + if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { + fuseVelPosHeight(); + _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; + } - if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) { - fuseRange(); - } + // Update optical flow bias estimates + calcOptFlowBias(); - if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) { - fuseAirspeed(); + // Fuse optical flow LOS rate observations into the main filter + if (_fuse_flow) { + fuseOptFlow(); + _last_known_posNE(0) = _state.pos(0); + _last_known_posNE(1) = _state.pos(1); + _fuse_flow = false; + } } + // the output observer always runs calculateOutputStates(); - return ret; + // We don't have valid data to output until tilt and yaw alignment is complete + if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) { + return true; + + } else { + return false; + } } bool Ekf::initialiseFilter(void) @@ -231,7 +284,8 @@ bool Ekf::initialiseFilter(void) // Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter // Sum the IMU delta angle measurements - _delVel_sum += _imu_down_sampled.delta_vel; + imuSample imu_init = _imu_buffer.get_newest(); + _delVel_sum += imu_init.delta_vel; // Sum the magnetometer measurements magSample mag_init = _mag_buffer.get_newest(); @@ -241,17 +295,26 @@ bool Ekf::initialiseFilter(void) _mag_sum += mag_init.mag; } - // Sum the barometer measurements - // initialize vertical position with newest baro measurement - baroSample baro_init = _baro_buffer.get_newest(); + if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { + rangeSample range_init = _range_buffer.get_newest(); - if (baro_init.time_us != 0) { - _baro_counter ++; - _baro_sum += baro_init.hgt; + if (range_init.time_us != 0) { + _hgt_counter ++; + _hgt_sum += range_init.rng; + } + + } else { + // initialize vertical position with newest baro measurement + baroSample baro_init = _baro_buffer.get_newest(); + + if (baro_init.time_us != 0) { + _hgt_counter ++; + _hgt_sum += baro_init.hgt; + } } - // check to see if we have enough measurements and return false if not - if (_baro_counter < 10 || _mag_counter < 10) { + // check to see if we have enough measruements and return false if not + if (_hgt_counter < 10 || _mag_counter < 10) { return false; } else { @@ -283,16 +346,18 @@ bool Ekf::initialiseFilter(void) matrix::Euler euler_init(roll, pitch, 0.0f); _state.quat_nominal = Quaternion(euler_init); _output_new.quat_nominal = _state.quat_nominal; - _control_status.flags.tilt_align = true; + + // initialise the filtered alignment error estimate to a larger starting value + _tilt_err_length_filt = 1.0f; // calculate the averaged magnetometer reading Vector3f mag_init = _mag_sum * (1.0f / (float(_mag_counter))); // calculate the initial magnetic field and yaw alignment - _control_status.flags.yaw_align = resetMagHeading(mag_init); + resetMagHeading(mag_init); // calculate the averaged barometer reading - _baro_at_alignment = _baro_sum / (float)_baro_counter; + _hgt_at_alignment = _hgt_sum / (float)_hgt_counter; // set the velocity to the GPS measurement (by definition, the initial position and height is at the origin) resetVelocity(); @@ -300,6 +365,9 @@ bool Ekf::initialiseFilter(void) // initialise the state covariance matrix initialiseCovariance(); + // initialise the terrain estimator + initHagl(); + return true; } } @@ -354,7 +422,6 @@ bool Ekf::collect_imu(imuSample &imu) _imu_down_sampled.delta_ang_dt += imu.delta_ang_dt; _imu_down_sampled.delta_vel_dt += imu.delta_vel_dt; - Quaternion delta_q; delta_q.rotate(imu.delta_ang); _q_down_sampled = _q_down_sampled * delta_q; @@ -453,8 +520,3 @@ void Ekf::fuseAirspeed() { } - -void Ekf::fuseRange() -{ - -}