PX4 Firmware
PX4 Autopilot Software http://px4.io
Ekf2 Class Referencefinal
Inheritance diagram for Ekf2:
Collaboration diagram for Ekf2:

Public Member Functions

 Ekf2 (bool replay_mode=false)
 
 ~Ekf2 () override
 
void Run () override
 
bool init ()
 
int print_status () override
 

Static Public Member Functions

static int task_spawn (int argc, char *argv[])
 
static int custom_command (int argc, char *argv[])
 
static int print_usage (const char *reason=nullptr)
 

Private Member Functions

int getRangeSubIndex ()
 get subscription index of first downward-facing range sensor More...
 
void fillGpsMsgWithVehicleGpsPosData (gps_message &msg, const vehicle_gps_position_s &data)
 
void runPreFlightChecks (float dt, const filter_control_status_u &control_status, const vehicle_status_s &vehicle_status, const ekf2_innovations_s &innov)
 
void resetPreFlightChecks ()
 
template<typename Param >
void update_mag_bias (Param &mag_bias_param, int axis_index)
 
template<typename Param >
bool update_mag_decl (Param &mag_decl_param)
 
bool publish_attitude (const sensor_combined_s &sensors, const hrt_abstime &now)
 
bool publish_wind_estimate (const hrt_abstime &timestamp)
 
const Vector3f get_vel_body_wind ()
 
bool blend_gps_data ()
 
void update_gps_blend_states ()
 
void update_gps_offsets ()
 
void apply_gps_offsets ()
 
void calc_gps_blend_output ()
 
float filter_altitude_ellipsoid (float amsl_hgt)
 

Private Attributes

PreFlightChecker _preflt_checker
 
const bool _replay_mode
 true when we use replay data from a log More...
 
uint64_t _integrated_time_us = 0
 integral of gyro delta time from start (uSec) More...
 
uint64_t _start_time_us = 0
 system time at EKF start (uSec) More...
 
int64_t _last_time_slip_us = 0
 Last time slip (uSec) More...
 
perf_counter_t _ekf_update_perf
 
uint8_t _invalid_mag_id_count = 0
 number of times an invalid magnetomer device ID has been detected More...
 
float _mag_data_sum [3] = {}
 summed magnetometer readings (Gauss) More...
 
uint64_t _mag_time_sum_ms = 0
 summed magnetoemter time stamps (mSec) More...
 
uint8_t _mag_sample_count = 0
 number of magnetometer measurements summed during downsampling More...
 
int32_t _mag_time_ms_last_used = 0
 time stamp of the last averaged magnetometer measurement sent to the EKF (mSec) More...
 
float _balt_data_sum = 0.0f
 summed pressure altitude readings (m) More...
 
uint64_t _balt_time_sum_ms = 0
 summed pressure altitude time stamps (mSec) More...
 
uint8_t _balt_sample_count = 0
 number of barometric altitude measurements summed More...
 
uint32_t _balt_time_ms_last_used
 time stamp of the last averaged barometric altitude measurement sent to the EKF (mSec) More...
 
hrt_abstime _last_magcal_us = 0
 last time the EKF was operating a mode that estimates magnetomer biases (uSec) More...
 
hrt_abstime _total_cal_time_us = 0
 accumulated calibration time since the last save More...
 
float _last_valid_mag_cal [3] = {}
 last valid XYZ magnetometer bias estimates (mGauss) More...
 
bool _valid_cal_available [3] = {}
 true when an unsaved valid calibration for the XYZ magnetometer bias is available More...
 
float _last_valid_variance [3] = {}
 variances for the last valid magnetometer XYZ bias estimates (mGauss**2) More...
 
bool _mag_decl_saved = false
 true when the magnetic declination has been saved More...
 
gps_message _gps_state [GPS_MAX_RECEIVERS] {}
 internal state data for the physical GPS More...
 
gps_message _gps_blended_state {}
 internal state data for the blended GPS More...
 
gps_message _gps_output [GPS_MAX_RECEIVERS+1] {}
 output state data for the physical and blended GPS More...
 
Vector2f _NE_pos_offset_m [GPS_MAX_RECEIVERS] = {}
 Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m) More...
 
float _hgt_offset_mm [GPS_MAX_RECEIVERS] = {}
 Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm) More...
 
Vector3f _blended_antenna_offset = {}
 blended antenna offset More...
 
float _blend_weights [GPS_MAX_RECEIVERS] = {}
 blend weight for each GPS. The blend weights must sum to 1.0 across all instances. More...
 
uint64_t _time_prev_us [GPS_MAX_RECEIVERS] = {}
 the previous value of time_us for that GPS instance - used to detect new data. More...
 
uint8_t _gps_best_index = 0
 index of the physical receiver with the lowest reported error More...
 
uint8_t _gps_select_index = 0
 0 = GPS1, 1 = GPS2, 2 = blended More...
 
uint8_t _gps_time_ref_index
 index of the receiver that is used as the timing reference for the blending update More...
 
uint8_t _gps_oldest_index = 0
 index of the physical receiver with the oldest data More...
 
uint8_t _gps_newest_index = 0
 index of the physical receiver with the newest data More...
 
uint8_t _gps_slowest_index = 0
 index of the physical receiver with the slowest update rate More...
 
float _gps_dt [GPS_MAX_RECEIVERS] = {}
 average time step in seconds. More...
 
bool _gps_new_output_data = false
 true if there is new output data for the EKF More...
 
bool _had_valid_terrain = false
 true if at any time there was a valid terrain estimate More...
 
int32_t _gps_alttitude_ellipsoid [GPS_MAX_RECEIVERS] {}
 altitude in 1E-3 meters (millimeters) above ellipsoid More...
 
uint64_t _gps_alttitude_ellipsoid_previous_timestamp [GPS_MAX_RECEIVERS] {}
 storage for previous timestamp to compute dt More...
 
float _wgs84_hgt_offset = 0
 height offset between AMSL and WGS84 More...
 
bool _imu_bias_reset_request {false}
 
bool new_ev_data_received = false
 
vehicle_odometry_s _ev_odom {}
 
