fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 14:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

PX4 style fixes (excluding EKF generated code)

Browse Source
This commit is contained in:
Daniel Agar
2018-05-03 22:25:36 -04:00
parent e5952fadaf
commit c5b2f59f57
22 changed files with 384 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/RingBuffer.h
+9 -6
View File
@@ -45,7 +45,8 @@ template <typename data_type>
class RingBuffer
{
public:
RingBuffer() {
RingBuffer()
{
if (allocate(1)) {
// initialize with one empty sample
data_type d = {};
@@ -81,17 +82,19 @@ public:
for (uint8_t index = 0; index < _size; index++) {
_buffer[index] = {};
}
_first_write = true;
return true;
}
void unallocate() {
void unallocate()
{
delete[] _buffer;
_buffer = nullptr;
}
void push(const data_type& sample)
void push(const data_type &sample)
{
uint8_t head_new = _head;
@@ -115,12 +118,12 @@ public:
data_type &operator[](const uint8_t index) { return _buffer[index]; }
const data_type& get_newest() { return _buffer[_head]; }
const data_type& get_oldest() { return _buffer[_tail]; }
const data_type &get_newest() { return _buffer[_head]; }
const data_type &get_oldest() { return _buffer[_tail]; }
uint8_t get_oldest_index() const { return _tail; }
bool pop_first_older_than(const uint64_t& timestamp, data_type *sample)
bool pop_first_older_than(const uint64_t &timestamp, data_type *sample)
{
// start looking from newest observation data
for (uint8_t i = 0; i < _size; i++) {
EKF/airspeed_fusion.cpp
+6 -2
View File
@@ -121,6 +121,7 @@ void Ekf::fuseAirspeed()
for (unsigned row = 0; row <= 21; row++) {
Kfusion[row] = 0.0f;
}
} else {
// we have no other source of aiding, so use airspeed measurements to correct states
Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]);
@@ -164,6 +165,7 @@ void Ekf::fuseAirspeed()
if (_tas_test_ratio > 1.0f) {
_innov_check_fail_status.flags.reject_airspeed = true;
return;
} else {
_innov_check_fail_status.flags.reject_airspeed = false;
}
@@ -176,6 +178,7 @@ void Ekf::fuseAirspeed()
// then calculate P - KHP
float KHP[_k_num_states][_k_num_states];
float KH[5];
for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion[row] * H_TAS[4];
@@ -198,11 +201,12 @@ void Ekf::fuseAirspeed()
// the covariance marix is unhealthy and must be corrected
bool healthy = true;
_fault_status.flags.bad_airspeed = false;
for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P,i,i);
zeroCols(P,i,i);
zeroRows(P, i, i);
zeroCols(P, i, i);
//flag as unhealthy
healthy = false;
EKF/control.cpp
+117 -55
View File
@@ -64,15 +64,19 @@ void Ekf::controlFusionModes()
// send alignment status message to the console
if (_control_status.flags.baro_hgt) {
ECL_INFO("EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length);
ECL_INFO("EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
} else if (_control_status.flags.ev_hgt) {
ECL_INFO("EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length);
ECL_INFO("EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
} else if (_control_status.flags.gps_hgt) {
ECL_INFO("EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length);
ECL_INFO("EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
} else if (_control_status.flags.rng_hgt) {
ECL_INFO("EKF aligned, (range height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length);
ECL_INFO("EKF aligned, (range height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
} else {
ECL_ERR("EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length);
ECL_ERR("EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
}
}
@@ -80,13 +84,13 @@ void Ekf::controlFusionModes()
}
// check faultiness (before pop_first_older_than) to see if we can change back to original height sensor
const baroSample& baro_init = _baro_buffer.get_newest();
const baroSample &baro_init = _baro_buffer.get_newest();
_baro_hgt_faulty = !((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
const gpsSample& gps_init = _gps_buffer.get_newest();
const gpsSample &gps_init = _gps_buffer.get_newest();
_gps_hgt_faulty = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
const rangeSample& rng_init = _range_buffer.get_newest();
const rangeSample &rng_init = _range_buffer.get_newest();
_rng_hgt_faulty = !((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
// check for arrival of new sensor data at the fusion time horizon
@@ -105,12 +109,12 @@ void Ekf::controlFusionModes()
// calculate 2,2 element of rotation matrix from sensor frame to earth frame
_R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng;
_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
&& (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt);
&& (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt);
checkForStuckRange();
_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
&& (_R_to_earth(2, 2) > 0.7071f);
&& (_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);
@@ -147,13 +151,17 @@ void Ekf::controlExternalVisionFusion()
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
// needs to be calculated and the observations rotated into the EKF frame of reference
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS)
&& !(_params.fusion_mode & MASK_USE_EVYAW)) {
// rotate EV measurements into the EKF Navigation frame
calcExtVisRotMat();
}
// 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) {
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
@@ -183,7 +191,7 @@ void Ekf::controlExternalVisionFusion()
Eulerf euler_init(q_init);
// get initial yaw from the observation quaternion
const extVisionSample& ev_newest = _ext_vision_buffer.get_newest();
const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();
Quatf q_obs(ev_newest.quat);
Eulerf euler_obs(q_obs);
euler_init(2) = euler_obs(2);
@@ -251,12 +259,13 @@ void Ekf::controlExternalVisionFusion()
// Use an incremental position fusion method for EV data if using GPS or if the observations are not in NED
if (_control_status.flags.gps || (_params.fusion_mode & MASK_ROTATE_EV)) {
_fuse_hpos_as_odom = true;
} else {
_fuse_hpos_as_odom = false;
}
if(_fuse_hpos_as_odom) {
if(!_hpos_prev_available) {
if (_fuse_hpos_as_odom) {
if (!_hpos_prev_available) {
// no previous observation available to calculate position change
_fuse_pos = false;
_hpos_prev_available = true;
@@ -292,6 +301,7 @@ void Ekf::controlExternalVisionFusion()
if (_time_last_imu - _time_last_of_fuse > (uint64_t)1E6) {
resetVelocity();
}
resetPosition();
}
}
@@ -316,7 +326,10 @@ void Ekf::controlExternalVisionFusion()
fuseHeading();
}
} else if (_control_status.flags.ev_pos && (_time_last_imu - _time_last_ext_vision > (uint64_t)_params.no_gps_timeout_max)) {
} else if (_control_status.flags.ev_pos
&& (_time_last_imu - _time_last_ext_vision > (uint64_t)_params.no_gps_timeout_max)) {
// Turn off EV fusion mode if no data has been received
_control_status.flags.ev_pos = false;
ECL_INFO("EKF External Vision Data Stopped");
@@ -331,12 +344,15 @@ void Ekf::controlOpticalFlowFusion()
bool motion_is_excessive = ((_accel_mag_filt > 10.8f)
|| (_accel_mag_filt < 8.8f)
|| (_ang_rate_mag_filt > 0.5f)
|| (_R_to_earth(2,2) < 0.866f));
|| (_R_to_earth(2, 2) < 0.866f));
if (motion_is_excessive) {
_time_bad_motion_us = _imu_sample_delayed.time_us;
} else {
_time_good_motion_us = _imu_sample_delayed.time_us;
}
} else {
_time_bad_motion_us = 0;
_time_good_motion_us = _imu_sample_delayed.time_us;
@@ -348,6 +364,7 @@ void Ekf::controlOpticalFlowFusion()
if ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5) {
_inhibit_gndobs_use = true;
}
} else {
if ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6) {
_inhibit_gndobs_use = false;
@@ -357,14 +374,14 @@ void Ekf::controlOpticalFlowFusion()
// Handle cases where we are using optical flow but are no longer able to because data is old
// or its use has been inhibited.
if (_control_status.flags.opt_flow) {
if (_inhibit_gndobs_use) {
_control_status.flags.opt_flow = false;
_time_last_of_fuse = 0;
if (_inhibit_gndobs_use) {
_control_status.flags.opt_flow = false;
_time_last_of_fuse = 0;
} else if (_time_last_imu - _flow_sample_delayed.time_us > (uint64_t)_params.no_gps_timeout_max) {
_control_status.flags.opt_flow = false;
} else if (_time_last_imu - _flow_sample_delayed.time_us > (uint64_t)_params.no_gps_timeout_max) {
_control_status.flags.opt_flow = false;
}
}
}
if (_flow_data_ready) {
@@ -376,10 +393,10 @@ void Ekf::controlOpticalFlowFusion()
// optical flow fusion mode selection logic
if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
&& !_control_status.flags.opt_flow // we are not yet using flow data
&& _control_status.flags.tilt_align // we know our tilt attitude
&& get_terrain_valid()) // we have a valid distance to ground estimate
{
&& !_control_status.flags.opt_flow // we are not yet using flow data
&& _control_status.flags.tilt_align // we know our tilt attitude
&& get_terrain_valid()) { // we have a valid distance to ground estimate
// 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);
@@ -402,6 +419,7 @@ void Ekf::controlOpticalFlowFusion()
}
}
} else if (!(_params.fusion_mode & MASK_USE_OF)) {
_control_status.flags.opt_flow = false;
@@ -409,9 +427,11 @@ void Ekf::controlOpticalFlowFusion()
// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
if (_control_status.flags.opt_flow
&& !_control_status.flags.gps
&& !_control_status.flags.ev_pos) {
&& !_control_status.flags.gps
&& !_control_status.flags.ev_pos) {
bool do_reset = _time_last_imu - _time_last_of_fuse > _params.no_gps_timeout_max;
if (do_reset) {
resetVelocity();
resetPosition();
@@ -441,7 +461,9 @@ void Ekf::controlGpsFusion()
// 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 > (uint64_t)5e6)) {
if (_control_status.flags.tilt_align && _NED_origin_initialised
&& (_time_last_imu - _last_gps_fail_us > (uint64_t)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);
@@ -453,15 +475,18 @@ void Ekf::controlGpsFusion()
// if we are not already aiding with optical flow, then we need to reset the position and velocity
// otherwise we only need to reset the position
_control_status.flags.gps = true;
if (!_control_status.flags.opt_flow) {
if (!resetPosition() || !resetVelocity()) {
_control_status.flags.gps = false;
}
} else if (!resetPosition()) {
_control_status.flags.gps = false;
}
if (_control_status.flags.gps) {
ECL_INFO("EKF commencing GPS fusion");
_time_last_gps = _time_last_imu;
@@ -493,6 +518,7 @@ void Ekf::controlGpsFusion()
// if flying a fixed wing aircraft, do a complete reset that includes yaw
realignYawGPS();
}
resetVelocity();
resetPosition();
_velpos_reset_request = false;
@@ -512,7 +538,7 @@ void Ekf::controlGpsFusion()
_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 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;
@@ -526,16 +552,19 @@ void Ekf::controlGpsFusion()
// calculate observation process noise
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) {
// if we are using other sources of aiding, then relax the upper observation
// noise limit which prevents bad GPS perturbing the position estimate
_posObsNoiseNE = fmaxf(_gps_sample_delayed.hacc, lower_limit);
} else {
// if we are not using another source of aiding, then we are reliant on the GPS
// observations to constrain attitude errors and must limit the observation noise value.
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
_posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
}
_velObsVarNE(1) = _velObsVarNE(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise));
// calculate innovations
@@ -551,9 +580,8 @@ void Ekf::controlGpsFusion()
}
} else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > (uint64_t)10e6)) {
_control_status.flags.gps = false;
ECL_WARN("EKF GPS data stopped");
_control_status.flags.gps = false;
ECL_WARN("EKF GPS data stopped");
}
}
@@ -578,6 +606,7 @@ void Ekf::controlHeightSensorTimeouts()
// record time of last bad vert accel
if (bad_vert_accel) {
_time_bad_vert_accel = _time_last_imu;
} else {
_time_good_vert_accel = _time_last_imu;
}
@@ -586,6 +615,7 @@ void Ekf::controlHeightSensorTimeouts()
// for a minimum of 10 seconds
if (_bad_vert_accel_detected) {
_bad_vert_accel_detected = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION);
} else {
_bad_vert_accel_detected = bad_vert_accel;
}
@@ -604,11 +634,11 @@ void Ekf::controlHeightSensorTimeouts()
// handle the case where we are using baro for height
if (_control_status.flags.baro_hgt) {
// check if GPS height is available
const gpsSample& gps_init = _gps_buffer.get_newest();
const 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);
const baroSample& baro_init = _baro_buffer.get_newest();
const 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
@@ -659,12 +689,12 @@ void Ekf::controlHeightSensorTimeouts()
// handle the case we are using GPS for height
if (_control_status.flags.gps_hgt) {
// check if GPS height is available
const gpsSample& gps_init = _gps_buffer.get_newest();
const 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
const baroSample& baro_init = _baro_buffer.get_newest();
const 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[9][9]) * sq(_params.baro_innov_gate);
@@ -711,11 +741,11 @@ void Ekf::controlHeightSensorTimeouts()
// handle the case we are using range finder for height
if (_control_status.flags.rng_hgt) {
// check if range finder data is available
const rangeSample& rng_init = _range_buffer.get_newest();
const 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
const baroSample& baro_init = _baro_buffer.get_newest();
const 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
@@ -757,11 +787,11 @@ void Ekf::controlHeightSensorTimeouts()
// handle the case where we are using external vision data for height
if (_control_status.flags.ev_hgt) {
// check if vision data is available
const extVisionSample& ev_init = _ext_vision_buffer.get_newest();
const 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
const baroSample& baro_init = _baro_buffer.get_newest();
const 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
@@ -831,6 +861,7 @@ void Ekf::controlHeightFusion()
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
if (get_terrain_valid()) {
_hgt_sensor_offset = _terrain_vpos;
} else {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
}
@@ -851,8 +882,9 @@ void Ekf::controlHeightFusion()
// since takeoff.
if (_control_status.flags.gnd_effect) {
if ((_time_last_imu - _time_last_gnd_effect_on > GNDEFFECT_TIMEOUT) ||
(((_last_on_ground_posD - _state.pos(2)) > _params.gnd_effect_max_hgt) &&
_control_status.flags.in_air)) {
(((_last_on_ground_posD - _state.pos(2)) > _params.gnd_effect_max_hgt) &&
_control_status.flags.in_air)) {
_control_status.flags.gnd_effect = false;
}
}
@@ -882,14 +914,17 @@ void Ekf::controlHeightFusion()
if (_control_status.flags.in_air && get_terrain_valid()) {
_hgt_sensor_offset = _terrain_vpos;
} else if (_control_status.flags.in_air) {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
} else {
_hgt_sensor_offset = _params.rng_gnd_clearance;
}
}
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _baro_data_ready && !_baro_hgt_faulty) {
setControlBaroHeight();
_fuse_height = true;
@@ -914,6 +949,7 @@ void Ekf::controlHeightFusion()
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
if (get_terrain_valid()) {
_hgt_sensor_offset = _terrain_vpos;
} else {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
}
@@ -929,6 +965,7 @@ void Ekf::controlHeightFusion()
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);
}
} else 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;
@@ -968,7 +1005,9 @@ void Ekf::controlHeightFusion()
_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
}
if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt && !_range_data_ready) {
if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt
&& !_range_data_ready) {
// 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) {
@@ -993,6 +1032,7 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
if (_params.range_aid) {
// check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching
bool use_range_finder;
if (in_range_aid_mode) {
use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid();
@@ -1003,7 +1043,7 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
}
bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow)
&& (_fault_status.value == 0);
&& (_fault_status.value == 0);
if (horz_vel_valid) {
float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1));
@@ -1041,7 +1081,8 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
void Ekf::checkForStuckRange()
{
if (_range_data_ready && _range_sample_delayed.time_us - _time_last_rng_ready > (uint64_t)10e6 &&
_control_status.flags.in_air) {
_control_status.flags.in_air) {
_control_status.flags.rng_stuck = true;
//require a variance of rangefinder values to check for "stuck" measurements
@@ -1118,7 +1159,8 @@ void Ekf::controlBetaFusion()
// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > (uint64_t)10e6;
bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > (uint64_t)10e6;
if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
_control_status.flags.wind = false;
}
@@ -1127,7 +1169,7 @@ void Ekf::controlBetaFusion()
// Suffient time has lapsed sice the last fusion
bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_us;
if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) {
if (beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) {
// If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) {
// activate the wind states
@@ -1158,6 +1200,7 @@ void Ekf::controlDragFusion()
fuseDrag();
}
} else {
_control_status.flags.wind = false;
@@ -1185,6 +1228,7 @@ void Ekf::controlMagFusion()
// do no magnetometer fusion at all
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO || _params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) {
// Check if height has increased sufficiently to be away from ground magnetic anomalies
bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f;
@@ -1193,9 +1237,11 @@ void Ekf::controlMagFusion()
// Apply hysteresis to check to avoid rapid toggling
if (_yaw_angle_observable) {
_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate;
} else {
_yaw_angle_observable = _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate;
}
_yaw_angle_observable = _yaw_angle_observable && (_control_status.flags.gps || _control_status.flags.ev_pos);
// check if there is enough yaw rotation to make the mag bias states observable
@@ -1204,6 +1250,7 @@ void Ekf::controlMagFusion()
_mag_bias_observable = true;
_yaw_delta_ef = 0.0f;
_time_yaw_started = _imu_sample_delayed.time_us;
} else if (_mag_bias_observable) {
// monitor yaw rotation in 45 deg sections.
// a rotation of 45 deg is sufficient to make the mag bias observable
@@ -1211,9 +1258,11 @@ void Ekf::controlMagFusion()
_time_yaw_started = _imu_sample_delayed.time_us;
_yaw_delta_ef = 0.0f;
}
// require sustained yaw motion of 50% the initial yaw rate threshold
float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * (1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started));
_mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req;
} else {
_mag_bias_observable = false;
}
@@ -1236,15 +1285,19 @@ void Ekf::controlMagFusion()
// If we are flying a vehicle that flies forward, eg plane, then we can use the GPS course to check and correct the heading
if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
_flt_mag_align_complete = realignYawGPS();
if (_velpos_reset_request) {
resetVelocity();
resetPosition();
_velpos_reset_request = false;
}
} else {
_flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag);
}
_control_status.flags.yaw_align = _control_status.flags.yaw_align || _flt_mag_align_complete;
} else {
// reset the mag field covariances
zeroRows(P, 16, 21);
@@ -1252,7 +1305,7 @@ void Ekf::controlMagFusion()
// re-instate the last used variances
for (uint8_t index = 0; index <= 5; index ++) {
P[index+16][index+16] = _saved_mag_variance[index];
P[index + 16][index + 16] = _saved_mag_variance[index];
}
}
}
@@ -1265,10 +1318,12 @@ void Ekf::controlMagFusion()
// save magnetic field state variances for next time
if (_control_status.flags.mag_3D) {
for (uint8_t index = 0; index <= 5; index ++) {
_saved_mag_variance[index] = P[index+16][index+16];
_saved_mag_variance[index] = P[index + 16][index + 16];
}
_control_status.flags.mag_3D = false;
}
_control_status.flags.mag_hdg = true;
}
@@ -1291,8 +1346,9 @@ void Ekf::controlMagFusion()
// the covariance terms to allow the field states re-learn rapidly
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
for (uint8_t index = 0; index <= 5; index ++) {
P[index+16][index+16] = sq(_params.mag_noise);
P[index + 16][index + 16] = sq(_params.mag_noise);
}
}
@@ -1351,10 +1407,12 @@ void Ekf::controlVelPosFusion()
if (!(_params.fusion_mode & MASK_USE_GPS)) {
_control_status.flags.gps = false;
}
if (!_control_status.flags.gps &&
!_control_status.flags.opt_flow &&
!_control_status.flags.ev_pos &&
!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) {
!_control_status.flags.opt_flow &&
!_control_status.flags.ev_pos &&
!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) {
// We now need to use a synthetic positon observation to prevent unconstrained drift of the INS states.
_using_synthetic_position = true;
@@ -1365,6 +1423,7 @@ void Ekf::controlVelPosFusion()
resetPosition();
resetVelocity();
_fuse_hpos_as_odom = false;
if (_time_last_fake_gps != 0) {
ECL_WARN("EKF stopping navigation");
}
@@ -1378,9 +1437,11 @@ void Ekf::controlVelPosFusion()
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
_posObsNoiseNE = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
} else {
_posObsNoiseNE = 0.5f;
}
_vel_pos_innov[0] = 0.0f;
_vel_pos_innov[1] = 0.0f;
_vel_pos_innov[2] = 0.0f;
@@ -1391,6 +1452,7 @@ void Ekf::controlVelPosFusion()
_posInnovGateNE = 100.0f;
}
} else {
_using_synthetic_position = false;
}
EKF/covariance.cpp
+50 -29
View File
@@ -72,12 +72,15 @@ void Ekf::initialiseCovariance()
// position
P[7][7] = sq(fmaxf(_params.gps_pos_noise, 0.01f));
P[8][8] = P[7][7];
if (_control_status.flags.rng_hgt) {
P[9][9] = sq(fmaxf(_params.range_noise, 0.01f));
} else if (_control_status.flags.gps_hgt) {
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
P[9][9] = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
} else {
P[9][9] = sq(fmaxf(_params.baro_noise, 0.01f));
}
@@ -165,32 +168,38 @@ void Ekf::predictCovariance()
_accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt);
if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim
|| _accel_mag_filt > _params.acc_bias_learn_acc_lim
|| _bad_vert_accel_detected) {
|| _accel_mag_filt > _params.acc_bias_learn_acc_lim
|| _bad_vert_accel_detected) {
// store the bias state variances to be reinstated later
if (!_accel_bias_inhibit) {
_prev_dvel_bias_var(0) = P[13][13];
_prev_dvel_bias_var(1) = P[14][14];
_prev_dvel_bias_var(2) = P[15][15];
}
_accel_bias_inhibit = true;
} else {
if (_accel_bias_inhibit) {
// reinstate the bias state variances
P[13][13] = _prev_dvel_bias_var(0);
P[14][14] = _prev_dvel_bias_var(1);
P[15][15] = _prev_dvel_bias_var(2);
} else {
// store the bias state variances to be reinstated later
_prev_dvel_bias_var(0) = P[13][13];
_prev_dvel_bias_var(1) = P[14][14];
_prev_dvel_bias_var(2) = P[15][15];
}
_accel_bias_inhibit = false;
}
// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
float mag_I_sig;
if (_control_status.flags.mag_3D && (P[16][16] + P[17][17] + P[18][18]) < 0.1f) {
mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
@@ -200,6 +209,7 @@ void Ekf::predictCovariance()
// Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
float mag_B_sig;
if (_control_status.flags.mag_3D && (P[19][19] + P[20][20] + P[21][21]) < 0.1f) {
mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);
@@ -222,6 +232,7 @@ void Ekf::predictCovariance()
for (unsigned i = 0; i <= 9; i++) {
process_noise[i] = 0.0f;
}
// delta angle bias states
process_noise[12] = process_noise[11] = process_noise[10] = sq(d_ang_bias_sig);
// delta_velocity bias states
@@ -240,11 +251,13 @@ void Ekf::predictCovariance()
float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f);
daxVar = dayVar = dazVar = sq(dt * gyro_noise);
float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f);
if (_bad_vert_accel_detected) {
// Increase accelerometer process noise if bad accel data is detected. Measurement errors due to
// vibration induced clipping commonly reach an equivalent 0.5g offset.
accel_noise = BADACC_BIAS_PNOISE;
}
dvxVar = dvyVar = dvzVar = sq(dt * accel_noise);
// predict the covariance
@@ -467,8 +480,8 @@ void Ekf::predictCovariance()
} else {
// Inhibit delta velocity bias learning by zeroing the covariance terms
zeroRows(nextP,13,15);
zeroCols(nextP,13,15);
zeroRows(nextP, 13, 15);
zeroCols(nextP, 13, 15);
}
// Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion
@@ -725,15 +738,16 @@ void Ekf::fixCovarianceErrors()
}
// force symmetry on the quaternion, velocity and positon state covariances
makeSymmetrical(P,0,12);
makeSymmetrical(P, 0, 12);
// the following states are optional and are deactivaed when not required
// by ensuring the corresponding covariance matrix values are kept at zero
// accelerometer bias states
if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) || _accel_bias_inhibit) {
zeroRows(P,13,15);
zeroCols(P,13,15);
zeroRows(P, 13, 15);
zeroCols(P, 13, 15);
} else {
// Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum
const float minSafeStateVar = 1e-9f;
@@ -795,6 +809,7 @@ void Ekf::fixCovarianceErrors()
if (!bad_acc_bias) {
_fault_status.flags.bad_acc_bias = false;
_time_acc_bias_check = _time_last_imu;
} else {
_fault_status.flags.bad_acc_bias = true;
}
@@ -805,63 +820,69 @@ void Ekf::fixCovarianceErrors()
float varX = P[13][13];
float varY = P[14][14];
float varZ = P[15][15];
zeroRows(P,13,15);
zeroCols(P,13,15);
zeroRows(P, 13, 15);
zeroCols(P, 13, 15);
P[13][13] = varX;
P[14][14] = varY;
P[15][15] = varZ;
_time_acc_bias_check = _time_last_imu;
_fault_status.flags.bad_acc_bias = false;
ECL_WARN("EKF invalid accel bias - resetting covariance");
} else {
// ensure the covariance values are symmetrical
makeSymmetrical(P,13,15);
makeSymmetrical(P, 13, 15);
}
}
// magnetic field states
if (!_control_status.flags.mag_3D) {
zeroRows(P,16,21);
zeroCols(P,16,21);
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
} else {
// constrain variances
for (int i = 16; i <= 18; i++) {
P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[5]);
}
for (int i = 19; i <= 21; i++) {
P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[6]);
}
// force symmetry
makeSymmetrical(P,16,21);
makeSymmetrical(P, 16, 21);
}
// wind velocity states
if (!_control_status.flags.wind) {
zeroRows(P,22,23);
zeroCols(P,22,23);
zeroRows(P, 22, 23);
zeroCols(P, 22, 23);
} else {
// constrain variances
for (int i = 22; i <= 23; i++) {
P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[7]);
}
// force symmetry
makeSymmetrical(P,22,23);
makeSymmetrical(P, 22, 23);
}
}
void Ekf::resetMagCovariance()
{
// set the quaternion covariance terms to zero
zeroRows(P,0,3);
zeroCols(P,0,3);
zeroRows(P, 0, 3);
zeroCols(P, 0, 3);
// set the magnetic field covariance terms to zero
zeroRows(P,16,21);
zeroCols(P,16,21);
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
// set the field state variance to the observation variance
for (uint8_t rc_index=16; rc_index <= 21; rc_index ++) {
for (uint8_t rc_index = 16; rc_index <= 21; rc_index ++) {
P[rc_index][rc_index] = sq(_params.mag_noise);
}
}
@@ -869,15 +890,15 @@ void Ekf::resetMagCovariance()
void Ekf::resetWindCovariance()
{
// set the wind covariance terms to zero
zeroRows(P,22,23);
zeroCols(P,22,23);
zeroRows(P, 22, 23);
zeroCols(P, 22, 23);
if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
// Use airspeed and zer sideslip assumption to set initial covariance values for wind states
// calculate the wind speed and bearing
float spd = sqrtf(sq(_state.wind_vel(0))+sq(_state.wind_vel(1)));
float yaw = atan2f(_state.wind_vel(1),_state.wind_vel(0));
float spd = sqrtf(sq(_state.wind_vel(0)) + sq(_state.wind_vel(1)));
float yaw = atan2f(_state.wind_vel(1), _state.wind_vel(0));
// calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle
// used to calculate the initial wind speed
@@ -889,12 +910,12 @@ void Ekf::resetWindCovariance()
float sin_yaw = sinf(yaw);
float cos_yaw_2 = sq(cos_yaw);
float sin_yaw_2 = sq(sin_yaw);
float sin_cos_yaw = sin_yaw*cos_yaw;
float sin_cos_yaw = sin_yaw * cos_yaw;
float spd_2 = sq(spd);
P[22][22] = R_yaw*spd_2*sin_yaw_2 + R_spd*cos_yaw_2;
P[22][23] = - R_yaw*sin_cos_yaw*spd_2 + R_spd*sin_cos_yaw;
P[22][22] = R_yaw * spd_2 * sin_yaw_2 + R_spd * cos_yaw_2;
P[22][23] = - R_yaw * sin_cos_yaw * spd_2 + R_spd * sin_cos_yaw;
P[23][22] = P[22][23];
P[23][23] = R_yaw*spd_2*cos_yaw_2 + R_spd*sin_yaw_2;
P[23][23] = R_yaw * spd_2 * cos_yaw_2 + R_spd * sin_yaw_2;
// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
P[22][22] += P[4][4];
EKF/drag_fusion.cpp
+7 -2
View File
@@ -57,6 +57,7 @@ void Ekf::fuseDrag()
if (_params.bcoef_x < 1.0f || _params.bcoef_y < 1.0f) {
return;
}
float BC_inv_x = 1.0f / _params.bcoef_x;
float BC_inv_y = 1.0f / _params.bcoef_y;
@@ -149,6 +150,7 @@ void Ekf::fuseDrag()
// calculate the predicted acceleration and innovation measured along the X body axis
float drag_sign;
if (rel_wind(axis_index) >= 0.0f) {
drag_sign = 1.0f;
@@ -223,6 +225,7 @@ void Ekf::fuseDrag()
// calculate the predicted acceleration and innovation measured along the Y body axis
float drag_sign;
if (rel_wind(axis_index) >= 0.0f) {
drag_sign = 1.0f;
@@ -243,6 +246,7 @@ void Ekf::fuseDrag()
// then calculate P - KHP
float KHP[_k_num_states][_k_num_states];
float KH[9];
for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion[row] * H_ACC[0];
@@ -272,12 +276,13 @@ void Ekf::fuseDrag()
// if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected
bool healthy = true;
//_fault_status.flags.bad_sideslip = false;
for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P,i,i);
zeroCols(P,i,i);
zeroRows(P, i, i);
zeroCols(P, i, i);
//flag as unhealthy
healthy = false;
EKF/ekf.cpp
+14 -10
View File
@@ -139,7 +139,7 @@ bool Ekf::initialiseFilter()
// Keep accumulating measurements until we have a minimum of 10 samples for the required sensors
// Sum the IMU delta angle measurements
const imuSample& imu_init = _imu_buffer.get_newest();
const imuSample &imu_init = _imu_buffer.get_newest();
_delVel_sum += imu_init.delta_vel;
// Sum the magnetometer measurements
@@ -191,7 +191,8 @@ bool Ekf::initialiseFilter()
// accumulate enough height measurements to be confident in the qulaity of the data
if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS ||
_primary_hgt_source == VDIST_SENSOR_RANGE || _primary_hgt_source == VDIST_SENSOR_EV) {
_primary_hgt_source == VDIST_SENSOR_RANGE || _primary_hgt_source == VDIST_SENSOR_EV) {
// if the user parameter specifies use of GPS/range/EV finder for height we use baro height initially and switch to GPS/range/EV finder
// later when it passes checks.
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
@@ -275,14 +276,16 @@ bool Ekf::initialiseFilter()
_control_status.flags.yaw_align = resetMagHeading(mag_init);
// initialise the rotation from EV to EKF navigation frame if required
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS)
&& !(_params.fusion_mode & MASK_USE_EVYAW)) {
resetExtVisRotMat();
}
if (_control_status.flags.rng_hgt) {
// if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup
// so it can be used as a backup ad set the initial height using the range finder
const baroSample& baro_newest = _baro_buffer.get_newest();
const baroSample &baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt;
_state.pos(2) = -math::max(_rng_filt_state * _R_rng_to_earth_2_2, _params.rng_gnd_clearance);
ECL_INFO("EKF using range finder height - commencing alignment");
@@ -443,7 +446,7 @@ bool Ekf::collect_imu(imuSample &imu)
void Ekf::calculateOutputStates()
{
// Use full rate IMU data at the current time horizon
const imuSample& imu_new = _imu_sample_new;
const imuSample &imu_new = _imu_sample_new;
// correct delta angles for bias offsets
Vector3f delta_angle;
@@ -453,9 +456,9 @@ void Ekf::calculateOutputStates()
delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2) * dt_scale_correction;
// calculate a yaw change about the earth frame vertical
float spin_del_ang_D = _R_to_earth_now(2,0) * delta_angle(0) +
_R_to_earth_now(2,1) * delta_angle(1) +
_R_to_earth_now(2,2) * delta_angle(2);
float spin_del_ang_D = _R_to_earth_now(2, 0) * delta_angle(0) +
_R_to_earth_now(2, 1) * delta_angle(1) +
_R_to_earth_now(2, 2) * delta_angle(2);
_yaw_delta_ef += spin_del_ang_D;
// Calculate filtered yaw rate to be used by the magnetomer fusion type selection logic
@@ -615,13 +618,14 @@ void Ekf::calculateOutputStates()
for (uint8_t counter = 0; counter < (size - 1); counter++) {
const uint8_t index_next = (index + 1) % size;
outputVert& current_state = _output_vert_buffer[index];
outputVert& next_state = _output_vert_buffer[index_next];
outputVert &current_state = _output_vert_buffer[index];
outputVert &next_state = _output_vert_buffer[index_next];
// correct the velocity
if (counter == 0) {
current_state.vel_d += vel_d_correction;
}
next_state.vel_d += vel_d_correction;
// position is propagated forward using the corrected velocity and a trapezoidal integrator
EKF/ekf_helper.cpp
+38 -23
View File
@@ -58,7 +58,7 @@ bool Ekf::resetVelocity()
_state.vel = _gps_sample_delayed.vel;
// use GPS accuracy to reset variances
setDiag(P,4,6,sq(_gps_sample_delayed.sacc));
setDiag(P, 4, 6, sq(_gps_sample_delayed.sacc));
} else if (_control_status.flags.opt_flow) {
// constrain height above ground to be above minimum possible
@@ -89,21 +89,21 @@ bool Ekf::resetVelocity()
}
// reset the velocity covariance terms
zeroRows(P,4,5);
zeroCols(P,4,5);
zeroRows(P, 4, 5);
zeroCols(P, 4, 5);
// reset the horizontal velocity variance using the optical flow noise variance
P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
} else if (_control_status.flags.ev_pos) {
_state.vel.setZero();
zeroOffDiag(P,4,6);
zeroOffDiag(P, 4, 6);
} else {
// Used when falling back to non-aiding mode of operation
_state.vel(0) = 0.0f;
_state.vel(1) = 0.0f;
setDiag(P,4,5,25.0f);
setDiag(P, 4, 5, 25.0f);
}
// calculate the change in velocity and apply to the output predictor state history
@@ -145,7 +145,7 @@ bool Ekf::resetPosition()
_state.pos(1) = _gps_sample_delayed.pos(1);
// use GPS accuracy to reset variances
setDiag(P,7,8,sq(_gps_sample_delayed.hacc));
setDiag(P, 7, 8, sq(_gps_sample_delayed.hacc));
} else if (_control_status.flags.ev_pos) {
// this reset is only called if we have new ev data at the fusion time horizon
@@ -153,7 +153,7 @@ bool Ekf::resetPosition()
_state.pos(1) = _ev_sample_delayed.posNED(1);
// use EV accuracy to reset variances
setDiag(P,7,8,sq(_ev_sample_delayed.posErr));
setDiag(P, 7, 8, sq(_ev_sample_delayed.posErr));
} else if (_control_status.flags.opt_flow) {
if (!_control_status.flags.in_air) {
@@ -167,13 +167,14 @@ bool Ekf::resetPosition()
_state.pos(1) = _last_known_posNE(1);
}
setDiag(P,7,8,sq(_params.pos_noaid_noise));
setDiag(P, 7, 8, sq(_params.pos_noaid_noise));
} else {
// Used when falling back to non-aiding mode of operation
_state.pos(0) = _last_known_posNE(0);
_state.pos(1) = _last_known_posNE(1);
setDiag(P,7,8,sq(_params.pos_noaid_noise));
setDiag(P, 7, 8, sq(_params.pos_noaid_noise));
}
// calculate the change in position and apply to the output predictor state history
@@ -200,7 +201,7 @@ bool Ekf::resetPosition()
void Ekf::resetHeight()
{
// Get the most recent GPS data
const gpsSample& gps_newest = _gps_buffer.get_newest();
const gpsSample &gps_newest = _gps_buffer.get_newest();
// store the current vertical position and velocity for reference so we can calculate and publish the reset amount
float old_vert_pos = _state.pos(2);
@@ -233,7 +234,7 @@ void Ekf::resetHeight()
vert_pos_reset = true;
// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
const baroSample& baro_newest = _baro_buffer.get_newest();
const baroSample &baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
} else {
@@ -243,7 +244,7 @@ void Ekf::resetHeight()
} else if (_control_status.flags.baro_hgt) {
// initialize vertical position with newest baro measurement
const baroSample& baro_newest = _baro_buffer.get_newest();
const baroSample &baro_newest = _baro_buffer.get_newest();
if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) {
_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
@@ -276,7 +277,7 @@ void Ekf::resetHeight()
vert_pos_reset = true;
// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
const baroSample& baro_newest = _baro_buffer.get_newest();
const baroSample &baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
} else {
@@ -285,7 +286,7 @@ void Ekf::resetHeight()
} else if (_control_status.flags.ev_hgt) {
// initialize vertical position with newest measurement
const extVisionSample& ev_newest = _ext_vision_buffer.get_newest();
const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();
// use the most recent data if it's time offset from the fusion time horizon is smaller
int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
@@ -365,6 +366,7 @@ void Ekf::resetHeight()
_output_vert_delayed.vel_d_integ = _state.pos(2);
_output_vert_new.vel_d_integ = _state.pos(2);
}
if (vert_vel_reset) {
_output_vert_delayed.vel_d = _state.vel(2);
_output_vert_new.vel_d = _state.vel(2);
@@ -398,6 +400,7 @@ bool Ekf::realignYawGPS()
{
// Need at least 5 m/s of GPS horizontal speed and ratio of velocity error to velocity < 0.15 for a reliable alignment
float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1)));
if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) {
// check for excessive GPS velocity innovations
bool badVelInnov = ((_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f)) && _control_status.flags.gps;
@@ -452,11 +455,12 @@ bool Ekf::realignYawGPS()
} else if (_control_status.flags.wind) {
// we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is
// aligned with the wind relative GPS velocity vector
euler321(2) = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)) , (_gps_sample_delayed.vel(0) - _state.wind_vel(0)));
euler321(2) = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)),
(_gps_sample_delayed.vel(0) - _state.wind_vel(0)));
} else {
// we don't have wind estimates, so align yaw to the GPS velocity vector
euler321(2) = atan2f(_gps_sample_delayed.vel(1) , _gps_sample_delayed.vel(0));
euler321(2) = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
}
@@ -699,7 +703,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// reset the rotation from the EV to EKF frame of reference if it is being used
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS)
&& !(_params.fusion_mode & MASK_USE_EVYAW)) {
resetExtVisRotMat();
}
@@ -741,6 +747,7 @@ void Ekf::calcMagDeclination()
if (_flt_mag_align_complete) {
// Use value consistent with earth field state
_mag_declination = atan2f(_state.mag_I(1), _state.mag_I(0));
} else if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
// use parameter value until GPS is available, then use value returned by geo library
if (_NED_origin_initialised) {
@@ -1055,10 +1062,11 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl)
// If relying on optical flow for navigation we need to keep within flow and range sensor limits
bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos);
if (relying_on_optical_flow) {
// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
flow_gnd_spd_max = 0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2));
flow_gnd_spd_max = fmaxf(flow_gnd_spd_max , 0.0f);
flow_gnd_spd_max = fmaxf(flow_gnd_spd_max, 0.0f);
flow_limit_hagl = true;
@@ -1136,7 +1144,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow ) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
soln_status.flags.pos_vert_agl = get_terrain_valid();
@@ -1214,6 +1222,7 @@ void Ekf::zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t fi
// save diagonal elements
uint8_t row;
float variances[_k_num_states];
for (row = first; row <= last; row++) {
variances[row] = cov_mat[row][row];
}
@@ -1236,6 +1245,7 @@ void Ekf::setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first,
// set diagonals
uint8_t row;
for (row = first; row <= last; row++) {
cov_mat[row][row] = variance;
}
@@ -1268,7 +1278,7 @@ void Ekf::update_deadreckoning_status()
}
// report if we have been deadreckoning for too long
_deadreckon_time_exceeded = ((_time_last_imu - _time_ins_deadreckon_start) > (unsigned)_params.valid_timeout_max);
_deadreckon_time_exceeded = ((_time_last_imu - _time_ins_deadreckon_start) > (unsigned)_params.valid_timeout_max);
}
// perform a vector cross product
@@ -1426,8 +1436,8 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
float t44 = t17-t36;
// zero all the quaternion covariances
zeroRows(P,0,3);
zeroCols(P,0,3);
zeroRows(P, 0, 3);
zeroCols(P, 0, 3);
// Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox
P[0][0] = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f;
@@ -1518,18 +1528,20 @@ void Ekf::calcExtVisRotMat()
Vector3f rot_vec = q_error.to_axis_angle();
float rot_vec_norm = rot_vec.norm();
if (rot_vec_norm > 1e-6f) {
// apply an input limiter to protect from spikes
Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt;
float input_delta_mag = _input_delta_vec.norm();
if (input_delta_mag > 0.1f) {
rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_mag);
}
// Apply a first order IIR low pass filter
const float omega_lpf_us = 0.2e-6f; // cutoff frequency in rad/uSec
float alpha = math::constrain(omega_lpf_us * (float)(_time_last_imu - _ev_rot_last_time_us) , 0.0f , 1.0f);
float alpha = math::constrain(omega_lpf_us * (float)(_time_last_imu - _ev_rot_last_time_us), 0.0f, 1.0f);
_ev_rot_last_time_us = _time_last_imu;
_ev_rot_vec_filt = _ev_rot_vec_filt * (1.0f - alpha) + rot_vec * alpha;
@@ -1554,8 +1566,10 @@ void Ekf::resetExtVisRotMat()
Vector3f rot_vec = q_error.to_axis_angle();
float rot_vec_norm = rot_vec.norm();
if (rot_vec_norm > 1e-9f) {
_ev_rot_vec_filt = rot_vec;
} else {
_ev_rot_vec_filt.zero();
}
@@ -1569,6 +1583,7 @@ void Ekf::get_ekf2ev_quaternion(float *quat)
{
Quatf quat_ekf2ev;
quat_ekf2ev.from_axis_angle(_ev_rot_vec_filt);
for (unsigned i = 0; i < 4; i++) {
quat[i] = quat_ekf2ev(i);
}
EKF/estimator_interface.cpp
+12 -1
View File
@@ -108,6 +108,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
// Do not retry if allocation has failed previously
if (_drag_buffer.get_length() < _obs_buffer_length) {
_drag_buffer_fail = !_drag_buffer.allocate(_obs_buffer_length);
if (_drag_buffer_fail) {
ECL_ERR("EKF drag buffer allocation failed");
return;
@@ -163,6 +164,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
// Do not retry if allocation has failed previously
if (_mag_buffer.get_length() < _obs_buffer_length) {
_mag_buffer_fail = !_mag_buffer.allocate(_obs_buffer_length);
if (_mag_buffer_fail) {
ECL_ERR("EKF mag buffer allocation failed");
return;
@@ -194,6 +196,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
// Do not retry if allocation has failed previously
if (_gps_buffer.get_length() < _obs_buffer_length) {
_gps_buffer_fail = !_gps_buffer.allocate(_obs_buffer_length);
if (_gps_buffer_fail) {
ECL_ERR("EKF GPS buffer allocation failed");
return;
@@ -247,6 +250,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
// Do not retry if allocation has failed previously
if (_baro_buffer.get_length() < _obs_buffer_length) {
_baro_buffer_fail = !_baro_buffer.allocate(_obs_buffer_length);
if (_baro_buffer_fail) {
ECL_ERR("EKF baro buffer allocation failed");
return;
@@ -279,6 +283,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
// Do not retry if allocation has failed previously
if (_airspeed_buffer.get_length() < _obs_buffer_length) {
_airspeed_buffer_fail = !_airspeed_buffer.allocate(_obs_buffer_length);
if (_airspeed_buffer_fail) {
ECL_ERR("EKF airspeed buffer allocation failed");
return;
@@ -308,6 +313,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data)
// Do not retry if allocation has failed previously
if (_range_buffer.get_length() < _obs_buffer_length) {
_range_buffer_fail = !_range_buffer.allocate(_obs_buffer_length);
if (_range_buffer_fail) {
ECL_ERR("EKF range finder buffer allocation failed");
return;
@@ -336,6 +342,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
// Do not retry if allocation has failed previously
if (_flow_buffer.get_length() < _obs_buffer_length) {
_flow_buffer_fail = !_flow_buffer.allocate(_obs_buffer_length);
if (_flow_buffer_fail) {
ECL_ERR("EKF optical flow buffer allocation failed");
return;
@@ -349,6 +356,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
float delta_time = 1e-6f * (float)flow->dt;
float delta_time_min = 5e-7f * (float)_min_obs_interval_us;
bool delta_time_good = delta_time >= delta_time_min;
if (!delta_time_good) {
// protect against overflow casued by division with very small delta_time
delta_time = delta_time_min;
@@ -415,6 +423,7 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
// Do not retry if allocation has failed previously
if (_ext_vision_buffer.get_length() < _obs_buffer_length) {
_ev_buffer_fail = !_ext_vision_buffer.allocate(_obs_buffer_length);
if (_ev_buffer_fail) {
ECL_ERR("EKF external vision buffer allocation failed");
return;
@@ -451,6 +460,7 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, float (&data)[2], flo
// Do not retry if allocation has failed previously
if (_auxvel_buffer.get_length() < _obs_buffer_length) {
_auxvel_buffer_fail = !_auxvel_buffer.allocate(_obs_buffer_length);
if (_auxvel_buffer_fail) {
ECL_ERR("EKF aux vel buffer allocation failed");
return;
@@ -553,7 +563,8 @@ bool EstimatorInterface::local_position_is_valid()
return !_deadreckon_time_exceeded;
}
void EstimatorInterface::print_status() {
void EstimatorInterface::print_status()
{
ECL_INFO("local position valid: %s", local_position_is_valid() ? "yes" : "no");
ECL_INFO("global position valid: %s", global_position_is_valid() ? "yes" : "no");
EKF/estimator_interface.h
+1
View File
@@ -262,6 +262,7 @@ public:
void get_velocity(float *vel)
{
Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned;
for (unsigned i = 0; i < 3; i++) {
vel[i] = vel_earth(i);
}
EKF/gps_checks.cpp
+3
View File
@@ -62,6 +62,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
// Also run checks if the vehicle is on-ground as the check data can be used by vehicle pre-flight checks
if (!_control_status.flags.in_air || !_NED_origin_initialised || !_control_status.flags.gps) {
bool gps_checks_pass = gps_is_good(gps);
if (!_NED_origin_initialised && gps_checks_pass) {
// If we have good GPS data set the origin's WGS-84 position to the last gps fix
double lat = gps->lat / 1.0e7;
@@ -93,6 +94,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
_control_status.flags.rng_hgt = false;
// zero the sensor offset
_hgt_sensor_offset = 0.0f;
} else {
ECL_INFO("EKF GPS checks passed (WGS-84 origin set)");
}
@@ -139,6 +141,7 @@ bool Ekf::gps_is_good(struct gps_message *gps)
// calculate position movement since last GPS fix
if (_gps_pos_prev.timestamp > 0) {
map_projection_project(&_gps_pos_prev, lat, lon, &delta_posN, &delta_PosE);
} else {
// no previous position has been set
map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
EKF/mag_fusion.cpp
+23 -6
View File
@@ -146,11 +146,14 @@ void Ekf::fuseMag()
// Perform an innovation consistency check and report the result
bool healthy = true;
for (uint8_t index = 0; index <= 2; index++) {
_mag_test_ratio[index] = sq(_mag_innov[index]) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var[index]);
if (_mag_test_ratio[index] > 1.0f) {
healthy = false;
_innov_check_fail_status.value |= (1 << (index + 3));
} else {
_innov_check_fail_status.value &= ~(1 << (index + 3));
}
@@ -213,6 +216,7 @@ void Ekf::fuseMag()
for (uint8_t i = 0; i < 16; i++) {
Kfusion[i] = 0.0f;
}
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
@@ -267,6 +271,7 @@ void Ekf::fuseMag()
for (uint8_t i = 0; i < 16; i++) {
Kfusion[i] = 0.0f;
}
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
@@ -321,6 +326,7 @@ void Ekf::fuseMag()
for (uint8_t i = 0; i < 16; i++) {
Kfusion[i] = 0.0f;
}
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
@@ -341,6 +347,7 @@ void Ekf::fuseMag()
// then calculate P - KHP
float KHP[_k_num_states][_k_num_states];
float KH[10];
for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion[row] * H_MAG[0];
@@ -374,11 +381,12 @@ void Ekf::fuseMag()
_fault_status.flags.bad_mag_x = false;
_fault_status.flags.bad_mag_y = false;
_fault_status.flags.bad_mag_z = false;
for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P,i,i);
zeroCols(P,i,i);
zeroRows(P, i, i);
zeroCols(P, i, i);
//flag as unhealthy
healthy = false;
@@ -386,8 +394,10 @@ void Ekf::fuseMag()
// update individual measurement health status
if (index == 0) {
_fault_status.flags.bad_mag_x = true;
} else if (index == 1) {
_fault_status.flags.bad_mag_y = true;
} else if (index == 2) {
_fault_status.flags.bad_mag_z = true;
}
@@ -473,6 +483,7 @@ void Ekf::fuseHeading()
if (_control_status.flags.mag_3D) {
// don't apply bias corrections if we are learning them
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
} else {
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
}
@@ -512,6 +523,7 @@ void Ekf::fuseHeading()
float t12 = t8*t11*4.0f;
float t13 = t12+1.0f;
float t14;
if (fabsf(t13) > 1e-6f) {
t14 = 1.0f/t13;
} else {
@@ -590,9 +602,11 @@ void Ekf::fuseHeading()
if (_control_status.flags.mag_hdg) {
// using magnetic heading tuning parameter
R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
} else if (_control_status.flags.ev_yaw) {
// using error estimate from external vision data
R_YAW = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
} else {
// there is no yaw observation
return;
@@ -602,6 +616,7 @@ void Ekf::fuseHeading()
// calculate the innovaton variance
float PH[4];
_heading_innov_var = R_YAW;
for (unsigned row = 0; row <= 3; row++) {
PH[row] = 0.0f;
@@ -696,6 +711,7 @@ void Ekf::fuseHeading()
// then calculate P - KHP
float KHP[_k_num_states][_k_num_states];
float KH[4];
for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion[row] * H_YAW[0];
@@ -716,11 +732,12 @@ void Ekf::fuseHeading()
// the covariance marix is unhealthy and must be corrected
bool healthy = true;
_fault_status.flags.bad_mag_hdg = false;
for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P,i,i);
zeroCols(P,i,i);
zeroRows(P, i, i);
zeroCols(P, i, i);
//flag as unhealthy
healthy = false;
@@ -853,8 +870,8 @@ void Ekf::fuseDeclination()
for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P,i,i);
zeroCols(P,i,i);
zeroRows(P, i, i);
zeroCols(P, i, i);
//flag as unhealthy
healthy = false;
EKF/optflow_fusion.cpp
+7 -2
View File
@@ -213,6 +213,7 @@ void Ekf::fuseOptFlow()
float t92 = t2*t7*t69;
float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
float t78;
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if (t77 >= R_LOS) {
t78 = 1.0f / t77;
@@ -403,6 +404,7 @@ void Ekf::fuseOptFlow()
// record the innovation test pass/fail
bool flow_fail = false;
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
if (optflow_test_ratio[obs_index] > 1.0f) {
flow_fail = true;
@@ -434,6 +436,7 @@ void Ekf::fuseOptFlow()
// then calculate P - KHP
float KHP[_k_num_states][_k_num_states];
float KH[7];
for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = gain[row] * H_LOS[obs_index][0];
@@ -461,11 +464,12 @@ void Ekf::fuseOptFlow()
bool healthy = true;
_fault_status.flags.bad_optflow_X = false;
_fault_status.flags.bad_optflow_Y = false;
for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P,i,i);
zeroCols(P,i,i);
zeroRows(P, i, i);
zeroCols(P, i, i);
//flag as unhealthy
healthy = false;
@@ -473,6 +477,7 @@ void Ekf::fuseOptFlow()
// update individual measurement health status
if (obs_index == 0) {
_fault_status.flags.bad_optflow_X = true;
} else if (obs_index == 1) {
_fault_status.flags.bad_optflow_Y = true;
}
EKF/terrain_estimator.cpp
+6 -3
View File
@@ -46,7 +46,7 @@
bool Ekf::initHagl()
{
// get most recent range measurement from buffer
const rangeSample& latest_measurement = _range_buffer.get_newest();
const rangeSample &latest_measurement = _range_buffer.get_newest();
if ((_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
@@ -154,6 +154,7 @@ void Ekf::fuseHagl()
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;
}
@@ -170,15 +171,17 @@ void Ekf::fuseHagl()
// return true if the terrain height estimate is valid
bool Ekf::get_terrain_valid()
{
return _hagl_valid;
return _hagl_valid;
}
// determine terrain validity
void Ekf::update_terrain_valid()
{
if (_terrain_initialised && _range_data_continuous && !_control_status.flags.rng_stuck &&
(_time_last_imu - _time_last_hagl_fuse < (uint64_t)5e6)) {
(_time_last_imu - _time_last_hagl_fuse < (uint64_t)5e6)) {
_hagl_valid = true;
} else {
_hagl_valid = false;
}
EKF/vel_pos_fusion.cpp
+27 -3
View File
@@ -109,10 +109,12 @@ void Ekf::fuseVelPosHeight()
// casued by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
float deadzone_start = 0.25f * _params.baro_noise;
float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
if (_control_status.flags.gnd_effect) {
if (innovation[5] < -deadzone_start) {
if (innovation[5] <= -deadzone_end) {
innovation[5] += deadzone_end;
} else {
innovation[5] = -deadzone_start;
}
@@ -136,11 +138,12 @@ void Ekf::fuseVelPosHeight()
fuse_map[5] = true;
// use range finder with tilt correction
innovation[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
// observation variance - user parameter defined
R[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f);
// innovation gate size
gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
} else if (_control_status.flags.ev_hgt) {
fuse_map[5] = true;
// calculate the innovation assuming the external vision observaton is in local NED frame
@@ -183,31 +186,39 @@ void Ekf::fuseVelPosHeight()
if ((_fuse_hor_vel || _fuse_hor_vel_aux) && vel_check_pass) {
_time_last_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_vel_NED = false;
} else if (!vel_check_pass) {
_innov_check_fail_status.flags.reject_vel_NED = true;
}
_fuse_hor_vel = _fuse_hor_vel_aux = false;
// record the successful position fusion event
if (pos_check_pass && _fuse_pos) {
if (!_fuse_hpos_as_odom) {
_time_last_pos_fuse = _time_last_imu;
} else {
_time_last_delpos_fuse = _time_last_imu;
}
_innov_check_fail_status.flags.reject_pos_NE = false;
} else if (!pos_check_pass) {
_innov_check_fail_status.flags.reject_pos_NE = true;
}
_fuse_pos = false;
// record the successful height fusion event
if (innov_check_pass_map[5] && _fuse_height) {
_time_last_hgt_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_pos_D = false;
} else if (!innov_check_pass_map[5]) {
_innov_check_fail_status.flags.reject_pos_D = true;
}
_fuse_height = false;
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
@@ -225,6 +236,7 @@ void Ekf::fuseVelPosHeight()
// update covarinace matrix via Pnew = (I - KH)P
float KHP[_k_num_states][_k_num_states];
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
KHP[row][column] = Kfusion[row] * P[state_index][column];
@@ -234,11 +246,12 @@ void Ekf::fuseVelPosHeight()
// if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected
bool healthy = true;
for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P,i,i);
zeroCols(P,i,i);
zeroRows(P, i, i);
zeroCols(P, i, i);
//flag as unhealthy
healthy = false;
@@ -246,29 +259,40 @@ void Ekf::fuseVelPosHeight()
// update individual measurement health status
if (obs_index == 0) {
_fault_status.flags.bad_vel_N = true;
} else if (obs_index == 1) {
_fault_status.flags.bad_vel_E = true;
} else if (obs_index == 2) {
_fault_status.flags.bad_vel_D = true;
} else if (obs_index == 3) {
_fault_status.flags.bad_pos_N = true;
} else if (obs_index == 4) {
_fault_status.flags.bad_pos_E = true;
} else if (obs_index == 5) {
_fault_status.flags.bad_pos_D = true;
}
} else {
// update individual measurement health status
if (obs_index == 0) {
_fault_status.flags.bad_vel_N = false;
} else if (obs_index == 1) {
_fault_status.flags.bad_vel_E = false;
} else if (obs_index == 2) {
_fault_status.flags.bad_vel_D = false;
} else if (obs_index == 3) {
_fault_status.flags.bad_pos_N = false;
} else if (obs_index == 4) {
_fault_status.flags.bad_pos_E = false;
} else if (obs_index == 5) {
_fault_status.flags.bad_pos_D = false;
}
airdata/WindEstimator.cpp
+2 -2
View File
@@ -339,8 +339,8 @@ WindEstimator::run_sanity_checks()
}
bool
WindEstimator::check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, uint64_t &time_meas_rejected,
bool &reinit_filter)
WindEstimator::check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size,
uint64_t &time_meas_rejected, bool &reinit_filter)
{
if (innov * innov > gate_size * gate_size * innov_var) {
time_meas_rejected = time_meas_rejected == 0 ? time_now : time_meas_rejected;
airdata/WindEstimator.hpp
+2 -2
View File
@@ -68,8 +68,8 @@ public:
bool is_estimate_valid() { return _initialised; }
bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, uint64_t &time_meas_rejected,
bool &reinit_filter);
bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size,
uint64_t &time_meas_rejected, bool &reinit_filter);
float get_tas_scale() { return _state(tas); }
float get_tas_innov() { return _tas_innov; }
attitude_fw/ecl_roll_controller.cpp
+1 -1
View File
@@ -126,7 +126,7 @@ float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_d
_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(ctl_data);
}
attitude_fw/ecl_yaw_controller.cpp
+2 -2
View File
@@ -110,8 +110,8 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
if (!inverted) {
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed < ctl_data.airspeed_min ?
ctl_data.airspeed_min : ctl_data.airspeed);
_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed <
ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
}
if (!ISFINITE(_rate_setpoint)) {
validation/data_validator.cpp
+9 -6
View File
@@ -78,6 +78,7 @@ DataValidator::put(uint64_t timestamp, float val[dimensions], uint64_t error_cou
if (error_count_in > _error_count) {
_error_density += (error_count_in - _error_count);
} else if (_error_density > 0) {
_error_density--;
}
@@ -90,6 +91,7 @@ DataValidator::put(uint64_t timestamp, float val[dimensions], uint64_t error_cou
_mean[i] = 0;
_lp[i] = val[i];
_M2[i] = 0;
} else {
float lp_val = val[i] - _lp[i];
@@ -100,6 +102,7 @@ DataValidator::put(uint64_t timestamp, float val[dimensions], uint64_t error_cou
if (fabsf(_value[i] - val[i]) < 0.000001f) {
_value_equal_count++;
} else {
_value_equal_count = 0;
}
@@ -126,23 +129,23 @@ DataValidator::confidence(uint64_t timestamp)
_error_mask |= ERROR_FLAG_NO_DATA;
ret = 0.0f;
/* timed out - that's it */
} else if (timestamp - _time_last > _timeout_interval) {
/* timed out - that's it */
_error_mask |= ERROR_FLAG_TIMEOUT;
ret = 0.0f;
/* we got the exact same sensor value N times in a row */
} else if (_value_equal_count > _value_equal_count_threshold) {
/* we got the exact same sensor value N times in a row */
_error_mask |= ERROR_FLAG_STALE_DATA;
ret = 0.0f;
/* check error count limit */
} else if (_error_count > NORETURN_ERRCOUNT) {
/* check error count limit */
_error_mask |= ERROR_FLAG_HIGH_ERRCOUNT;
ret = 0.0f;
/* cap error density counter at window size */
} else if (_error_density > ERROR_DENSITY_WINDOW) {
/* cap error density counter at window size */
_error_mask |= ERROR_FLAG_HIGH_ERRDENSITY;
_error_density = ERROR_DENSITY_WINDOW;
@@ -171,7 +174,7 @@ DataValidator::print()
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(ecl_absolute_time()));
(double) _value[i], (double)_lp[i], (double)_mean[i],
(double)_rms[i], (double)confidence(ecl_absolute_time()));
}
}
validation/data_validator.h
+9 -8
View File
@@ -44,7 +44,8 @@
#include <cmath>
#include <stdint.h>
class DataValidator {
class DataValidator
{
public:
static const unsigned dimensions = 3;
@@ -70,13 +71,13 @@ public:
*
* @return the next sibling
*/
DataValidator* sibling() { return _sibling; }
DataValidator *sibling() { return _sibling; }
/**
* Set the sibling to the next node in the group
*
*/
void setSibling(DataValidator* new_sibling) { _sibling = new_sibling; }
void setSibling(DataValidator *new_sibling) { _sibling = new_sibling; }
/**
* Get the confidence of this validator
@@ -94,7 +95,7 @@ public:
* Get the values of this validator
* @return the stored value
*/
float* value() { return _value; }
float *value() { return _value; }
/**
* Get the used status of this validator
@@ -123,13 +124,13 @@ public:
* Get the RMS values of this validator
* @return the stored RMS
*/
float* rms() { return _rms; }
float *rms() { return _rms; }
/**
* Get the vibration offset
* @return the stored vibration offset
*/
float* vibration_offset() { return _vibe; }
float *vibration_offset() { return _vibe; }
/**
* Print the validator value
@@ -190,6 +191,6 @@ private:
static const constexpr unsigned VALUE_EQUAL_COUNT_DEFAULT = 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */
/* we don't want this class to be copied */
DataValidator(const DataValidator&) = delete;
DataValidator operator=(const DataValidator&) = delete;
DataValidator(const DataValidator &) = delete;
DataValidator operator=(const DataValidator &) = delete;
};
validation/data_validator_group.cpp
+33 -18
View File
@@ -56,17 +56,20 @@ DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
for (unsigned i = 0; i < siblings; i++) {
next = new DataValidator();
if(i == 0) {
if (i == 0) {
_first = next;
} else {
prev->setSibling(next);
}
prev = next;
}
_last = next;
if(_first) {
if (_first) {
_timeout_interval_us = _first->get_timeout();
}
}
@@ -74,7 +77,7 @@ DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
DataValidatorGroup::~DataValidatorGroup()
{
while (_first) {
DataValidator* next = _first->sibling();
DataValidator *next = _first->sibling();
delete (_first);
_first = next;
}
@@ -83,9 +86,11 @@ DataValidatorGroup::~DataValidatorGroup()
DataValidator *DataValidatorGroup::add_new_validator()
{
DataValidator *validator = new DataValidator();
if (!validator) {
return nullptr;
}
_last->setSibling(validator);
_last = validator;
_last->set_timeout(_timeout_interval_us);
@@ -101,6 +106,7 @@ DataValidatorGroup::set_timeout(uint32_t timeout_interval_us)
next->set_timeout(timeout_interval_us);
next = next->sibling();
}
_timeout_interval_us = timeout_interval_us;
}
@@ -127,12 +133,13 @@ DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64
next->put(timestamp, val, error_count, priority);
break;
}
next = next->sibling();
i++;
}
}
float*
float *
DataValidatorGroup::get_best(uint64_t timestamp, int *index)
{
DataValidator *next = _first;
@@ -162,9 +169,10 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
* 2) the confidence is no less than 1% different and the priority is higher
*/
if ((((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) ||
(confidence > max_confidence && (next->priority() >= max_priority)) ||
(fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))
) && (confidence > 0.0f)) {
(confidence > max_confidence && (next->priority() >= max_priority)) ||
(fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))
) && (confidence > 0.0f)) {
max_index = i;
max_confidence = confidence;
max_priority = next->priority();
@@ -183,7 +191,7 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
/* check whether the switch was a failsafe or preferring a higher priority sensor */
if (pre_check_prio != -1 && pre_check_prio < max_priority &&
fabsf(pre_check_confidence - max_confidence) < 0.1f) {
fabsf(pre_check_confidence - max_confidence) < 0.1f) {
/* this is not a failover */
true_failsafe = false;
/* reset error flags, this is likely a hotplug sensor coming online late */
@@ -193,6 +201,7 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
if (_curr_best < 0) {
_prev_best = max_index;
} else {
/* we were initialized before, this is a real failsafe */
_prev_best = pre_check_best;
@@ -210,6 +219,7 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
/* for all cases we want to keep a record of the best index */
_curr_best = max_index;
}
*index = max_index;
return (best) ? best->value() : nullptr;
}
@@ -225,7 +235,7 @@ DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
while (next != nullptr) {
if (next->confidence(timestamp) > 0.5f) {
float* rms = next->rms();
float *rms = next->rms();
for (unsigned j = 0; j < 3; j++) {
if (rms[j] > vibe) {
@@ -251,7 +261,7 @@ DataValidatorGroup::get_vibration_offset(uint64_t timestamp, int axis)
while (next != nullptr) {
if (next->confidence(timestamp) > 0.5f) {
float* vibration_offset = next->vibration_offset();
float *vibration_offset = next->vibration_offset();
if (vibe < 0.0f || vibration_offset[axis] < vibe) {
vibe = vibration_offset[axis];
@@ -269,8 +279,8 @@ 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);
_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
_toggle_count);
DataValidator *next = _first;
unsigned i = 0;
@@ -280,15 +290,16 @@ DataValidatorGroup::print()
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" : ""));
((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++;
}
@@ -310,9 +321,11 @@ DataValidatorGroup::failover_index()
if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
return i;
}
next = next->sibling();
i++;
}
return -1;
}
@@ -326,8 +339,10 @@ DataValidatorGroup::failover_state()
if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
return next->state();
}
next = next->sibling();
i++;
}
return DataValidator::ERROR_FLAG_NO_ERROR;
}
validation/data_validator_group.h
+6 -6
View File
@@ -43,7 +43,8 @@
#include "data_validator.h"
class DataValidatorGroup {
class DataValidatorGroup
{
public:
/**
* @param siblings initial number of DataValidator's. Must be > 0.
@@ -66,15 +67,14 @@ public:
* @param error_count The current error count of the sensor
* @param priority The priority of the sensor
*/
void put(unsigned index, uint64_t timestamp,
float val[3], uint64_t error_count, int priority);
void put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority);
/**
* Get the best data triplet of the group
*
* @return pointer to the array of best values
*/
float* get_best(uint64_t timestamp, int *index);
float *get_best(uint64_t timestamp, int *index);
/**
* Get the RMS / vibration factor
@@ -143,6 +143,6 @@ private:
static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f;
/* we don't want this class to be copied */
DataValidatorGroup(const DataValidatorGroup&);
DataValidatorGroup operator=(const DataValidatorGroup&);
DataValidatorGroup(const DataValidatorGroup &);
DataValidatorGroup operator=(const DataValidatorGroup &);
};