/**************************************************************************** * * Copyright (c) 2015-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /** * @file ekf_helper.cpp * Definition of ekf helper functions. * * @author Roman Bast * */ #include "ekf.h" #include #include #include bool Ekf::isHeightResetRequired() const { // check if height is continuously failing because of accel errors const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us); // check if height has been inertial deadreckoning for too long const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, _params.hgt_fusion_timeout_max); return (continuous_bad_accel_hgt || hgt_fusion_timeout); } Vector3f Ekf::calcEarthRateNED(float lat_rad) const { return Vector3f(CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad), 0.0f, -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad)); } void Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const { origin_time = _local_origin_lat_lon.getProjectionReferenceTimestamp(); latitude = _local_origin_lat_lon.getProjectionReferenceLat(); longitude = _local_origin_lat_lon.getProjectionReferenceLon(); origin_alt = getEkfGlobalOriginAltitude(); } bool Ekf::checkLatLonValidity(const double latitude, const double longitude) { const bool lat_valid = (PX4_ISFINITE(latitude) && (abs(latitude) <= 90)); const bool lon_valid = (PX4_ISFINITE(longitude) && (abs(longitude) <= 180)); return (lat_valid && lon_valid); } bool Ekf::checkAltitudeValidity(const float altitude) { // sanity check valid altitude anywhere between the Mariana Trench and edge of Space return (PX4_ISFINITE(altitude) && ((altitude > -12'000.f) && (altitude < 100'000.f))); } bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float hpos_var, const float vpos_var) { if (!setLatLonOrigin(latitude, longitude, hpos_var)) { return false; } // altitude is optional setAltOrigin(altitude, vpos_var); return true; } bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float hpos_var) { if (!checkLatLonValidity(latitude, longitude)) { return false; } if (!_local_origin_lat_lon.isInitialized() && isLocalHorizontalPositionValid()) { // Already navigating in a local frame, use the origin to initialize global position const Vector2f pos_prev = getLocalHorizontalPosition(); _local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us); double new_latitude; double new_longitude; _local_origin_lat_lon.reproject(pos_prev(0), pos_prev(1), new_latitude, new_longitude); resetHorizontalPositionTo(new_latitude, new_longitude, hpos_var); } else { // Simply move the origin and compute the change in local position const Vector2f pos_prev = getLocalHorizontalPosition(); _local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us); const Vector2f pos_new = getLocalHorizontalPosition(); const Vector2f delta_pos = pos_new - pos_prev; updateHorizontalPositionResetStatus(delta_pos); } return true; } bool Ekf::setAltOrigin(const float altitude, const float vpos_var) { if (!checkAltitudeValidity(altitude)) { return false; } ECL_INFO("EKF origin altitude %.1fm -> %.1fm", (double)_local_origin_alt, (double)altitude); if (!PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid()) { const float local_alt_prev = _gpos.altitude(); _local_origin_alt = altitude; resetAltitudeTo(local_alt_prev + _local_origin_alt); } else { const float delta_origin_alt = altitude - _local_origin_alt; _local_origin_alt = altitude; updateVerticalPositionResetStatus(-delta_origin_alt); #if defined(CONFIG_EKF2_TERRAIN) updateTerrainResetStatus(-delta_origin_alt); #endif // CONFIG_EKF2_TERRAIN } return true; } bool Ekf::resetGlobalPositionTo(const double latitude, const double longitude, const float altitude, const float hpos_var, const float vpos_var) { if (!resetLatLonTo(latitude, longitude, hpos_var)) { return false; } // altitude is optional initialiseAltitudeTo(altitude, vpos_var); return true; } bool Ekf::resetLatLonTo(const double latitude, const double longitude, const float hpos_var) { if (!checkLatLonValidity(latitude, longitude)) { return false; } Vector2f pos_prev; if (!_local_origin_lat_lon.isInitialized()) { MapProjection zero_ref; zero_ref.initReference(0.0, 0.0); pos_prev = zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg()); _local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us); // if we are already doing aiding, correct for the change in position since the EKF started navigating if (isLocalHorizontalPositionValid()) { double est_lat; double est_lon; _local_origin_lat_lon.reproject(-pos_prev(0), -pos_prev(1), est_lat, est_lon); _local_origin_lat_lon.initReference(est_lat, est_lon, _time_delayed_us); } ECL_INFO("Origin set to lat=%.6f, lon=%.6f", _local_origin_lat_lon.getProjectionReferenceLat(), _local_origin_lat_lon.getProjectionReferenceLon()); } else { pos_prev = _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg()); } _gpos.setLatLonDeg(latitude, longitude); _output_predictor.resetLatLonTo(latitude, longitude); const Vector2f delta_horz_pos = getLocalHorizontalPosition() - pos_prev; #if defined(CONFIG_EKF2_EXTERNAL_VISION) _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - delta_horz_pos); #endif // CONFIG_EKF2_EXTERNAL_VISION updateHorizontalPositionResetStatus(delta_horz_pos); if (PX4_ISFINITE(hpos_var)) { P.uncorrelateCovarianceSetVariance<2>(State::pos.idx, math::max(sq(0.01f), hpos_var)); } // Reset the timout timer _time_last_hor_pos_fuse = _time_delayed_us; return true; } bool Ekf::initialiseAltitudeTo(const float altitude, const float vpos_var) { if (!checkAltitudeValidity(altitude)) { return false; } if (!PX4_ISFINITE(_local_origin_alt)) { const float local_alt_prev = _gpos.altitude(); if (isLocalVerticalPositionValid()) { _local_origin_alt = altitude - local_alt_prev; } else { _local_origin_alt = altitude; } ECL_INFO("Origin alt=%.3f", (double)_local_origin_alt); } resetAltitudeTo(altitude, vpos_var); return true; } void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const { if (global_origin_valid()) { get_ekf_lpos_accuracy(ekf_eph, ekf_epv); } else { *ekf_eph = INFINITY; *ekf_epv = INFINITY; } } void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const { // TODO - allow for baro drift in vertical position error float hpos_err = sqrtf(P.trace<2>(State::pos.idx)); // If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations if (_horizontal_deadreckon_time_exceeded) { #if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gnss_pos) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm()); } #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_pos) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm()); } #endif // CONFIG_EKF2_EXTERNAL_VISION } *ekf_eph = hpos_err; *ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2)); } void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const { float hvel_err = sqrtf(P.trace<2>(State::vel.idx)); // If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal velocity error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations if (_horizontal_deadreckon_time_exceeded) { float vel_err_conservative = 0.0f; #if defined(CONFIG_EKF2_OPTICAL_FLOW) if (_control_status.flags.opt_flow) { float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f); vel_err_conservative = math::max(getHagl(), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm(); } #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gnss_pos) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm()); } if (_control_status.flags.gnss_vel) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_vel.innovation).norm()); } #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_pos) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_pos.innovation).norm()); } if (_control_status.flags.ev_vel) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_vel.innovation).norm()); } #endif // CONFIG_EKF2_EXTERNAL_VISION hvel_err = math::max(hvel_err, vel_err_conservative); } *ekf_evh = hvel_err; *ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2)); } void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z, float *hagl_max_xy) const { // Do not require limiting by default *vxy_max = NAN; *vz_max = NAN; *hagl_min = NAN; *hagl_max_z = NAN; *hagl_max_xy = NAN; #if defined(CONFIG_EKF2_RANGE_FINDER) // Calculate range finder limits const float rangefinder_hagl_min = _range_sensor.getValidMinVal(); // Allow use of 90% of rangefinder maximum range to allow for angular motion const float rangefinder_hagl_max = 0.9f * _range_sensor.getValidMaxVal(); // TODO : calculate visual odometry limits const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt); if (relying_on_rangefinder) { *hagl_min = rangefinder_hagl_min; *hagl_max_z = rangefinder_hagl_max; } # if defined(CONFIG_EKF2_OPTICAL_FLOW) // Keep within flow AND range sensor limits when exclusively using optical flow const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow); if (relying_on_optical_flow) { // Calculate optical flow limits float flow_hagl_min = _flow_min_distance; float flow_hagl_max = _flow_max_distance; // only limit optical flow height is dependent on range finder or terrain estimate invalid (precaution) if ((!_control_status.flags.opt_flow_terrain && _control_status.flags.rng_terrain) || !isTerrainEstimateValid() ) { flow_hagl_min = math::max(flow_hagl_min, rangefinder_hagl_min); flow_hagl_max = math::min(flow_hagl_max, rangefinder_hagl_max); } const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max); // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height; flow_hagl_max = math::max(flow_hagl_max * 0.9f, flow_hagl_max - 1.0f); *vxy_max = flow_vxy_max; *hagl_min = flow_hagl_min; *hagl_max_xy = flow_hagl_max; } # endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_RANGE_FINDER } void Ekf::resetGyroBias() { // Zero the gyro bias states _state.gyro_bias.zero(); resetGyroBiasCov(); } void Ekf::resetAccelBias() { // Zero the accel bias states _state.accel_bias.zero(); resetAccelBiasCov(); } float Ekf::getHeadingInnovationTestRatio() const { // return the largest heading innovation test ratio float test_ratio = -1.f; #if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { for (auto &test_ratio_filtered : _aid_src_mag.test_ratio_filtered) { test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); } } #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gnss_yaw) { test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_yaw.test_ratio_filtered)); } #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_yaw.test_ratio_filtered)); } #endif // CONFIG_EKF2_EXTERNAL_VISION if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { return sqrtf(test_ratio); } return NAN; } float Ekf::getHorizontalVelocityInnovationTestRatio() const { // return the largest velocity innovation test ratio float test_ratio = -1.f; #if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gnss_vel) { for (int i = 0; i < 2; i++) { // only xy test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i])); } } #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_vel) { for (int i = 0; i < 2; i++) { // only xy test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[i])); } } #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_OPTICAL_FLOW) if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) { for (auto &test_ratio_filtered : _aid_src_optical_flow.test_ratio_filtered) { test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); } } #endif // CONFIG_EKF2_OPTICAL_FLOW if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { return sqrtf(test_ratio); } return NAN; } float Ekf::getVerticalVelocityInnovationTestRatio() const { // return the largest velocity innovation test ratio float test_ratio = -1.f; #if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gnss_vel) { test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[2])); } #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_vel) { test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[2])); } #endif // CONFIG_EKF2_EXTERNAL_VISION if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { return sqrtf(test_ratio); } return NAN; } float Ekf::getHorizontalPositionInnovationTestRatio() const { // return the largest position innovation test ratio float test_ratio = -1.f; #if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gnss_pos) { for (auto &test_ratio_filtered : _aid_src_gnss_pos.test_ratio_filtered) { test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); } } #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_pos) { for (auto &test_ratio_filtered : _aid_src_ev_pos.test_ratio_filtered) { test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); } } #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) if (_control_status.flags.aux_gpos) { test_ratio = math::max(test_ratio, fabsf(_aux_global_position.test_ratio_filtered())); } #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { return sqrtf(test_ratio); } return NAN; } float Ekf::getVerticalPositionInnovationTestRatio() const { // return the combined vertical position innovation test ratio float hgt_sum = 0.f; int n_hgt_sources = 0; #if defined(CONFIG_EKF2_BAROMETER) if (_control_status.flags.baro_hgt) { hgt_sum += sqrtf(fabsf(_aid_src_baro_hgt.test_ratio_filtered)); n_hgt_sources++; } #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps_hgt) { hgt_sum += sqrtf(fabsf(_aid_src_gnss_hgt.test_ratio_filtered)); n_hgt_sources++; } #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { hgt_sum += sqrtf(fabsf(_aid_src_rng_hgt.test_ratio_filtered)); n_hgt_sources++; } #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_hgt) { hgt_sum += sqrtf(fabsf(_aid_src_ev_hgt.test_ratio_filtered)); n_hgt_sources++; } #endif // CONFIG_EKF2_EXTERNAL_VISION if (n_hgt_sources > 0) { return math::max(hgt_sum / static_cast(n_hgt_sources), FLT_MIN); } return NAN; } float Ekf::getAirspeedInnovationTestRatio() const { #if defined(CONFIG_EKF2_AIRSPEED) if (_control_status.flags.fuse_aspd) { // return the airspeed fusion innovation test ratio return sqrtf(fabsf(_aid_src_airspeed.test_ratio_filtered)); } #endif // CONFIG_EKF2_AIRSPEED return NAN; } float Ekf::getSyntheticSideslipInnovationTestRatio() const { #if defined(CONFIG_EKF2_SIDESLIP) if (_control_status.flags.fuse_beta) { // return the synthetic sideslip innovation test ratio return sqrtf(fabsf(_aid_src_sideslip.test_ratio_filtered)); } #endif // CONFIG_EKF2_SIDESLIP return NAN; } float Ekf::getHeightAboveGroundInnovationTestRatio() const { // return the combined HAGL innovation test ratio float hagl_sum = 0.f; int n_hagl_sources = 0; #if defined(CONFIG_EKF2_TERRAIN) # if defined(CONFIG_EKF2_OPTICAL_FLOW) if (_control_status.flags.opt_flow_terrain) { hagl_sum += sqrtf(math::max(fabsf(_aid_src_optical_flow.test_ratio_filtered[0]), _aid_src_optical_flow.test_ratio_filtered[1])); n_hagl_sources++; } # endif // CONFIG_EKF2_OPTICAL_FLOW # if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_terrain) { hagl_sum += sqrtf(fabsf(_aid_src_rng_hgt.test_ratio_filtered)); n_hagl_sources++; } # endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_TERRAIN if (n_hagl_sources > 0) { return math::max(hagl_sum / static_cast(n_hagl_sources), FLT_MIN); } return NAN; } uint16_t Ekf::get_ekf_soln_status() const { // LEGACY Mavlink bitmask containing state of estimator solution (see Mavlink ESTIMATOR_STATUS_FLAGS) union ekf_solution_status_u { struct { uint16_t attitude : 1; uint16_t velocity_horiz : 1; uint16_t velocity_vert : 1; uint16_t pos_horiz_rel : 1; uint16_t pos_horiz_abs : 1; uint16_t pos_vert_abs : 1; uint16_t pos_vert_agl : 1; uint16_t const_pos_mode : 1; uint16_t pred_pos_horiz_rel : 1; uint16_t pred_pos_horiz_abs : 1; uint16_t gps_glitch : 1; uint16_t accel_error : 1; } flags; uint16_t value; } soln_status{}; // 1 ESTIMATOR_ATTITUDE True if the attitude estimate is good soln_status.flags.attitude = attitude_valid(); // 2 ESTIMATOR_VELOCITY_HORIZ True if the horizontal velocity estimate is good soln_status.flags.velocity_horiz = isLocalHorizontalPositionValid(); // 4 ESTIMATOR_VELOCITY_VERT True if the vertical velocity estimate is good soln_status.flags.velocity_vert = isLocalVerticalVelocityValid() || isLocalVerticalPositionValid(); // 8 ESTIMATOR_POS_HORIZ_REL True if the horizontal position (relative) estimate is good soln_status.flags.pos_horiz_rel = isLocalHorizontalPositionValid(); // 16 ESTIMATOR_POS_HORIZ_ABS True if the horizontal position (absolute) estimate is good soln_status.flags.pos_horiz_abs = isGlobalHorizontalPositionValid(); // 32 ESTIMATOR_POS_VERT_ABS True if the vertical position (absolute) estimate is good soln_status.flags.pos_vert_abs = isVerticalAidingActive(); // 64 ESTIMATOR_POS_VERT_AGL True if the vertical position (above ground) estimate is good #if defined(CONFIG_EKF2_TERRAIN) soln_status.flags.pos_vert_agl = isTerrainEstimateValid(); #endif // CONFIG_EKF2_TERRAIN // 128 ESTIMATOR_CONST_POS_MODE True if the EKF is in a constant position mode and is not using external measurements (eg GNSS or optical flow) soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.valid_fake_pos || _control_status.flags.vehicle_at_rest; // 256 ESTIMATOR_PRED_POS_HORIZ_REL True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate soln_status.flags.pred_pos_horiz_rel = isHorizontalAidingActive(); // 512 ESTIMATOR_PRED_POS_HORIZ_ABS True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate soln_status.flags.pred_pos_horiz_abs = _control_status.flags.gnss_pos || _control_status.flags.aux_gpos; // 1024 ESTIMATOR_GPS_GLITCH True if the EKF has detected a GNSS glitch #if defined(CONFIG_EKF2_GNSS) const bool gnss_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f; const bool gnss_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f; soln_status.flags.gps_glitch = (gnss_vel_innov_bad || gnss_pos_innov_bad); #endif // CONFIG_EKF2_GNSS // 2048 ESTIMATOR_ACCEL_ERROR True if the EKF has detected bad accelerometer data soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical || _fault_status.flags.bad_acc_clipping; return soln_status.value; } void Ekf::fuse(const VectorState &K, float innovation) { // quat_nominal Quatf delta_quat(matrix::AxisAnglef(K.slice(State::quat_nominal.idx, 0) * (-1.f * innovation))); _state.quat_nominal = delta_quat * _state.quat_nominal; _state.quat_nominal.normalize(); _R_to_earth = Dcmf(_state.quat_nominal); // vel _state.vel = matrix::constrain(_state.vel - K.slice(State::vel.idx, 0) * innovation, -1.e3f, 1.e3f); // pos const Vector3f pos_correction = K.slice(State::pos.idx, 0) * (-innovation); // Accumulate position in global coordinates _gpos += pos_correction; _state.pos.zero(); // Also store altitude in the state vector as this is used for optical flow fusion _state.pos(2) = -_gpos.altitude(); // gyro_bias _state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice(State::gyro_bias.idx, 0) * innovation, -getGyroBiasLimit(), getGyroBiasLimit()); // accel_bias _state.accel_bias = matrix::constrain(_state.accel_bias - K.slice(State::accel_bias.idx, 0) * innovation, -getAccelBiasLimit(), getAccelBiasLimit()); #if defined(CONFIG_EKF2_MAGNETOMETER) // mag_I, mag_B if (_control_status.flags.mag) { _state.mag_I = matrix::constrain(_state.mag_I - K.slice(State::mag_I.idx, 0) * innovation, -1.f, 1.f); _state.mag_B = matrix::constrain(_state.mag_B - K.slice(State::mag_B.idx, 0) * innovation, -getMagBiasLimit(), getMagBiasLimit()); } #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) // wind_vel if (_control_status.flags.wind) { _state.wind_vel = matrix::constrain(_state.wind_vel - K.slice(State::wind_vel.idx, 0) * innovation, -1.e2f, 1.e2f); } #endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_TERRAIN) _state.terrain = math::constrain(_state.terrain - K(State::terrain.idx) * innovation, -1e4f, 1e4f); #endif // CONFIG_EKF2_TERRAIN } void Ekf::updateDeadReckoningStatus() { updateHorizontalDeadReckoningstatus(); updateVerticalDeadReckoningStatus(); } void Ekf::updateHorizontalDeadReckoningstatus() { bool inertial_dead_reckoning = true; bool aiding_expected_in_air = false; // velocity aiding active if ((_control_status.flags.gnss_vel || _control_status.flags.ev_vel) && isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max) ) { inertial_dead_reckoning = false; } // position aiding active if ((_control_status.flags.gnss_pos || _control_status.flags.ev_pos || _control_status.flags.aux_gpos) && isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) ) { inertial_dead_reckoning = false; } #if defined(CONFIG_EKF2_OPTICAL_FLOW) // optical flow active if (_control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max) ) { inertial_dead_reckoning = false; } else { if (!_control_status.flags.in_air && (_params.flow_ctrl == 1) && isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max) ) { // currently landed, but optical flow aiding should be possible once in air aiding_expected_in_air = true; } } #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_AIRSPEED) // air data aiding active if ((_control_status.flags.fuse_aspd && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max)) && (_control_status.flags.fuse_beta && isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max)) ) { // wind_dead_reckoning: no other aiding but air data _control_status.flags.wind_dead_reckoning = inertial_dead_reckoning; // air data aiding is active, we're not inertial dead reckoning inertial_dead_reckoning = false; } else { _control_status.flags.wind_dead_reckoning = false; if (!_control_status.flags.in_air && _control_status.flags.fixed_wing && (_params.beta_fusion_enabled == 1) && (_params.arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max) ) { // currently landed, but air data aiding should be possible once in air aiding_expected_in_air = true; } } #endif // CONFIG_EKF2_AIRSPEED // zero velocity update if (isRecent(_zero_velocity_update.time_last_fuse(), _params.no_aid_timeout_max)) { // only respect as a valid aiding source now if we expect to have another valid source once in air if (aiding_expected_in_air) { inertial_dead_reckoning = false; } } if (_control_status.flags.valid_fake_pos && isRecent(_aid_src_fake_pos.time_last_fuse, _params.no_aid_timeout_max)) { // only respect as a valid aiding source now if we expect to have another valid source once in air if (aiding_expected_in_air) { inertial_dead_reckoning = false; } } if (inertial_dead_reckoning) { if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max)) { // deadreckon time exceeded if (!_horizontal_deadreckon_time_exceeded) { ECL_WARN("horizontal dead reckon time exceeded"); _horizontal_deadreckon_time_exceeded = true; } } } else { if (_time_delayed_us > _params.no_aid_timeout_max) { _time_last_horizontal_aiding = _time_delayed_us - _params.no_aid_timeout_max; } _horizontal_deadreckon_time_exceeded = false; } _control_status.flags.inertial_dead_reckoning = inertial_dead_reckoning; } void Ekf::updateVerticalDeadReckoningStatus() { if (isVerticalPositionAidingActive()) { _time_last_v_pos_aiding = _time_last_hgt_fuse; _vertical_position_deadreckon_time_exceeded = false; } else if (isTimedOut(_time_last_v_pos_aiding, (uint64_t)_params.valid_timeout_max)) { _vertical_position_deadreckon_time_exceeded = true; } if (isVerticalVelocityAidingActive()) { _time_last_v_vel_aiding = _time_last_ver_vel_fuse; _vertical_velocity_deadreckon_time_exceeded = false; } else if (isTimedOut(_time_last_v_vel_aiding, (uint64_t)_params.valid_timeout_max) && _vertical_position_deadreckon_time_exceeded) { _vertical_velocity_deadreckon_time_exceeded = true; } } Vector3f Ekf::getRotVarBody() const { const matrix::SquareMatrix3f rot_cov_body = getStateCovariance(); return matrix::SquareMatrix3f(_R_to_earth.T() * rot_cov_body * _R_to_earth).diag(); } Vector3f Ekf::getRotVarNed() const { const matrix::SquareMatrix3f rot_cov_ned = getStateCovariance(); return rot_cov_ned.diag(); } float Ekf::getYawVar() const { return getRotVarNed()(2); } float Ekf::getTiltVariance() const { const Vector3f rot_var_ned = getRotVarNed(); return rot_var_ned(0) + rot_var_ned(1); } #if defined(CONFIG_EKF2_BAROMETER) void Ekf::updateGroundEffect() { if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) { #if defined(CONFIG_EKF2_TERRAIN) if (isTerrainEstimateValid()) { // automatically set ground effect if terrain is valid float height = getHagl(); _control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt); } else #endif // CONFIG_EKF2_TERRAIN if (_control_status.flags.gnd_effect) { // Turn off ground effect compensation if it times out if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { _control_status.flags.gnd_effect = false; } } } else { _control_status.flags.gnd_effect = false; } } #endif // CONFIG_EKF2_BAROMETER void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) { // inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected // xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector { const Vector3f gyro_corrected = imu_delayed.delta_ang / imu_delayed.delta_ang_dt - _state.gyro_bias; const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.acc_bias_learn_tc), 0.f, 1.f); const float beta = 1.f - alpha; _ang_rate_magnitude_filt = fmaxf(gyro_corrected.norm(), beta * _ang_rate_magnitude_filt); } { const Vector3f accel_corrected = imu_delayed.delta_vel / imu_delayed.delta_vel_dt - _state.accel_bias; const float alpha = math::constrain((imu_delayed.delta_vel_dt / _params.acc_bias_learn_tc), 0.f, 1.f); const float beta = 1.f - alpha; _accel_magnitude_filt = fmaxf(accel_corrected.norm(), beta * _accel_magnitude_filt); } const bool is_manoeuvre_level_high = (_ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim) || (_accel_magnitude_filt > _params.acc_bias_learn_acc_lim); // gyro bias inhibit const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::GyroBias)); for (unsigned index = 0; index < State::gyro_bias.dof; index++) { bool is_bias_observable = true; // TODO: gyro bias conditions _gyro_bias_inhibit[index] = do_inhibit_all_gyro_axes || !is_bias_observable; } // accel bias inhibit const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::AccelBias)) || is_manoeuvre_level_high || _fault_status.flags.bad_acc_vertical; for (unsigned index = 0; index < State::accel_bias.dof; index++) { bool is_bias_observable = true; if (_control_status.flags.vehicle_at_rest) { is_bias_observable = true; } else if (_control_status.flags.fake_hgt) { is_bias_observable = false; } else if (_control_status.flags.fake_pos || _control_status.flags.gravity_vector) { // only consider an accel bias observable if aligned with the gravity vector is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966 } _accel_bias_inhibit[index] = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable; } } void Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index) { VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. // calculate kalman gain K = PHS, where S = 1/innovation variance for (int row = 0; row < State::size; row++) { K(row) = P(row, state_index) / innov_var; } clearInhibitedStateKalmanGains(K); #if false // Matrix implementation of the Joseph stabilized covariance update // This is extremely expensive to compute. Use for debugging purposes only. auto A = matrix::eye(); VectorState H; H(state_index) = 1.f; A -= K.multiplyByTranspose(H); P = A * P; P = P.multiplyByTranspose(A); const VectorState KR = K * R; P += KR.multiplyByTranspose(K); #else // Efficient implementation of the Joseph stabilized covariance update // Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006" // P = (I - K * H) * P * (I - K * H).T + K * R * K.T // = P_temp * (I - H.T * K.T) + K * R * K.T // = P_temp - P_temp * H.T * K.T + K * R * K.T // Step 1: conventional update // Compute P_temp and store it in P to avoid allocating more memory // P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major VectorState PH = P.row(state_index); for (unsigned i = 0; i < State::size; i++) { for (unsigned j = 0; j < State::size; j++) { P(i, j) -= K(i) * PH(j); // P is now not symmetric if K is not optimal (e.g.: some gains have been zeroed) } } // Step 2: stabilized update // P (or "P_temp") is not symmetric so we must take the column PH = P.col(state_index); for (unsigned i = 0; i < State::size; i++) { for (unsigned j = 0; j <= i; j++) { P(i, j) = P(i, j) - PH(i) * K(j) + K(i) * R * K(j); P(j, i) = P(i, j); } } #endif constrainStateVariances(); // apply the state corrections fuse(K, innov); } bool Ekf::measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation) { clearInhibitedStateKalmanGains(K); #if false // Matrix implementation of the Joseph stabilized covariance update // This is extremely expensive to compute. Use for debugging purposes only. auto A = matrix::eye(); A -= K.multiplyByTranspose(H); P = A * P; P = P.multiplyByTranspose(A); const VectorState KR = K * R; P += KR.multiplyByTranspose(K); #else // Efficient implementation of the Joseph stabilized covariance update // Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006" // P = (I - K * H) * P * (I - K * H).T + K * R * K.T // = P_temp * (I - H.T * K.T) + K * R * K.T // = P_temp - P_temp * H.T * K.T + K * R * K.T // Step 1: conventional update // Compute P_temp and store it in P to avoid allocating more memory // P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major VectorState PH = P * H; // H is stored as a column vector. H is in fact H.T for (unsigned i = 0; i < State::size; i++) { for (unsigned j = 0; j < State::size; j++) { P(i, j) -= K(i) * PH(j); // P is now not symmetrical if K is not optimal (e.g.: some gains have been zeroed) } } // Step 2: stabilized update PH = P * H; // H is stored as a column vector. H is in fact H.T for (unsigned i = 0; i < State::size; i++) { for (unsigned j = 0; j <= i; j++) { P(i, j) = P(i, j) - PH(i) * K(j) + K(i) * R * K(j); P(j, i) = P(i, j); } } #endif constrainStateVariances(); // apply the state corrections fuse(K, innovation); return true; } void Ekf::resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const { status.time_last_fuse = _time_delayed_us; status.innovation = 0.f; status.innovation_filtered = 0.f; status.innovation_variance = status.observation_variance; status.test_ratio = 0.f; status.test_ratio_filtered = 0.f; status.innovation_rejected = false; status.fused = true; } void Ekf::updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t ×tamp_sample, const float &observation, const float &observation_variance, const float &innovation, const float &innovation_variance, float innovation_gate) const { bool innovation_rejected = false; const float test_ratio = sq(innovation) / (sq(innovation_gate) * innovation_variance); if ((status.timestamp_sample > 0) && (timestamp_sample > status.timestamp_sample)) { const float dt_s = math::constrain((timestamp_sample - status.timestamp_sample) * 1e-6f, 0.001f, 1.f); static constexpr float tau = 0.5f; const float alpha = math::constrain(dt_s / (dt_s + tau), 0.f, 1.f); // test_ratio_filtered if (PX4_ISFINITE(status.test_ratio_filtered)) { status.test_ratio_filtered += alpha * (matrix::sign(innovation) * test_ratio - status.test_ratio_filtered); } else { // otherwise, init the filtered test ratio status.test_ratio_filtered = test_ratio; } // innovation_filtered if (PX4_ISFINITE(status.innovation_filtered)) { status.innovation_filtered += alpha * (innovation - status.innovation_filtered); } else { // otherwise, init the filtered innovation status.innovation_filtered = innovation; } // limit extremes in filtered values static constexpr float kNormalizedInnovationLimit = 2.f; static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit); if (test_ratio > kTestRatioLimit) { status.test_ratio_filtered = math::constrain(status.test_ratio_filtered, -kTestRatioLimit, kTestRatioLimit); const float innov_limit = kNormalizedInnovationLimit * innovation_gate * sqrtf(innovation_variance); status.innovation_filtered = math::constrain(status.innovation_filtered, -innov_limit, innov_limit); } } else { // invalid timestamp_sample, reset status.test_ratio_filtered = test_ratio; status.innovation_filtered = innovation; } status.test_ratio = test_ratio; status.observation = observation; status.observation_variance = observation_variance; status.innovation = innovation; status.innovation_variance = innovation_variance; if ((test_ratio > 1.f) || !PX4_ISFINITE(test_ratio) || !PX4_ISFINITE(status.innovation) || !PX4_ISFINITE(status.innovation_variance) ) { innovation_rejected = true; } status.timestamp_sample = timestamp_sample; // if any of the innovations are rejected, then the overall innovation is rejected status.innovation_rejected = innovation_rejected; // reset status.fused = false; } void Ekf::clearInhibitedStateKalmanGains(VectorState &K) const { for (unsigned i = 0; i < State::gyro_bias.dof; i++) { if (_gyro_bias_inhibit[i]) { K(State::gyro_bias.idx + i) = 0.f; } } for (unsigned i = 0; i < State::accel_bias.dof; i++) { if (_accel_bias_inhibit[i]) { K(State::accel_bias.idx + i) = 0.f; } } #if defined(CONFIG_EKF2_MAGNETOMETER) if (!_control_status.flags.mag) { for (unsigned i = 0; i < State::mag_I.dof; i++) { K(State::mag_I.idx + i) = 0.f; } } if (!_control_status.flags.mag) { for (unsigned i = 0; i < State::mag_B.dof; i++) { K(State::mag_B.idx + i) = 0.f; } } #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) if (!_control_status.flags.wind) { for (unsigned i = 0; i < State::wind_vel.dof; i++) { K(State::wind_vel.idx + i) = 0.f; } } #endif // CONFIG_EKF2_WIND } float Ekf::getHeadingInnov() const { #if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { return Vector3f(_aid_src_mag.innovation).max(); } #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gnss_yaw) { return _aid_src_gnss_yaw.innovation; } #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { return _aid_src_ev_yaw.innovation; } #endif // CONFIG_EKF2_EXTERNAL_VISION return 0.f; } float Ekf::getHeadingInnovVar() const { #if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { return Vector3f(_aid_src_mag.innovation_variance).max(); } #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gnss_yaw) { return _aid_src_gnss_yaw.innovation_variance; } #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { return _aid_src_ev_yaw.innovation_variance; } #endif // CONFIG_EKF2_EXTERNAL_VISION return 0.f; } float Ekf::getHeadingInnovRatio() const { #if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { return Vector3f(_aid_src_mag.test_ratio).max(); } #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gnss_yaw) { return _aid_src_gnss_yaw.test_ratio; } #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { return _aid_src_ev_yaw.test_ratio; } #endif // CONFIG_EKF2_EXTERNAL_VISION return 0.f; }