void DataValidatorGroup::print() { /* print the group's state */ ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)", _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", _toggle_count); DataValidator *next = _first; unsigned i = 0; while (next != nullptr) { if (next->used()) { uint32_t flags = next->state(); ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(), ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TOUT" : ""), ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ECNT" : ""), ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " EDNST" : ""), ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); next->print(); } next = next->sibling(); i++; } }
void DataValidator::print() { if (_time_last == 0) { ECL_INFO("\tno data"); return; } for (unsigned i = 0; i < dimensions; i++) { ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f", (double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i], (double)confidence(hrt_absolute_time())); } }
void Ekf::controlFusionModes() { // Store the status to enable change detection _control_status_prev.value = _control_status.value; // Get the magnetic declination calcMagDeclination(); // monitor the tilt alignment if (!_control_status.flags.tilt_align) { // whilst we are aligning the tilt, monitor the variances 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(0.05235f)) { _control_status.flags.tilt_align = true; _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); ECL_INFO("EKF alignment complete"); } } // check for arrival of new sensor data at the fusion time horizon _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); _baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed); _range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) && (_R_to_earth(2, 2) > 0.7071f); _flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed) && (_R_to_earth(2, 2) > 0.7071f); _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(); controlExternalVisionFusion(); controlOpticalFlowFusion(); controlGpsFusion(); controlBaroFusion(); controlRangeFinderFusion(); controlAirDataFusion(); // for efficiency, fusion of direct state observations for position ad velocity is performed sequentially // in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc) controlVelPosFusion(); }
bool EstimatorInterface::initialise_interface(uint64_t timestamp) { // find the maximum time delay required to compensate for uint16_t max_time_delay_ms = math::max(_params.mag_delay_ms, math::max(_params.range_delay_ms, math::max(_params.gps_delay_ms, math::max(_params.flow_delay_ms, math::max(_params.ev_delay_ms, math::max(_params.airspeed_delay_ms, _params.baro_delay_ms)))))); // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter _imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1; // set the observaton buffer length to handle the minimum time of arrival between observations in combination // with the worst case delay from current time to ekf fusion time // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceil((float)max_time_delay_ms * 0.5f)); _obs_buffer_length = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1; // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) _obs_buffer_length = math::min(_obs_buffer_length,_imu_buffer_length); ECL_INFO("EKF IMU buffer length = %i",(int)_imu_buffer_length); ECL_INFO("EKF observation buffer length = %i",(int)_obs_buffer_length); if (!(_imu_buffer.allocate(_imu_buffer_length) && _gps_buffer.allocate(_obs_buffer_length) && _mag_buffer.allocate(_obs_buffer_length) && _baro_buffer.allocate(_obs_buffer_length) && _range_buffer.allocate(_obs_buffer_length) && _airspeed_buffer.allocate(_obs_buffer_length) && _flow_buffer.allocate(_obs_buffer_length) && _ext_vision_buffer.allocate(_obs_buffer_length) && _output_buffer.allocate(_imu_buffer_length))) { ECL_ERR("EKF buffer allocation failed!"); unallocate_buffers(); return false; } // zero the data in the observation buffers for (int index=0; index < _obs_buffer_length; index++) { gpsSample gps_sample_init = {}; _gps_buffer.push(gps_sample_init); magSample mag_sample_init = {}; _mag_buffer.push(mag_sample_init); baroSample baro_sample_init = {}; _baro_buffer.push(baro_sample_init); rangeSample range_sample_init = {}; _range_buffer.push(range_sample_init); airspeedSample airspeed_sample_init = {}; _airspeed_buffer.push(airspeed_sample_init); flowSample flow_sample_init = {}; _flow_buffer.push(flow_sample_init); extVisionSample ext_vision_sample_init = {}; _ext_vision_buffer.push(ext_vision_sample_init); } // zero the data in the imu data and output observer state buffers for (int index=0; index < _imu_buffer_length; index++) { imuSample imu_sample_init = {}; _imu_buffer.push(imu_sample_init); outputSample output_sample_init = {}; _output_buffer.push(output_sample_init); } _dt_imu_avg = 0.0f; _imu_sample_delayed.delta_ang.setZero(); _imu_sample_delayed.delta_vel.setZero(); _imu_sample_delayed.delta_ang_dt = 0.0f; _imu_sample_delayed.delta_vel_dt = 0.0f; _imu_sample_delayed.time_us = timestamp; _imu_ticks = 0; _initialised = false; _time_last_imu = 0; _time_last_gps = 0; _time_last_mag = 0; _time_last_baro = 0; _time_last_range = 0; _time_last_airspeed = 0; _time_last_optflow = 0; memset(&_fault_status.flags, 0, sizeof(_fault_status.flags)); _time_last_ext_vision = 0; return true; }
void Ekf::controlExternalVisionFusion() { // Check for new exernal vision data if (_ev_data_ready) { // external vision position aiding selection logic if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) { // check for a exernal vision measurement that has fallen behind the fusion time horizon if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // turn on use of external vision measurements for position and height _control_status.flags.ev_pos = true; ECL_INFO("EKF switching to external vision position fusion"); // turn off other forms of height aiding _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; // reset the position, height and velocity resetPosition(); resetVelocity(); resetHeight(); } } // external vision yaw aiding selection logic if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { // check for a exernal vision measurement that has fallen behind the fusion time horizon if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // reset the yaw angle to the value from the observaton quaternion // get the roll, pitch, yaw estimates from the quaternion states matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2), _state.quat_nominal(3)); matrix::Euler<float> euler_init(q_init); // get initial yaw from the observation quaternion extVisionSample ev_newest = _ext_vision_buffer.get_newest(); matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3)); matrix::Euler<float> euler_obs(q_obs); euler_init(2) = euler_obs(2); // save a copy of the quaternion state for later use in calculating the amount of reset change Quaternion quat_before_reset = _state.quat_nominal; // calculate initial quaternion states for the ekf _state.quat_nominal = Quaternion(euler_init); // calculate the amount that the quaternion has changed by _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed(); // add the reset amount to the output observer buffered data outputSample output_states; unsigned output_length = _output_buffer.get_length(); for (unsigned i=0; i < output_length; i++) { output_states = _output_buffer.get_from_index(i); output_states.quat_nominal *= _state_reset_status.quat_change; _output_buffer.push_to_index(i,output_states); } // capture the reset event _state_reset_status.quat_counter++; // flag the yaw as aligned _control_status.flags.yaw_align = true; // turn on fusion of external vision yaw measurements and disable all magnetoemter fusion _control_status.flags.ev_yaw = true; _control_status.flags.mag_hdg = false; _control_status.flags.mag_3D = false; _control_status.flags.mag_dec = false; ECL_INFO("EKF switching to external vision yaw fusion"); } } // determine if we should use the height observation if (_params.vdist_sensor_type == VDIST_SENSOR_EV) { _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = true; _fuse_height = true; } // determine if we should use the horizontal position observations if (_control_status.flags.ev_pos) { _fuse_pos = true; // correct position and height for offset relative to IMU Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _ev_sample_delayed.posNED(0) -= pos_offset_earth(0); _ev_sample_delayed.posNED(1) -= pos_offset_earth(1); _ev_sample_delayed.posNED(2) -= pos_offset_earth(2); } // determine if we should use the yaw observation if (_control_status.flags.ev_yaw) { fuseHeading(); } } }
void Ekf::controlHeightSensorTimeouts() { /* * Handle the case where we have not fused height measurements recently and * uncertainty exceeds the max allowable. Reset using the best available height * measurement source, continue using it after the reset and declare the current * source failed if we have switched. */ // check for inertial sensing errors as evidenced by the vertical innovations having the same sign and not stale bool bad_vert_accel = (_control_status.flags.baro_hgt && // we can only run this check if vertical position and velocity observations are indepedant (_vel_pos_innov[5] * _vel_pos_innov[2] > 0.0f) && // vertical position and velocity sensors are in agreement ((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) && // vertical position data is fresh ((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) && // vertical velocity data is freshs _vel_pos_test_ratio[2] > 1.0f && // vertical velocty innovations have failed innovation consistency checks _vel_pos_test_ratio[5] > 1.0f); // vertical position innovations have failed innovation consistency checks // record time of last bad vert accel if (bad_vert_accel) { _time_bad_vert_accel = _time_last_imu; } if ((P[9][9] > sq(_params.hgt_reset_lim)) && ((_time_last_imu - _time_last_hgt_fuse) > 5e6)) { // boolean that indicates we will do a height reset bool reset_height = false; // handle the case where we are using baro for height if (_control_status.flags.baro_hgt) { // check if GPS height is available gpsSample gps_init = _gps_buffer.get_newest(); bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); baroSample baro_init = _baro_buffer.get_newest(); bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); // check for inertial sensing errors in the last 10 seconds bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < 10E6); // reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data bool reset_to_gps = gps_hgt_available && gps_hgt_accurate && !_gps_hgt_faulty && !prev_bad_vert_accel; // reset to GPS if GPS data is available and there is no Baro data reset_to_gps = reset_to_gps || (gps_hgt_available && !baro_hgt_available); // reset to Baro if we are not doing a GPS reset and baro data is available bool reset_to_baro = !reset_to_gps && baro_hgt_available; if (reset_to_gps) { // set height sensor health _baro_hgt_faulty = true; _gps_hgt_faulty = false; // declare the GPS height healthy _gps_hgt_faulty = false; // reset the height mode _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; // request a reset reset_height = true; ECL_INFO("EKF baro hgt timeout - reset to GPS"); } else if (reset_to_baro){ // set height sensor health _baro_hgt_faulty = false; // reset the height mode _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; // request a reset reset_height = true; ECL_INFO("EKF baro hgt timeout - reset to baro"); } else { // we have nothing we can reset to // deny a reset reset_height = false; } } // handle the case we are using GPS for height if (_control_status.flags.gps_hgt) { // check if GPS height is available gpsSample gps_init = _gps_buffer.get_newest(); bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); // check the baro height source for consistency and freshness baroSample baro_init = _baro_buffer.get_newest(); bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate); // if baro data is acceptable and GPS data is inaccurate, reset height to baro bool reset_to_baro = baro_data_consistent && baro_data_fresh && !_baro_hgt_faulty && !gps_hgt_accurate; // if GPS height is unavailable and baro data is available, reset height to baro reset_to_baro = reset_to_baro || (!gps_hgt_available && baro_data_fresh); // if we cannot switch to baro and GPS data is available, reset height to GPS bool reset_to_gps = !reset_to_baro && gps_hgt_available; if (reset_to_baro) { // set height sensor health _gps_hgt_faulty = true; _baro_hgt_faulty = false; // reset the height mode _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; // request a reset reset_height = true; ECL_INFO("EKF gps hgt timeout - reset to baro"); } else if (reset_to_gps) { // set height sensor health _gps_hgt_faulty = false; // reset the height mode _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; // request a reset reset_height = true; ECL_INFO("EKF gps hgt timeout - reset to GPS"); } else { // we have nothing to reset to reset_height = false; } } // handle the case we are using range finder for height if (_control_status.flags.rng_hgt) { // check if range finder data is available rangeSample rng_init = _range_buffer.get_newest(); bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL); // check if baro data is available baroSample baro_init = _baro_buffer.get_newest(); bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); // reset to baro if we have no range data and baro data is available bool reset_to_baro = !rng_data_available && baro_data_available; // reset to range data if it is available bool reset_to_rng = rng_data_available; if (reset_to_baro) { // set height sensor health _rng_hgt_faulty = true; _baro_hgt_faulty = false; // reset the height mode _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; // request a reset reset_height = true; ECL_INFO("EKF rng hgt timeout - reset to baro"); } else if (reset_to_rng) { // set height sensor health _rng_hgt_faulty = false; // reset the height mode _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = true; _control_status.flags.ev_hgt = false; // request a reset reset_height = true; ECL_INFO("EKF rng hgt timeout - reset to rng hgt"); } else { // we have nothing to reset to reset_height = false; } } // handle the case where we are using external vision data for height if (_control_status.flags.ev_hgt) { // check if vision data is available extVisionSample ev_init = _ext_vision_buffer.get_newest(); bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL); // check if baro data is available baroSample baro_init = _baro_buffer.get_newest(); bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); // reset to baro if we have no vision data and baro data is available bool reset_to_baro = !ev_data_available && baro_data_available; // reset to ev data if it is available bool reset_to_ev = ev_data_available; if (reset_to_baro) { // set height sensor health _rng_hgt_faulty = true; _baro_hgt_faulty = false; // reset the height mode _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; // request a reset reset_height = true; ECL_INFO("EKF ev hgt timeout - reset to baro"); } else if (reset_to_ev) { // reset the height mode _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = true; // request a reset reset_height = true; ECL_INFO("EKF ev hgt timeout - reset to ev hgt"); } else { // we have nothing to reset to reset_height = false; } } // Reset vertical position and velocity states to the last measurement if (reset_height) { resetHeight(); // Reset the timout timer _time_last_hgt_fuse = _time_last_imu; } } }
void Ekf::controlGpsFusion() { // Check for new GPS data that has fallen behind the fusion time horizon if (_gps_data_ready) { // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { if (_control_status.flags.tilt_align && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) { // 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 gps aiding if (_control_status.flags.yaw_align) { _control_status.flags.gps = true; _time_last_gps = _time_last_imu; // if we are not already aiding with optical flow, then we need to reset the position and velocity if (!_control_status.flags.opt_flow) { if (resetPosition() && resetVelocity()) { _control_status.flags.gps = true; } else { _control_status.flags.gps = false; } } if (_control_status.flags.gps) { ECL_INFO("EKF commencing GPS aiding"); } } } } 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 if (_control_status.flags.gps && !_control_status.flags.opt_flow) { // We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) { if (_time_last_imu - _time_last_gps > 5e5) { // if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states // and set the synthetic GPS position to the current estimate _control_status.flags.gps = false; _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); _state.vel.setZero(); ECL_WARN("EKF GPS fusion timout - stopping GPS aiding"); } else { // Reset states to the last GPS measurement resetPosition(); resetVelocity(); ECL_WARN("EKF GPS fusion timout - resetting to GPS"); // Reset the timeout counters _time_last_pos_fuse = _time_last_imu; _time_last_vel_fuse = _time_last_imu; } } } // Only use GPS data for position and velocity aiding if enabled if (_control_status.flags.gps) { _fuse_pos = true; _fuse_vert_vel = true; _fuse_hor_vel = true; // correct velocity for offset relative to IMU Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt); Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body); Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; _gps_sample_delayed.vel -= vel_offset_earth; // correct position and height for offset relative to IMU Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _gps_sample_delayed.pos(0) -= pos_offset_earth(0); _gps_sample_delayed.pos(1) -= pos_offset_earth(1); _gps_sample_delayed.hgt += pos_offset_earth(2); } // Determine if GPS should be used as the height source if (((_params.vdist_sensor_type == VDIST_SENSOR_GPS)) && !_gps_hgt_faulty) { _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; _fuse_height = true; } } }