uORB::Subscription _airdata_sub {ORB_ID(vehicle_air_data)}
 
uORB::Subscription _airspeed_sub {ORB_ID(airspeed)}
 
uORB::Subscription _ev_odom_sub {ORB_ID(vehicle_visual_odometry)}
 
uORB::Subscription _landing_target_pose_sub {ORB_ID(landing_target_pose)}
 
uORB::Subscription _magnetometer_sub {ORB_ID(vehicle_magnetometer)}
 
uORB::Subscription _optical_flow_sub {ORB_ID(optical_flow)}
 
uORB::Subscription _parameter_update_sub {ORB_ID(parameter_update)}
 
uORB::Subscription _sensor_selection_sub {ORB_ID(sensor_selection)}
 
uORB::Subscription _status_sub {ORB_ID(vehicle_status)}
 
uORB::Subscription _vehicle_land_detected_sub {ORB_ID(vehicle_land_detected)}
 
uORB::SubscriptionCallbackWorkItem _sensors_sub {this, ORB_ID(sensor_combined)}
 
uORB::Subscription _range_finder_subs [ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}
 
int _range_finder_sub_index = -1
 
uORB::Subscription _gps_subs [GPS_MAX_RECEIVERS] {{ORB_ID(vehicle_gps_position), 0}, {ORB_ID(vehicle_gps_position), 1}}
 
sensor_selection_s _sensor_selection {}
 
vehicle_land_detected_s _vehicle_land_detected {}
 
vehicle_status_s _vehicle_status {}
 
uORB::Publication< ekf2_innovations_s_estimator_innovations_pub {ORB_ID(ekf2_innovations)}
 
uORB::Publication< ekf2_timestamps_s_ekf2_timestamps_pub {ORB_ID(ekf2_timestamps)}
 
uORB::Publication< ekf_gps_drift_s_ekf_gps_drift_pub {ORB_ID(ekf_gps_drift)}
 
uORB::Publication< ekf_gps_position_s_blended_gps_pub {ORB_ID(ekf_gps_position)}
 
uORB::Publication< estimator_status_s_estimator_status_pub {ORB_ID(estimator_status)}
 
uORB::Publication< sensor_bias_s_sensor_bias_pub {ORB_ID(sensor_bias)}
 
uORB::Publication< vehicle_attitude_s_att_pub {ORB_ID(vehicle_attitude)}
 
uORB::Publication< vehicle_odometry_s_vehicle_odometry_pub {ORB_ID(vehicle_odometry)}
 
uORB::PublicationMulti< wind_estimate_s_wind_pub {ORB_ID(wind_estimate)}
 
uORB::PublicationData< vehicle_global_position_s_vehicle_global_position_pub {ORB_ID(vehicle_global_position)}
 
uORB::PublicationData< vehicle_local_position_s_vehicle_local_position_pub {ORB_ID(vehicle_local_position)}
 
uORB::PublicationData< vehicle_odometry_s_vehicle_visual_odometry_aligned_pub {ORB_ID(vehicle_visual_odometry_aligned)}
 
Ekf _ekf
 
parameters_params
 pointer to ekf parameter struct (located in _ekf class instance) More...
 

Static Private Attributes

static constexpr float ep_max_std_dev = 100.0f
 Maximum permissible standard deviation for estimated position. More...
 
static constexpr float eo_max_std_dev = 100.0f
 Maximum permissible standard deviation for estimated orientation. More...
 

Detailed Description

Definition at line 95 of file ekf2_main.cpp.

Constructor & Destructor Documentation

◆ Ekf2()

Ekf2::Ekf2 ( bool  replay_mode = false)
explicit

Definition at line 533 of file ekf2_main.cpp.

References Ekf::set_min_required_gps_health_time().

Referenced by task_spawn().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ~Ekf2()

Ekf2::~Ekf2 ( )
override

Definition at line 641 of file ekf2_main.cpp.

References perf_free().

Here is the call graph for this function:

Member Function Documentation

◆ apply_gps_offsets()

void Ekf2::apply_gps_offsets ( )
private

Definition at line 2235 of file ekf2_main.cpp.

References _gps_output, _gps_state, add_vector_to_global_position(), estimator::gps_message::alt, estimator::gps_message::eph, estimator::gps_message::epv, estimator::gps_message::fix_type, GPS_MAX_RECEIVERS, estimator::gps_message::lat, estimator::gps_message::lon, estimator::gps_message::nsats, estimator::gps_message::pdop, estimator::gps_message::sacc, estimator::gps_message::time_usec, estimator::gps_message::vel_m_s, estimator::gps_message::vel_ned, estimator::gps_message::vel_ned_valid, estimator::gps_message::yaw, and estimator::gps_message::yaw_offset.

Referenced by Run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ blend_gps_data()

bool Ekf2::blend_gps_data ( )
private

Definition at line 1778 of file ekf2_main.cpp.

References _gps_blended_state, _gps_newest_index, _gps_slowest_index, _gps_state, _gps_time_ref_index, BLEND_MASK_USE_HPOS_ACC, BLEND_MASK_USE_SPD_ACC, BLEND_MASK_USE_VPOS_ACC, estimator::gps_message::eph, estimator::gps_message::epv, f(), GPS_MAX_RECEIVERS, estimator::gps_message::sacc, estimator::gps_message::time_usec, update_gps_blend_states(), and update_gps_offsets().

Referenced by Run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ calc_gps_blend_output()

void Ekf2::calc_gps_blend_output ( )
private

◆ custom_command()

int Ekf2::custom_command ( int  argc,
char *  argv[] 
)
static
See also
ModuleBase

Definition at line 2351 of file ekf2_main.cpp.

References print_usage().

Here is the call graph for this function:

◆ fillGpsMsgWithVehicleGpsPosData()

◆ filter_altitude_ellipsoid()

float Ekf2::filter_altitude_ellipsoid ( float  amsl_hgt)
private

Definition at line 2329 of file ekf2_main.cpp.

