/**************************************************************************** * * 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 EKF2.cpp * Implementation of the attitude and position estimator. * * @author Roman Bapst */ #ifndef EKF2_HPP #define EKF2_HPP #include "EKF/ekf.h" #include "Utility/PreFlightChecker.hpp" #include "EKF2Selector.hpp" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #if defined(CONFIG_EKF2_AIRSPEED) # include # include #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_AUXVEL) # include #endif // CONFIG_EKF2_AUXVEL #if defined(CONFIG_EKF2_OPTICAL_FLOW) # include # include #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_RANGE_FINDER) # include #endif // CONFIG_EKF2_RANGE_FINDER extern pthread_mutex_t ekf2_module_mutex; class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem { public: EKF2() = delete; EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode, int config_param); ~EKF2() override; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); /** @see ModuleBase */ static int custom_command(int argc, char *argv[]); /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); int print_status(); bool should_exit() const { return _task_should_exit.load(); } void request_stop() { _task_should_exit.store(true); } static void lock_module() { pthread_mutex_lock(&ekf2_module_mutex); } static bool trylock_module() { return (pthread_mutex_trylock(&ekf2_module_mutex) == 0); } static void unlock_module() { pthread_mutex_unlock(&ekf2_module_mutex); } #if defined(CONFIG_EKF2_MULTI_INSTANCE) bool multi_init(int imu, int mag); #endif // CONFIG_EKF2_MULTI_INSTANCE int instance() const { return _instance; } private: //TODO: remove after 1.14 release enum SensorFusionMask : uint16_t { // Bit locations for fusion_mode DEPRECATED_USE_GPS = (1 << 0), ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl) DEPRECATED_USE_OPT_FLOW = (1 << 1), ///< set to true to use optical flow data DEPRECATED_INHIBIT_ACC_BIAS = (1 << 2), ///< set to true to inhibit estimation of accelerometer delta velocity bias DEPRECATED_USE_EXT_VIS_POS = (1 << 3), ///< set to true to use external vision position data DEPRECATED_USE_EXT_VIS_YAW = (1 << 4), ///< set to true to use external vision quaternion data for yaw DEPRECATED_USE_DRAG = (1 << 5), ///< set to true to use the multi-rotor drag model to estimate wind DEPRECATED_ROTATE_EXT_VIS = (1 << 6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used DEPRECATED_USE_GPS_YAW = (1 << 7), ///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl) DEPRECATED_USE_EXT_VIS_VEL = (1 << 8), ///< set to true to use external vision velocity data }; static constexpr uint8_t MAX_NUM_IMUS = 4; static constexpr uint8_t MAX_NUM_MAGS = 4; static constexpr uint8_t MAX_NUM_CONFS = 2; void Run() override; void VerifyParams(); void PublishAidSourceStatus(const hrt_abstime ×tamp); void PublishAttitude(const hrt_abstime ×tamp); void PublishBaroBias(const hrt_abstime ×tamp); void PublishGnssHgtBias(const hrt_abstime ×tamp); #if defined(CONFIG_EKF2_RANGE_FINDER) void PublishRngHgtBias(const hrt_abstime ×tamp); #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) void PublishEvPosBias(const hrt_abstime ×tamp); #endif // CONFIG_EKF2_EXTERNAL_VISION estimator_bias_s fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us, uint64_t timestamp, uint32_t device_id = 0); void PublishEventFlags(const hrt_abstime ×tamp); void PublishGlobalPosition(const hrt_abstime ×tamp); void PublishGpsStatus(const hrt_abstime ×tamp); void PublishInnovations(const hrt_abstime ×tamp); void PublishInnovationTestRatios(const hrt_abstime ×tamp); void PublishInnovationVariances(const hrt_abstime ×tamp); void PublishLocalPosition(const hrt_abstime ×tamp); void PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sample); void PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_odometry_s &ev_odom); #if defined(CONFIG_EKF2_OPTICAL_FLOW) void PublishOpticalFlowVel(const hrt_abstime ×tamp); #endif // CONFIG_EKF2_OPTICAL_FLOW void PublishSensorBias(const hrt_abstime ×tamp); void PublishStates(const hrt_abstime ×tamp); void PublishStatus(const hrt_abstime ×tamp); void PublishStatusFlags(const hrt_abstime ×tamp); void PublishWindEstimate(const hrt_abstime ×tamp); void PublishYawEstimatorStatus(const hrt_abstime ×tamp); #if defined(CONFIG_EKF2_AIRSPEED) void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_AUXVEL) void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_AUXVEL void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps); #if defined(CONFIG_EKF2_EXTERNAL_VISION) bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_OPTICAL_FLOW) bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_OPTICAL_FLOW void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); #if defined(CONFIG_EKF2_RANGE_FINDER) void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_RANGE_FINDER void UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps); // Used to check, save and use learned accel/gyro/mag biases struct InFlightCalibration { hrt_abstime last_us{0}; ///< last time the EKF was operating a mode that estimates accelerometer biases (uSec) hrt_abstime total_time_us{0}; ///< accumulated calibration time since the last save matrix::Vector3f bias{}; bool cal_available{false}; ///< true when an unsaved valid calibration for the XYZ accelerometer bias is available }; void UpdateCalibration(const hrt_abstime ×tamp, InFlightCalibration &cal, const matrix::Vector3f &bias, const matrix::Vector3f &bias_variance, float bias_limit, bool bias_valid, bool learning_valid); void UpdateAccelCalibration(const hrt_abstime ×tamp); void UpdateGyroCalibration(const hrt_abstime ×tamp); void UpdateMagCalibration(const hrt_abstime ×tamp); // publish helper for estimator_aid_source topics template void PublishAidSourceStatus(const T &status, hrt_abstime &status_publish_last, uORB::PublicationMulti &pub) { if (status.timestamp_sample > status_publish_last) { // publish if updated T status_out{status}; status_out.estimator_instance = _instance; status_out.timestamp = hrt_absolute_time(); pub.publish(status_out); // record timestamp sample status_publish_last = status.timestamp_sample; } } /* * Calculate filtered WGS84 height from estimated AMSL height */ float filter_altitude_ellipsoid(float amsl_hgt); static constexpr float sq(float x) { return x * x; }; const bool _replay_mode{false}; ///< true when we use replay data from a log const bool _multi_mode; const int _config_param{0}; int _instance{0}; px4::atomic_bool _task_should_exit{false}; // time slip monitoring uint64_t _integrated_time_us = 0; ///< integral of gyro delta time from start (uSec) uint64_t _start_time_us = 0; ///< system time at EKF start (uSec) int64_t _last_time_slip_us = 0; ///< Last time slip (uSec) perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")}; perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")}; perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")}; perf_counter_t _msg_missed_air_data_perf{nullptr}; perf_counter_t _msg_missed_gps_perf{nullptr}; perf_counter_t _msg_missed_magnetometer_perf{nullptr}; perf_counter_t _msg_missed_odometry_perf{nullptr}; // Used to control saving of mag declination to be used on next startup bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved InFlightCalibration _accel_cal{}; InFlightCalibration _gyro_cal{}; InFlightCalibration _mag_cal{}; uint64_t _gps_time_usec{0}; int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 uint8_t _accel_calibration_count{0}; uint8_t _baro_calibration_count{0}; uint8_t _gyro_calibration_count{0}; uint8_t _mag_calibration_count{0}; uint32_t _device_id_accel{0}; uint32_t _device_id_baro{0}; uint32_t _device_id_gyro{0}; uint32_t _device_id_mag{0}; Vector3f _last_accel_bias_published{}; Vector3f _last_gyro_bias_published{}; Vector3f _last_mag_bias_published{}; hrt_abstime _last_sensor_bias_published{0}; hrt_abstime _last_gps_status_published{0}; hrt_abstime _status_baro_hgt_pub_last{0}; hrt_abstime _status_rng_hgt_pub_last{0}; hrt_abstime _status_fake_hgt_pub_last{0}; hrt_abstime _status_fake_pos_pub_last{0}; #if defined(CONFIG_EKF2_EXTERNAL_VISION) uORB::PublicationMulti _estimator_aid_src_ev_hgt_pub {ORB_ID(estimator_aid_src_ev_hgt)}; uORB::PublicationMulti _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)}; uORB::PublicationMulti _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)}; uORB::PublicationMulti _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)}; hrt_abstime _status_ev_hgt_pub_last{0}; hrt_abstime _status_ev_pos_pub_last{0}; hrt_abstime _status_ev_vel_pub_last{0}; hrt_abstime _status_ev_yaw_pub_last{0}; matrix::Vector3f _last_ev_bias_published{}; uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)}; uORB::PublicationMulti _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_bias)}; #endif // CONFIG_EKF2_EXTERNAL_VISION hrt_abstime _status_gnss_hgt_pub_last{0}; hrt_abstime _status_gnss_pos_pub_last{0}; hrt_abstime _status_gnss_vel_pub_last{0}; #if defined(CONFIG_EKF2_GNSS_YAW) hrt_abstime _status_gnss_yaw_pub_last {0}; #endif // CONFIG_EKF2_GNSS_YAW hrt_abstime _status_mag_pub_last{0}; hrt_abstime _status_mag_heading_pub_last{0}; hrt_abstime _status_gravity_pub_last{0}; #if defined(CONFIG_EKF2_AUXVEL) uORB::Subscription _landing_target_pose_sub {ORB_ID(landing_target_pose)}; uORB::PublicationMulti _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)}; hrt_abstime _status_aux_vel_pub_last{0}; perf_counter_t _msg_missed_landing_target_pose_perf{nullptr}; #endif // CONFIG_EKF2_AUXVEL #if defined(CONFIG_EKF2_OPTICAL_FLOW) uORB::Subscription _vehicle_optical_flow_sub {ORB_ID(vehicle_optical_flow)}; uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; uORB::PublicationMulti _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)}; hrt_abstime _status_optical_flow_pub_last{0}; hrt_abstime _status_terrain_optical_flow_pub_last{0}; perf_counter_t _msg_missed_optical_flow_perf{nullptr}; #endif // CONFIG_EKF2_OPTICAL_FLOW float _last_baro_bias_published{}; float _last_gnss_hgt_bias_published{}; float _last_rng_hgt_bias_published{}; #if defined(CONFIG_EKF2_AIRSPEED) uORB::Subscription _airspeed_sub {ORB_ID(airspeed)}; uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements hrt_abstime _airspeed_validated_timestamp_last{0}; uORB::PublicationMulti _estimator_aid_src_airspeed_pub {ORB_ID(estimator_aid_src_airspeed)}; hrt_abstime _status_airspeed_pub_last{0}; perf_counter_t _msg_missed_airspeed_perf{nullptr}; perf_counter_t _msg_missed_airspeed_validated_perf{nullptr}; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) uORB::PublicationMulti _estimator_aid_src_sideslip_pub {ORB_ID(estimator_aid_src_sideslip)}; hrt_abstime _status_sideslip_pub_last {0}; #endif // CONFIG_EKF2_SIDESLIP orb_advert_t _mavlink_log_pub{nullptr}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; #if defined(CONFIG_EKF2_RANGE_FINDER) uORB::PublicationMulti _estimator_rng_hgt_bias_pub {ORB_ID(estimator_rng_hgt_bias)}; uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; hrt_abstime _last_range_sensor_update{0}; int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations unsigned _distance_sensor_last_generation{0}; perf_counter_t _msg_missed_distance_sensor_perf{nullptr}; #endif // CONFIG_EKF2_RANGE_FINDER bool _callback_registered{false}; hrt_abstime _last_event_flags_publish{0}; hrt_abstime _last_status_flags_publish{0}; uint64_t _filter_control_status{0}; uint32_t _filter_fault_status{0}; uint32_t _innov_check_fail_status{0}; uint32_t _filter_control_status_changes{0}; uint32_t _filter_fault_status_changes{0}; uint32_t _innov_check_fail_status_changes{0}; uint32_t _filter_warning_event_changes{0}; uint32_t _filter_information_event_changes{0}; uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; uORB::PublicationMulti _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)}; uORB::PublicationMultiData _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; uORB::PublicationMulti _estimator_gps_status_pub{ORB_ID(estimator_gps_status)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; uORB::PublicationMulti _estimator_innovations_pub{ORB_ID(estimator_innovations)}; uORB::PublicationMulti _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; uORB::PublicationMulti _estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)}; uORB::PublicationMulti _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)}; uORB::PublicationMulti _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)}; uORB::PublicationMulti _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)}; uORB::PublicationMulti _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)}; uORB::PublicationMulti _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)}; #if defined(CONFIG_EKF2_GNSS_YAW) uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)}; #endif // CONFIG_EKF2_GNSS_YAW uORB::PublicationMulti _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)}; uORB::PublicationMulti _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)}; uORB::PublicationMulti _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; // publications with topic dependent on multi-mode uORB::PublicationMulti _attitude_pub; uORB::PublicationMulti _local_position_pub; uORB::PublicationMulti _global_position_pub; uORB::PublicationMulti _odometry_pub; uORB::PublicationMulti _wind_pub; PreFlightChecker _preflt_checker; Ekf _ekf; parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance) DEFINE_PARAMETERS( (ParamExtInt) _param_ekf2_predict_us, (ParamExtInt) _param_ekf2_imu_ctrl, (ParamExtFloat) _param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec) (ParamExtFloat) _param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec) (ParamExtFloat) _param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec) #if defined(CONFIG_EKF2_AUXVEL) (ParamExtFloat) _param_ekf2_avel_delay, ///< auxiliary velocity measurement delay relative to the IMU (mSec) #endif // CONFIG_EKF2_AUXVEL (ParamExtFloat) _param_ekf2_gyr_noise, ///< IMU angular rate noise used for covariance prediction (rad/sec) (ParamExtFloat) _param_ekf2_acc_noise, ///< IMU acceleration noise use for covariance prediction (m/sec**2) // process noise (ParamExtFloat) _param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2) (ParamExtFloat) _param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3) (ParamExtFloat) _param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec) (ParamExtFloat) _param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec) (ParamExtFloat) _param_ekf2_wind_nsd, ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz)) (ParamExtFloat) _param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec) (ParamExtFloat) _param_ekf2_gps_p_noise, ///< minimum allowed observation noise for gps position fusion (m) (ParamExtFloat) _param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m) (ParamExtFloat) _param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m) (ParamExtFloat) _param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD) (ParamExtFloat) _param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m) (ParamExtFloat) _param_ekf2_gnd_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m) (ParamExtFloat) _param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD) (ParamExtFloat) _param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD) #if defined(CONFIG_EKF2_AIRSPEED) (ParamExtFloat) _param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec) (ParamExtFloat) _param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD) (ParamExtFloat) _param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec) // control of airspeed fusion (ParamExtFloat) _param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec) #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) (ParamExtFloat) _param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD) (ParamExtFloat) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad) (ParamExtInt) _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables #endif // CONFIG_EKF2_SIDESLIP // control of magnetometer fusion (ParamExtFloat) _param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad) (ParamExtFloat) _param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) (ParamExtFloat) _param_ekf2_mag_decl,///< magnetic declination (degrees) (ParamExtFloat) _param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD) (ParamExtFloat) _param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD) (ParamExtInt) _param_ekf2_decl_type, ///< bitmask used to control the handling of declination data (ParamExtInt) _param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used (ParamExtFloat) _param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used (ParamExtFloat) _param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec) (ParamExtInt) _param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used (ParamExtFloat) _param_ekf2_req_eph, ///< maximum acceptable horiz position error (m) (ParamExtFloat) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m) (ParamExtFloat) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s) (ParamExtInt) _param_ekf2_req_nsats, ///< minimum acceptable satellite count (ParamExtFloat) _param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision (ParamExtFloat) _param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s) (ParamExtFloat) _param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s) // measurement source control (ParamInt) _param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used (ParamExtInt) _param_ekf2_hgt_ref, ///< selects the primary source for height data (ParamExtInt) _param_ekf2_baro_ctrl,///< barometer control selection (ParamInt) _param_ekf2_gps_ctrl, ///< GPS control selection (ParamInt) _param_ekf2_1_gps_ctrl, ///< Alternate GPS control selection (ParamExtInt) _param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec) #if defined(CONFIG_EKF2_RANGE_FINDER) // range finder fusion (ParamExtInt) _param_ekf2_rng_ctrl, ///< range finder control selection (ParamExtFloat) _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) (ParamExtFloat) _param_ekf2_rng_noise, ///< observation noise for range finder measurements (m) (ParamExtFloat) _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m) (ParamExtFloat) _param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD) (ParamExtFloat) _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m) (ParamExtFloat) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad) (ParamExtFloat) _param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s) (ParamExtFloat) _param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m) (ParamExtFloat) _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) (ParamExtFloat) _param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) (ParamExtFloat) _param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD) (ParamExtFloat) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) (ParamExtFloat) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) (ParamExtFloat) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) (ParamExtInt) _param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation (ParamExtFloat) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec) (ParamExtFloat) _param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m) #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) // vision estimate fusion (ParamExtFloat) _param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec) (ParamInt) _param_ekf2_ev_ctrl, ///< external vision (EV) control selection (ParamInt) _param_ekf2_1_ev_ctrl, ///< Alternate external vision (EV) control selection (ParamInt) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise (ParamExtInt) _param_ekf2_ev_qmin, (ParamExtFloat) _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m) (ParamExtFloat) _param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s) (ParamExtFloat) _param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad) (ParamExtFloat) _param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD) (ParamExtFloat) _param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD) (ParamExtFloat) _param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m) (ParamExtFloat) _param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m) (ParamExtFloat) _param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m) #endif // CONFIG_EKF2_EXTERNAL_VISION (ParamExtFloat) _param_ekf2_grav_noise, ///< default accelerometer noise for gravity fusion measurements (m/s**2) #if defined(CONFIG_EKF2_OPTICAL_FLOW) // optical flow fusion (ParamExtInt) _param_ekf2_of_ctrl, ///< optical flow fusion selection (ParamExtFloat) _param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval (ParamExtFloat) _param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec) (ParamExtFloat) _param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec) (ParamExtInt) _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor (ParamExtFloat) _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD) (ParamExtFloat) _param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m) (ParamExtFloat) _param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m) (ParamExtFloat) _param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m) #endif // CONFIG_EKF2_OPTICAL_FLOW // sensor positions in body frame (ParamExtFloat) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m) (ParamExtFloat) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m) (ParamExtFloat) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m) (ParamExtFloat) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m) (ParamExtFloat) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m) (ParamExtFloat) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m) // output predictor filter time constants (ParamFloat) _param_ekf2_tau_vel, ///< time constant used by the output velocity complementary filter (sec) (ParamFloat) _param_ekf2_tau_pos, ///< time constant used by the output position complementary filter (sec) // IMU switch on bias parameters (ParamExtFloat) _param_ekf2_gbias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) (ParamExtFloat) _param_ekf2_abias_init, ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) (ParamExtFloat) _param_ekf2_angerr_init, ///< 1-sigma tilt error after initial alignment using gravity vector (rad) // EKF accel bias learning control (ParamExtFloat) _param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2) (ParamExtFloat) _param_ekf2_abl_acclim, ///< Maximum IMU accel magnitude that allows IMU bias learning (m/s**2) (ParamExtFloat) _param_ekf2_abl_gyrlim, ///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2) (ParamExtFloat) _param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec) (ParamExtFloat) _param_ekf2_gyr_b_lim, ///< Gyro bias learning limit (rad/s) #if defined(CONFIG_EKF2_DRAG_FUSION) (ParamExtInt) _param_ekf2_drag_ctrl, ///< drag fusion selection // Multi-rotor drag specific force fusion (ParamExtFloat) _param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2 (ParamExtFloat) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2) (ParamExtFloat) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2) (ParamExtFloat) _param_ekf2_mcoef, ///< propeller momentum drag coefficient (1/s) #endif // CONFIG_EKF2_DRAG_FUSION #if defined(CONFIG_EKF2_BARO_COMPENSATION) // Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth // Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2 (ParamExtFloat) _param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2) (ParamExtFloat) _param_ekf2_pcoef_xp, ///< static pressure position error coefficient along the positive X body axis (ParamExtFloat) _param_ekf2_pcoef_xn, ///< static pressure position error coefficient along the negative X body axis (ParamExtFloat) _param_ekf2_pcoef_yp, ///< static pressure position error coefficient along the positive Y body axis (ParamExtFloat) _param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis (ParamExtFloat) _param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis #endif // CONFIG_EKF2_BARO_COMPENSATION (ParamFloat) _param_ekf2_req_gps_h, ///< Required GPS health time (ParamExtInt) _param_ekf2_mag_check, ///< Mag field strength check (ParamExtInt) _param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone. // Used by EKF-GSF experimental yaw estimator (ParamExtFloat) _param_ekf2_gsf_tas_default ///< default value of true airspeed assumed during fixed wing operation ) }; #endif // !EKF2_HPP