References _gps_alttitude_ellipsoid, _gps_alttitude_ellipsoid_previous_timestamp, _gps_state, _wgs84_hgt_offset, math::constrain(), dt, f(), and estimator::gps_message::time_usec.

Referenced by Run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_vel_body_wind()

const Vector3f Ekf2::get_vel_body_wind ( )
private

Definition at line 1758 of file ekf2_main.cpp.

References EstimatorInterface::get_quaternion(), EstimatorInterface::get_velocity(), Ekf::get_wind_velocity(), matrix::Quaternion< Type >::inversed(), and velocity.

Referenced by Run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getRangeSubIndex()

int Ekf2::getRangeSubIndex ( )
private

get subscription index of first downward-facing range sensor

Definition at line 1687 of file ekf2_main.cpp.

References _range_finder_subs, and ORB_MULTI_MAX_INSTANCES.

Referenced by Run().

Here is the caller graph for this function:

◆ init()

bool Ekf2::init ( )

Definition at line 647 of file ekf2_main.cpp.

References _sensors_sub, and uORB::SubscriptionCallback::registerCallback().

Referenced by task_spawn().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ print_status()

int Ekf2::print_status ( )
override

Definition at line 657 of file ekf2_main.cpp.

References Ekf::global_position_is_valid(), EstimatorInterface::local_position_is_valid(), and perf_print_counter().

Here is the call graph for this function:

◆ print_usage()

int Ekf2::print_usage ( const char *  reason = nullptr)
static
See also
ModuleBase

Definition at line 2386 of file ekf2_main.cpp.

Referenced by custom_command().

Here is the caller graph for this function:

◆ publish_attitude()

bool Ekf2::publish_attitude ( const sensor_combined_s sensors,
const hrt_abstime now 
)
private

Definition at line 1704 of file ekf2_main.cpp.

References _att_pub, EstimatorInterface::attitude_valid(), Ekf::calculate_quaternion(), vehicle_attitude_s::delta_q_reset, Ekf::get_quat_reset(), uORB::Publication< T >::publish(), vehicle_attitude_s::q, vehicle_attitude_s::quat_reset_counter, and vehicle_attitude_s::timestamp.

Referenced by Run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ publish_wind_estimate()

bool Ekf2::publish_wind_estimate ( const hrt_abstime timestamp)
private

Definition at line 1730 of file ekf2_main.cpp.

References _wind_pub, Ekf::get_airspeed_innov(), Ekf::get_airspeed_innov_var(), Ekf::get_beta_innov(), Ekf::get_beta_innov_var(), EstimatorInterface::get_wind_status(), Ekf::get_wind_velocity(), Ekf::get_wind_velocity_var(), and uORB::PublicationMulti< T >::publish().

Referenced by Run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ resetPreFlightChecks()

void Ekf2::resetPreFlightChecks ( )
private

Definition at line 1682 of file ekf2_main.cpp.

References PreFlightChecker::reset().

Referenced by Run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ Run()

void Ekf2::Run ( )
override

Definition at line 703 of file ekf2_main.cpp.

References _airdata_sub, _airspeed_sub, _balt_sample_count, _blended_gps_pub, _ekf2_timestamps_pub, _ekf_gps_drift_pub, _estimator_innovations_pub, _estimator_status_pub, _ev_odom, _ev_odom_sub, _gps_alttitude_ellipsoid, _gps_output, _gps_select_index, _gps_state, _gps_subs, _imu_bias_reset_request, _landing_target_pose_sub, _last_magcal_us, _mag_sample_count, _magnetometer_sub, _optical_flow_sub, _parameter_update_sub, _range_finder_subs, _sensor_bias_pub, _sensor_selection, _sensor_selection_sub, _sensors_sub, _start_time_us, _status_sub, _vehicle_global_position_pub, _vehicle_land_detected, _vehicle_land_detected_sub, _vehicle_local_position_pub, _vehicle_odometry_pub, _vehicle_status, _vehicle_visual_odometry_aligned_pub, sensor_selection_s::accel_device_id, sensor_combined_s::accelerometer_integral_dt, sensor_combined_s::accelerometer_m_s2, ekf2_innovations_s::airspeed_innov, ekf2_innovations_s::airspeed_innov_var, vehicle_global_position_s::alt, ekf_gps_position_s::alt, estimator::gps_message::alt, vehicle_global_position_s::alt_ellipsoid, vehicle_gps_position_s::alt_ellipsoid, estimator::ext_vision_message::angErr, apply_gps_offsets(), vehicle_status_s::arming_state, ekf2_innovations_s::aux_vel_innov, vehicle_local_position_s::ax, vehicle_local_position_s::ay, vehicle_local_position_s::az, vehicle_air_data_s::baro_alt_meter, ekf2_innovations_s::beta_innov, ekf2_innovations_s::beta_innov_var, estimator_status_s::beta_test_ratio, blend_gps_data(), ekf_gps_drift_s::blocked, calc_gps_blend_output(), CONSTANTS_ONE_G, estimator_status_s::control_mode_flags, uORB::Subscription::copy(), matrix::Matrix< Type, M, N >::copyTo(), landing_target_pose_s::cov_vx_rel, landing_target_pose_s::cov_vy_rel, estimator_status_s::covariances, Ekf::covariances_diagonal(), distance_sensor_s::current_distance, vehicle_global_position_s::dead_reckoning, vehicle_global_position_s::delta_alt, estimator::imuSample::delta_ang, estimator::imuSample::delta_ang_dt, estimator::imuSample::delta_vel, estimator::imuSample::delta_vel_dt, vehicle_local_position_s::delta_vxy, vehicle_local_position_s::delta_vz, vehicle_local_position_s::delta_xy, vehicle_local_position_s::delta_z, vehicle_local_position_s::dist_bottom, vehicle_local_position_s::dist_bottom_rate, vehicle_local_position_s::dist_bottom_valid, ekf2_innovations_s::drag_innov, ekf2_innovations_s::drag_innov_var, estimator::flow_message::dt, ekf_gps_position_s::eph, vehicle_global_position_s::eph, estimator::gps_message::eph, vehicle_local_position_s::eph, ekf_gps_position_s::epv, vehicle_global_position_s::epv, estimator::gps_message::epv, vehicle_local_position_s::epv, vehicle_local_position_s::evh, vehicle_local_position_s::evv, f(), fillGpsMsgWithVehicleGpsPosData(), filter_altitude_ellipsoid(), estimator_status_s::filter_fault_flags, estimator::gps_message::fix_type, ekf_gps_position_s::fix_type, estimator::filter_control_status_u::flags, ekf2_innovations_s::flow_innov, ekf2_innovations_s::flow_innov_var, estimator::flow_message::flowdata, FLT_EPSILON, uORB::PublicationData< T >::get(), Ekf::get_accel_bias(), Ekf::get_airspeed_innov(), Ekf::get_airspeed_innov_var(), Ekf::get_aux_vel_innov(), Ekf::get_beta_innov(), Ekf::get_beta_innov_var(), EstimatorInterface::get_control_mode(), Ekf::get_drag_innov(), Ekf::get_drag_innov_var(), Ekf::get_ekf_ctrl_limits(), Ekf::get_ekf_gpos_accuracy(), Ekf::get_ekf_lpos_accuracy(), Ekf::get_ekf_origin(), Ekf::get_ekf_soln_status(), Ekf::get_ekf_vel_accuracy(), Ekf::get_ev2ekf_quaternion(), EstimatorInterface::get_filter_fault_status(), Ekf::get_flow_innov(), Ekf::get_flow_innov_var(), Ekf::get_gps_check_status(), Ekf::get_gps_drift_metrics(), Ekf::get_gyro_bias(), EstimatorInterface::get_hagl_innov(), EstimatorInterface::get_hagl_innov_var(), Ekf::get_heading_innov(), Ekf::get_heading_innov_var(), Ekf::get_imu_vibe_metrics(), Ekf::get_innovation_test_status(), Ekf::get_mag_innov(), Ekf::get_mag_innov_var(), Ekf::get_output_tracking_error(), EstimatorInterface::get_pos_d_deriv(), Ekf::get_posD_reset(), EstimatorInterface::get_position(), Ekf::get_posNE_reset(), EstimatorInterface::get_quaternion(), Ekf::get_state_delayed(), EstimatorInterface::get_terrain_valid(), EstimatorInterface::get_terrain_vert_pos(), get_vel_body_wind(), EstimatorInterface::get_vel_deriv_ned(), Ekf::get_vel_pos_innov(), Ekf::get_vel_pos_innov_var(), Ekf::get_velD_reset(), Ekf::get_velNE_reset(), EstimatorInterface::get_velocity(), getRangeSubIndex(), Ekf::global_position_is_valid(), gps, estimator_status_s::gps_check_fail_flags, estimator::parameters::gps_check_mask, sensor_selection_s::gyro_device_id, sensor_combined_s::gyro_integral_dt, sensor_combined_s::gyro_rad, optical_flow_s::gyro_x_rate_integral, optical_flow_s::gyro_y_rate_integral, optical_flow_s::gyro_z_rate_integral, estimator::flow_message::gyrodata, ekf2_innovations_s::hagl_innov, ekf2_innovations_s::hagl_innov_var, vehicle_local_position_s::hagl_max, vehicle_local_position_s::hagl_min, estimator_status_s::hagl_test_ratio, PreFlightChecker::hasFailed(), PreFlightChecker::hasHeadingFailed(), PreFlightChecker::hasHeightFailed(), PreFlightChecker::hasHorizFailed(), PreFlightChecker::hasHorizVelFailed(), PreFlightChecker::hasVertFailed(), PreFlightChecker::hasVertVelFailed(), ekf_gps_position_s::heading, ekf2_innovations_s::heading_innov, ekf2_innovations_s::heading_innov_var, ekf_gps_position_s::heading_offset, estimator_status_s::health_flags, estimator_status_s::hgt_test_ratio, estimator::ext_vision_message::hgtErr, ekf_gps_drift_s::hpos_drift_rate, hrt_abstime, ekf_gps_drift_s::hspd, vehicle_land_detected_s::in_ground_effect, airspeed_s::indicated_airspeed_m_s, EstimatorInterface::inertial_dead_reckoning(), estimator_status_s::innovation_check_flags, optical_flow_s::integration_timespan, is_fixed_wing(), landing_target_pose_s::is_static, vehicle_land_detected_s::landed, ekf_gps_position_s::lat, vehicle_global_position_s::lat, estimator::gps_message::lat, vehicle_global_position_s::lat_lon_reset_counter, map_projection_reference_s::lat_rad, EstimatorInterface::local_position_is_valid(), ekf_gps_position_s::lon, vehicle_global_position_s::lon, estimator::gps_message::lon, map_projection_reference_s::lon_rad, M_PI, estimator::filter_control_status_u::mag_3D, sensor_selection_s::mag_device_id, estimator::filter_control_status_u::mag_field_disturbed, ekf2_innovations_s::mag_innov, ekf2_innovations_s::mag_innov_var, estimator_status_s::mag_test_ratio, vehicle_magnetometer_s::magnetometer_ga, map_projection_reproject(), distance_sensor_s::max_distance, optical_flow_s::max_flow_rate, optical_flow_s::max_ground_distance, distance_sensor_s::min_distance, optical_flow_s::min_ground_distance, estimator_status_s::n_states, estimator::gps_message::nsats, ekf2_innovations_s::output_tracking_error, perf_begin(), perf_end(), optical_flow_s::pixel_flow_x_integral, optical_flow_s::pixel_flow_y_integral, estimator::ext_vision_message::pos, estimator_status_s::pos_horiz_accuracy, estimator_status_s::pos_test_ratio, estimator_status_s::pos_vert_accuracy, vehicle_odometry_s::pose_covariance, estimator::ext_vision_message::posErr, position, estimator_status_s::pre_flt_fail_innov_heading, estimator_status_s::pre_flt_fail_innov_height, estimator_status_s::pre_flt_fail_innov_vel_horiz, estimator_status_s::pre_flt_fail_innov_vel_vert, estimator_status_s::pre_flt_fail_mag_field_disturbed, uORB::Publication< T >::publish(), publish_attitude(), publish_wind_estimate(), vehicle_odometry_s::q, vehicle_odometry_s::q_offset, optical_flow_s::quality, estimator::flow_message::quality, estimator::ext_vision_message::quat, vehicle_local_position_s::ref_alt, vehicle_local_position_s::ref_lat, vehicle_local_position_s::ref_lon, vehicle_local_position_s::ref_timestamp, landing_target_pose_s::rel_vel_valid, Ekf::reset_imu_bias(), resetPreFlightChecks(), vehicle_air_data_s::rho, runPreFlightChecks(), ekf_gps_position_s::s_variance_m_s, estimator::gps_message::sacc, ekf_gps_position_s::satellites_used, ekf_gps_position_s::selected, estimator::parameters::sensor_interval_min_ms, EstimatorInterface::set_air_density(), EstimatorInterface::set_fuse_beta_flag(), EstimatorInterface::set_gnd_effect_flag(), EstimatorInterface::set_in_air_status(), EstimatorInterface::set_is_fixed_wing(), EstimatorInterface::set_optical_flow_limits(), EstimatorInterface::set_rangefinder_limits(), EstimatorInterface::setAirspeedData(), EstimatorInterface::setAuxVelData(), EstimatorInterface::setBaroData(), EstimatorInterface::setExtVisionData(), EstimatorInterface::setGpsData(), EstimatorInterface::setIMUData(), EstimatorInterface::setMagData(), EstimatorInterface::setOpticalFlowData(), EstimatorInterface::setRangeData(), distance_sensor_s::signal_quality, estimator_status_s::solution_status_flags, estimator_status_s::states, status, estimator_status_s::tas_test_ratio, vehicle_global_position_s::terrain_alt, vehicle_global_position_s::terrain_alt_valid, estimator::filter_control_status_u::tilt_align, estimator_status_s::time_slip, estimator::imuSample::time_us, estimator::gps_message::time_usec, estimator_status_s::timeout_flags, vehicle_air_data_s::timestamp, ekf_gps_position_s::timestamp, vehicle_gps_position_s::timestamp, ekf_gps_drift_s::timestamp, landing_target_pose_s::timestamp, optical_flow_s::timestamp, vehicle_magnetometer_s::timestamp, sensor_bias_s::timestamp, vehicle_local_position_s::timestamp, airspeed_s::timestamp, ekf2_innovations_s::timestamp, sensor_selection_s::timestamp, vehicle_global_position_s::timestamp, sensor_combined_s::timestamp, ekf2_timestamps_s::timestamp, vehicle_odometry_s::timestamp, distance_sensor_s::timestamp, estimator_status_s::timestamp, airspeed_s::true_airspeed_m_s, uORB::SubscriptionCallback::unregisterCallback(), Ekf::update(), uORB::SubscriptionInterval::update(), uORB::Subscription::update(), uORB::PublicationData< T >::update(), update_mag_bias(), update_mag_decl(), uORB::Subscription::updated(), vehicle_local_position_s::v_xy_valid, vehicle_local_position_s::v_z_valid, estimator::filter_control_status_u::value, vehicle_status_s::vehicle_type, VEHICLE_TYPE_FIXED_WING, estimator::ext_vision_message::vel, vehicle_global_position_s::vel_d, ekf_gps_position_s::vel_d_m_s, vehicle_global_position_s::vel_e, ekf_gps_position_s::vel_e_m_s, ekf_gps_position_s::vel_m_s, estimator::gps_message::vel_m_s, vehicle_global_position_s::vel_n, ekf_gps_position_s::vel_n_m_s, estimator::gps_message::vel_ned, ekf_gps_position_s::vel_ned_valid, estimator::gps_message::vel_ned_valid, ekf2_innovations_s::vel_pos_innov, ekf2_innovations_s::vel_pos_innov_var, estimator_status_s::vel_test_ratio, estimator::ext_vision_message::velErr, velocity, estimator_status_s::vibe, ekf_gps_drift_s::vpos_drift_rate, vehicle_local_position_s::vx, vehicle_odometry_s::vx, landing_target_pose_s::vx_rel, vehicle_local_position_s::vxy_max, vehicle_local_position_s::vxy_reset_counter, vehicle_local_position_s::vy, vehicle_odometry_s::vy, landing_target_pose_s::vy_rel, vehicle_local_position_s::vz, vehicle_odometry_s::vz, vehicle_local_position_s::vz_max, vehicle_local_position_s::vz_reset_counter, vehicle_local_position_s::x, vehicle_odometry_s::x, vehicle_local_position_s::xy_global, vehicle_local_position_s::xy_reset_counter, vehicle_local_position_s::xy_valid, vehicle_local_position_s::y, vehicle_odometry_s::y, vehicle_global_position_s::yaw, estimator::gps_message::yaw, vehicle_local_position_s::yaw, estimator::gps_message::yaw_offset, vehicle_local_position_s::z, vehicle_odometry_s::z, vehicle_local_position_s::z_deriv, vehicle_local_position_s::z_global, vehicle_local_position_s::z_reset_counter, and vehicle_local_position_s::z_valid.

◆ runPreFlightChecks()

void Ekf2::runPreFlightChecks ( float  dt,
const filter_control_status_u control_status,
const vehicle_status_s vehicle_status,
const ekf2_innovations_s innov 
)
private

Definition at line 1667 of file ekf2_main.cpp.

References estimator::filter_control_status_u::ev_pos, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::gps, estimator::filter_control_status_u::opt_flow, PreFlightChecker::setUsingEvPosAiding(), PreFlightChecker::setUsingFlowAiding(), PreFlightChecker::setUsingGpsAiding(), PreFlightChecker::setVehicleCanObserveHeadingInFlight(), PreFlightChecker::update(), and vehicle_status_s::vehicle_type.

Referenced by Run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ task_spawn()

int Ekf2::task_spawn ( int  argc,
char *  argv[] 
)
static
See also
ModuleBase

Definition at line 2356 of file ekf2_main.cpp.

References Ekf2(), init(), and ll40ls::instance.

Here is the call graph for this function:

◆ update_gps_blend_states()

void Ekf2::update_gps_blend_states ( )
private

◆ update_gps_offsets()

void Ekf2::update_gps_offsets ( )
private

Definition at line 2168 of file ekf2_main.cpp.

References _gps_blended_state, _gps_state, estimator::gps_message::alt, math::constrain(), f(), get_vector_to_next_waypoint(), GPS_MAX_RECEIVERS, estimator::gps_message::lat, estimator::gps_message::lon, and estimator::gps_message::time_usec.

Referenced by blend_gps_data().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ update_mag_bias()

template<typename Param >
void Ekf2::update_mag_bias ( Param &  mag_bias_param,
int  axis_index 
)
private

Definition at line 670 of file ekf2_main.cpp.

References math::constrain(), and f().

Referenced by Run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ update_mag_decl()

template<typename Param >
bool Ekf2::update_mag_decl ( Param &  mag_decl_param)
private

Definition at line 689 of file ekf2_main.cpp.

References EstimatorInterface::get_mag_decl_deg().

Referenced by Run().

Here is the call graph for this function:
Here is the caller graph for this function:

Member Data Documentation

◆ _airdata_sub

uORB::Subscription Ekf2::_airdata_sub {ORB_ID(vehicle_air_data)}
private

Definition at line 246 of file ekf2_main.cpp.

Referenced by Run().

◆ _airspeed_sub

uORB::Subscription Ekf2::_airspeed_sub {ORB_ID(airspeed)}
private

Definition at line 247 of file ekf2_main.cpp.

Referenced by Run().

◆ _att_pub

uORB::Publication<vehicle_attitude_s> Ekf2::_att_pub {ORB_ID(vehicle_attitude)}
private

Definition at line 276 of file ekf2_main.cpp.

Referenced by publish_attitude().

◆ _balt_data_sum

float Ekf2::_balt_data_sum = 0.0f
private

summed pressure altitude readings (m)

Definition at line 193 of file ekf2_main.cpp.

◆ _balt_sample_count

uint8_t Ekf2::_balt_sample_count = 0
private

number of barometric altitude measurements summed

Definition at line 195 of file ekf2_main.cpp.

Referenced by Run().

◆ _balt_time_ms_last_used

uint32_t Ekf2::_balt_time_ms_last_used
private
Initial value:
=
0

time stamp of the last averaged barometric altitude measurement sent to the EKF (mSec)

Definition at line 196 of file ekf2_main.cpp.

◆ _balt_time_sum_ms

uint64_t Ekf2::_balt_time_sum_ms = 0
private

summed pressure altitude time stamps (mSec)

Definition at line 194 of file ekf2_main.cpp.

◆ _blend_weights

float Ekf2::_blend_weights[GPS_MAX_RECEIVERS] = {}
private

blend weight for each GPS. The blend weights must sum to 1.0 across all instances.

Definition at line 223 of file ekf2_main.cpp.

◆ _blended_antenna_offset

Vector3f Ekf2::_blended_antenna_offset = {}
private

blended antenna offset

Definition at line 222 of file ekf2_main.cpp.

◆ _blended_gps_pub

uORB::Publication<ekf_gps_position_s> Ekf2::_blended_gps_pub {ORB_ID(ekf_gps_position)}
private

Definition at line 273 of file ekf2_main.cpp.

Referenced by Run().

◆ _ekf

Ekf Ekf2::_ekf
private

Definition at line 283 of file ekf2_main.cpp.

◆ _ekf2_timestamps_pub

uORB::Publication<ekf2_timestamps_s> Ekf2::_ekf2_timestamps_pub {ORB_ID(ekf2_timestamps)}
private

Definition at line 271 of file ekf2_main.cpp.

Referenced by Run().

◆ _ekf_gps_drift_pub

uORB::Publication<ekf_gps_drift_s> Ekf2::_ekf_gps_drift_pub {ORB_ID(ekf_gps_drift)}
private

Definition at line 272 of file ekf2_main.cpp.

Referenced by Run().

◆ _ekf_update_perf

perf_counter_t Ekf2::_ekf_update_perf
private

Definition at line 181 of file ekf2_main.cpp.

◆ _estimator_innovations_pub

uORB::Publication<ekf2_innovations_s> Ekf2::_estimator_innovations_pub {ORB_ID(ekf2_innovations)}
private

Definition at line 270 of file ekf2_main.cpp.

Referenced by Run().

◆ _estimator_status_pub

uORB::Publication<estimator_status_s> Ekf2::_estimator_status_pub {ORB_ID(estimator_status)}
private

Definition at line 274 of file ekf2_main.cpp.

Referenced by Run().

◆ _ev_odom

vehicle_odometry_s Ekf2::_ev_odom {}
private

Definition at line 244 of file ekf2_main.cpp.

Referenced by Run().

◆ _ev_odom_sub

uORB::Subscription Ekf2::_ev_odom_sub {ORB_ID(vehicle_visual_odometry)}
private

Definition at line 248 of file ekf2_main.cpp.

Referenced by Run().

◆ _gps_alttitude_ellipsoid

int32_t Ekf2::_gps_alttitude_ellipsoid[GPS_MAX_RECEIVERS] {}
private

altitude in 1E-3 meters (millimeters) above ellipsoid

Definition at line 236 of file ekf2_main.cpp.

Referenced by filter_altitude_ellipsoid(), and Run().

◆ _gps_alttitude_ellipsoid_previous_timestamp

uint64_t Ekf2::_gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}
private

storage for previous timestamp to compute dt

Definition at line 237 of file ekf2_main.cpp.

Referenced by filter_altitude_ellipsoid().

◆ _gps_best_index

uint8_t Ekf2::_gps_best_index = 0
private

index of the physical receiver with the lowest reported error

Definition at line 225 of file ekf2_main.cpp.

◆ _gps_blended_state

gps_message Ekf2::_gps_blended_state {}
private

internal state data for the blended GPS

Definition at line 218 of file ekf2_main.cpp.

Referenced by blend_gps_data(), calc_gps_blend_output(), update_gps_blend_states(), and update_gps_offsets().

◆ _gps_dt

float Ekf2::_gps_dt[GPS_MAX_RECEIVERS] = {}
private

average time step in seconds.

Definition at line 232 of file ekf2_main.cpp.

◆ _gps_new_output_data

bool Ekf2::_gps_new_output_data = false
private

true if there is new output data for the EKF

Definition at line 233 of file ekf2_main.cpp.

◆ _gps_newest_index

uint8_t Ekf2::_gps_newest_index = 0
private

index of the physical receiver with the newest data

Definition at line 230 of file ekf2_main.cpp.

Referenced by blend_gps_data().

◆ _gps_oldest_index

uint8_t Ekf2::_gps_oldest_index = 0
private

index of the physical receiver with the oldest data

Definition at line 229 of file ekf2_main.cpp.

◆ _gps_output

gps_message Ekf2::_gps_output[GPS_MAX_RECEIVERS+1] {}
private

output state data for the physical and blended GPS

Definition at line 219 of file ekf2_main.cpp.

Referenced by apply_gps_offsets(), calc_gps_blend_output(), and Run().

◆ _gps_select_index

uint8_t Ekf2::_gps_select_index = 0
private

0 = GPS1, 1 = GPS2, 2 = blended

Definition at line 226 of file ekf2_main.cpp.

Referenced by Run().

◆ _gps_slowest_index

uint8_t Ekf2::_gps_slowest_index = 0
private

index of the physical receiver with the slowest update rate

Definition at line 231 of file ekf2_main.cpp.

Referenced by blend_gps_data().

◆ _gps_state

gps_message Ekf2::_gps_state[GPS_MAX_RECEIVERS] {}
private

internal state data for the physical GPS

Definition at line 217 of file ekf2_main.cpp.

Referenced by apply_gps_offsets(), blend_gps_data(), filter_altitude_ellipsoid(), Run(), update_gps_blend_states(), and update_gps_offsets().

◆ _gps_subs

uORB::Subscription Ekf2::_gps_subs[GPS_MAX_RECEIVERS] {{ORB_ID(vehicle_gps_position), 0}, {ORB_ID(vehicle_gps_position), 1}}
private

Definition at line 264 of file ekf2_main.cpp.

Referenced by Run().

◆ _gps_time_ref_index

uint8_t Ekf2::_gps_time_ref_index
private
Initial value:
=
0

index of the receiver that is used as the timing reference for the blending update

Definition at line 227 of file ekf2_main.cpp.

Referenced by blend_gps_data().

◆ _had_valid_terrain

bool Ekf2::_had_valid_terrain = false
private

true if at any time there was a valid terrain estimate

Definition at line 234 of file ekf2_main.cpp.

◆ _hgt_offset_mm

float Ekf2::_hgt_offset_mm[GPS_MAX_RECEIVERS] = {}
private

Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm)

Definition at line 221 of file ekf2_main.cpp.

◆ _imu_bias_reset_request

bool Ekf2::_imu_bias_reset_request {false}
private

Definition at line 240 of file ekf2_main.cpp.

Referenced by Run().

◆ _integrated_time_us

uint64_t Ekf2::_integrated_time_us = 0
private

integral of gyro delta time from start (uSec)

Definition at line 177 of file ekf2_main.cpp.

◆ _invalid_mag_id_count

uint8_t Ekf2::_invalid_mag_id_count = 0
private

number of times an invalid magnetomer device ID has been detected

Definition at line 184 of file ekf2_main.cpp.

◆ _landing_target_pose_sub

uORB::Subscription Ekf2::_landing_target_pose_sub {ORB_ID(landing_target_pose)}
private

Definition at line 249 of file ekf2_main.cpp.

Referenced by Run().

◆ _last_magcal_us

hrt_abstime Ekf2::_last_magcal_us = 0
private

last time the EKF was operating a mode that estimates magnetomer biases (uSec)

Definition at line 200 of file ekf2_main.cpp.

Referenced by Run().

◆ _last_time_slip_us

int64_t Ekf2::_last_time_slip_us = 0
private

Last time slip (uSec)

Definition at line 179 of file ekf2_main.cpp.

◆ _last_valid_mag_cal

float Ekf2::_last_valid_mag_cal[3] = {}
private

last valid XYZ magnetometer bias estimates (mGauss)

Definition at line 203 of file ekf2_main.cpp.

◆ _last_valid_variance

float Ekf2::_last_valid_variance[3] = {}
private

variances for the last valid magnetometer XYZ bias estimates (mGauss**2)

Definition at line 205 of file ekf2_main.cpp.

◆ _mag_data_sum

float Ekf2::_mag_data_sum[3] = {}
private

summed magnetometer readings (Gauss)

Definition at line 187 of file ekf2_main.cpp.

◆ _mag_decl_saved

bool Ekf2::_mag_decl_saved = false
private

true when the magnetic declination has been saved

Definition at line 208 of file ekf2_main.cpp.

◆ _mag_sample_count

uint8_t Ekf2::_mag_sample_count = 0
private

number of magnetometer measurements summed during downsampling

Definition at line 189 of file ekf2_main.cpp.

Referenced by Run().

◆ _mag_time_ms_last_used

int32_t Ekf2::_mag_time_ms_last_used = 0
private

time stamp of the last averaged magnetometer measurement sent to the EKF (mSec)

Definition at line 190 of file ekf2_main.cpp.

◆ _mag_time_sum_ms

uint64_t Ekf2::_mag_time_sum_ms = 0
private

summed magnetoemter time stamps (mSec)

Definition at line 188 of file ekf2_main.cpp.

◆ _magnetometer_sub

uORB::Subscription Ekf2::_magnetometer_sub {ORB_ID(vehicle_magnetometer)}
private

Definition at line 250 of file ekf2_main.cpp.

Referenced by Run().

◆ _NE_pos_offset_m

Vector2f Ekf2::_NE_pos_offset_m[GPS_MAX_RECEIVERS] = {}
private

Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)

Definition at line 220 of file ekf2_main.cpp.

◆ _optical_flow_sub

uORB::Subscription Ekf2::_optical_flow_sub {ORB_ID(optical_flow)}
private

Definition at line 251 of file ekf2_main.cpp.

Referenced by Run().

◆ _parameter_update_sub

uORB::Subscription Ekf2::_parameter_update_sub {ORB_ID(parameter_update)}
private

Definition at line 252 of file ekf2_main.cpp.

Referenced by Run().

◆ _params

parameters* Ekf2::_params
private

pointer to ekf parameter struct (located in _ekf class instance)

Definition at line 285 of file ekf2_main.cpp.

◆ _preflt_checker

PreFlightChecker Ekf2::_preflt_checker
private

Definition at line 120 of file ekf2_main.cpp.

◆ _range_finder_sub_index

int Ekf2::_range_finder_sub_index = -1
private

Definition at line 261 of file ekf2_main.cpp.

◆ _range_finder_subs

uORB::Subscription Ekf2::_range_finder_subs[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}
private

Definition at line 260 of file ekf2_main.cpp.

Referenced by getRangeSubIndex(), and Run().

◆ _replay_mode

const bool Ekf2::_replay_mode
private

true when we use replay data from a log

Definition at line 174 of file ekf2_main.cpp.

◆ _sensor_bias_pub

uORB::Publication<sensor_bias_s> Ekf2::_sensor_bias_pub {ORB_ID(sensor_bias)}
private

Definition at line 275 of file ekf2_main.cpp.

Referenced by Run().

◆ _sensor_selection

sensor_selection_s Ekf2::_sensor_selection {}
private

Definition at line 266 of file ekf2_main.cpp.

Referenced by Run().

◆ _sensor_selection_sub

uORB::Subscription Ekf2::_sensor_selection_sub {ORB_ID(sensor_selection)}
private

Definition at line 253 of file ekf2_main.cpp.

Referenced by Run().

◆ _sensors_sub

uORB::SubscriptionCallbackWorkItem Ekf2::_sensors_sub {this, ORB_ID(sensor_combined)}
private

Definition at line 257 of file ekf2_main.cpp.

Referenced by init(), and Run().

◆ _start_time_us

uint64_t Ekf2::_start_time_us = 0
private

system time at EKF start (uSec)

Definition at line 178 of file ekf2_main.cpp.

Referenced by Run().

◆ _status_sub

uORB::Subscription Ekf2::_status_sub {ORB_ID(vehicle_status)}
private

Definition at line 254 of file ekf2_main.cpp.

Referenced by Run().

◆ _time_prev_us

uint64_t Ekf2::_time_prev_us[GPS_MAX_RECEIVERS] = {}
private

the previous value of time_us for that GPS instance - used to detect new data.

Definition at line 224 of file ekf2_main.cpp.

◆ _total_cal_time_us

hrt_abstime Ekf2::_total_cal_time_us = 0
private

accumulated calibration time since the last save

Definition at line 201 of file ekf2_main.cpp.

◆ _valid_cal_available

bool Ekf2::_valid_cal_available[3] = {}
private

true when an unsaved valid calibration for the XYZ magnetometer bias is available

Definition at line 204 of file ekf2_main.cpp.

◆ _vehicle_global_position_pub

uORB::PublicationData<vehicle_global_position_s> Ekf2::_vehicle_global_position_pub {ORB_ID(vehicle_global_position)}
private

Definition at line 279 of file ekf2_main.cpp.

Referenced by Run().

◆ _vehicle_land_detected

vehicle_land_detected_s Ekf2::_vehicle_land_detected {}
private

Definition at line 267 of file ekf2_main.cpp.

Referenced by Run().

◆ _vehicle_land_detected_sub

uORB::Subscription Ekf2::_vehicle_land_detected_sub {ORB_ID(vehicle_land_detected)}
private

Definition at line 255 of file ekf2_main.cpp.

Referenced by Run().

◆ _vehicle_local_position_pub

uORB::PublicationData<vehicle_local_position_s> Ekf2::_vehicle_local_position_pub {ORB_ID(vehicle_local_position)}
private

Definition at line 280 of file ekf2_main.cpp.

Referenced by Run().

◆ _vehicle_odometry_pub

uORB::Publication<vehicle_odometry_s> Ekf2::_vehicle_odometry_pub {ORB_ID(vehicle_odometry)}
private

Definition at line 277 of file ekf2_main.cpp.

Referenced by Run().

◆ _vehicle_status

vehicle_status_s Ekf2::_vehicle_status {}
private

Definition at line 268 of file ekf2_main.cpp.

Referenced by Run().

◆ _vehicle_visual_odometry_aligned_pub

uORB::PublicationData<vehicle_odometry_s> Ekf2::_vehicle_visual_odometry_aligned_pub {ORB_ID(vehicle_visual_odometry_aligned)}
private

Definition at line 281 of file ekf2_main.cpp.

Referenced by Run().

◆ _wgs84_hgt_offset

float Ekf2::_wgs84_hgt_offset = 0
private

height offset between AMSL and WGS84

Definition at line 238 of file ekf2_main.cpp.

Referenced by filter_altitude_ellipsoid().

◆ _wind_pub

uORB::PublicationMulti<wind_estimate_s> Ekf2::_wind_pub {ORB_ID(wind_estimate)}
private

Definition at line 278 of file ekf2_main.cpp.

Referenced by publish_wind_estimate().

◆ eo_max_std_dev

constexpr float Ekf2::eo_max_std_dev = 100.0f
staticprivate

Maximum permissible standard deviation for estimated orientation.

Definition at line 213 of file ekf2_main.cpp.

◆ ep_max_std_dev

constexpr float Ekf2::ep_max_std_dev = 100.0f
staticprivate

Maximum permissible standard deviation for estimated position.

Definition at line 212 of file ekf2_main.cpp.

◆ new_ev_data_received

bool Ekf2::new_ev_data_received = false
private

Definition at line 243 of file ekf2_main.cpp.


The documentation for this class was generated from the following file: