#include <ekf.h>

Inheritance diagram for Ekf:

Collaboration diagram for Ekf:

Public Member Functions
	Ekf ()=default

virtual	~Ekf ()=default

bool	init (uint64_t timestamp) override

void	reset (uint64_t timestamp) override

void	resetStatesAndCovariances () override

void	resetStates () override

bool	update () override

void	get_vel_pos_innov (float vel_pos_innov[6]) override

void	get_aux_vel_innov (float aux_vel_innov[2]) override

void	get_mag_innov (float mag_innov[3]) override

void	get_heading_innov (float *heading_innov) override

void	get_vel_pos_innov_var (float vel_pos_innov_var[6]) override

void	get_mag_innov_var (float mag_innov_var[3]) override

void	get_airspeed_innov (float *airspeed_innov) override

void	get_airspeed_innov_var (float *airspeed_innov_var) override

void	get_beta_innov (float *beta_innov) override

void	get_beta_innov_var (float *beta_innov_var) override

void	get_heading_innov_var (float *heading_innov_var) override

void	get_flow_innov_var (float flow_innov_var[2]) override

void	get_flow_innov (float flow_innov[2]) override

void	get_drag_innov_var (float drag_innov_var[2]) override

void	get_drag_innov (float drag_innov[2]) override

void	getHaglInnovVar (float *hagl_innov_var) override

void	getHaglInnov (float *hagl_innov) override

void	get_state_delayed (float *state) override

void	get_wind_velocity (float *wind) override

void	get_wind_velocity_var (float *wind_var) override

void	get_true_airspeed (float *tas) override

matrix::SquareMatrix< float, 24 >	covariances () const

matrix::Vector< float, 24 >	covariances_diagonal () const

matrix::SquareMatrix< float, 4 >	orientation_covariances () const

matrix::SquareMatrix< float, 3 >	velocity_covariances () const

matrix::SquareMatrix< float, 3 >	position_covariances () const

bool	collect_gps (const gps_message &gps) override

bool	collect_imu (const imuSample &imu) override

bool	get_ekf_origin (uint64_t origin_time, map_projection_reference_s origin_pos, float *origin_alt) override

void	get_ekf_gpos_accuracy (float ekf_eph, float ekf_epv) override

void	get_ekf_lpos_accuracy (float ekf_eph, float ekf_epv) override

void	get_ekf_vel_accuracy (float ekf_evh, float ekf_evv) override

void	get_ekf_ctrl_limits (float vxy_max, float vz_max, float hagl_min, float hagl_max) override

bool	reset_imu_bias () override

void	get_vel_var (Vector3f &vel_var) override

void	get_pos_var (Vector3f &pos_var) override

void	get_output_tracking_error (float error[3]) override

void	get_imu_vibe_metrics (float vibe[3]) override

bool	get_gps_drift_metrics (float drift[3], bool *blocked) override

bool	global_position_is_valid () override

void	update_deadreckoning_status ()

bool	isTerrainEstimateValid () const override

void	updateTerrainValidity ()

void	getTerrainVertPos (float *ret) override

float	get_terrain_var () const

void	get_accel_bias (float bias[3]) override

void	get_gyro_bias (float bias[3]) override

void	get_gps_check_status (uint16_t *val) override

void	get_posD_reset (float delta, uint8_t counter) override

void	get_velD_reset (float delta, uint8_t counter) override

void	get_posNE_reset (float delta[2], uint8_t *counter) override

void	get_velNE_reset (float delta[2], uint8_t *counter) override

void	get_quat_reset (float delta_quat[4], uint8_t *counter) override

void	get_innovation_test_status (uint16_t status, float mag, float vel, float pos, float hgt, float tas, float hagl, float beta) override

void	get_ekf_soln_status (uint16_t *status) override

void	get_ev2ekf_quaternion (float *quat) override

Quatf	calculate_quaternion () const

void	set_min_required_gps_health_time (uint32_t time_us)

Public Member Functions inherited from EstimatorInterface
	EstimatorInterface ()=default

virtual	~EstimatorInterface ()=default

void	get_hagl_innov_var (float *hagl_innov_var)

void	get_hagl_innov (float *hagl_innov)

void	setIMUData (const imuSample &imu_sample)

void	setIMUData (uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float(&delta_ang)[3], float(&delta_vel)[3])

void	setMagData (uint64_t time_usec, float(&data)[3])

void	setGpsData (uint64_t time_usec, const gps_message &gps)

void	setBaroData (uint64_t time_usec, float data)

void	setAirspeedData (uint64_t time_usec, float true_airspeed, float eas2tas)

void	setRangeData (uint64_t time_usec, float data, int8_t quality)

void	setOpticalFlowData (uint64_t time_usec, flow_message *flow)

void	setExtVisionData (uint64_t time_usec, ext_vision_message *evdata)

void	setAuxVelData (uint64_t time_usec, float(&data)[2], float(&variance)[2])

parameters *	getParamHandle ()

void	set_in_air_status (bool in_air)

bool	attitude_valid ()

bool	get_in_air_status ()

bool	get_wind_status ()

void	set_is_fixed_wing (bool is_fixed_wing)

void	set_fuse_beta_flag (bool fuse_beta)

void	set_gnd_effect_flag (bool gnd_effect)

void	set_air_density (float air_density)

void	set_rangefinder_limits (float min_distance, float max_distance)

void	set_optical_flow_limits (float max_flow_rate, float min_distance, float max_distance)

bool	inertial_dead_reckoning ()

bool	get_terrain_valid ()

void	get_terrain_vert_pos (float *ret)

bool	local_position_is_valid ()

const matrix::Quatf &	get_quaternion () const

void	get_velocity (float *vel)

void	get_vel_deriv_ned (float *vel_deriv)

void	get_pos_d_deriv (float *pos_d_deriv)

void	get_position (float *pos)

void	copy_timestamp (uint64_t *time_us)

bool	get_mag_decl_deg (float *val)

void	get_control_mode (uint32_t *val)

void	get_filter_fault_status (uint16_t *val)

bool	isVehicleAtRest () const

float	get_dt_imu_avg () const

imuSample	get_imu_sample_delayed ()

baroSample	get_baro_sample_delayed ()

void	print_status ()

Private Member Functions
void	calculateOutputStates ()

bool	initialiseFilter (void)

void	initialiseCovariance ()

void	predictState ()

void	predictCovariance ()

void	fuseMag ()

void	fuseHeading ()

void	fuseGpsAntYaw ()

bool	resetGpsAntYaw ()

void	fuseDeclination (float decl_sigma)

void	limitDeclination ()

void	fuseAirspeed ()

void	fuseSideslip ()

void	fuseDrag ()

void	fuseVelPosHeight ()

bool	resetVelocity ()

void	fuseOptFlow ()

bool	calcOptFlowBodyRateComp ()

bool	initHagl ()

void	runTerrainEstimator ()

void	fuseHagl ()

void	fuseFlowForTerrain ()

bool	resetMagHeading (const Vector3f &mag_init, bool increase_yaw_var=true, bool update_buffer=true)

bool	realignYawGPS ()

float	getMagDeclination ()

bool	resetPosition ()

void	resetHeight ()

void	alignOutputFilter ()

void	calcExtVisRotMat ()

void	resetExtVisRotMat ()

void	fixCovarianceErrors ()

void	makeSymmetrical (float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)

void	constrainStates ()

void	fuse (float *K, float innovation)

void	calcEarthRateNED (Vector3f &omega, float lat_rad) const

bool	gps_is_good (const gps_message &gps)

void	controlFusionModes ()

void	controlExternalVisionFusion ()

void	controlOpticalFlowFusion ()

void	controlGpsFusion ()

void	controlMagFusion ()

void	updateMagFilter ()

bool	canRunMagFusion () const

void	checkHaglYawResetReq ()

float	getTerrainVPos () const

void	runOnGroundYawReset ()

bool	isYawResetAuthorized () const

bool	canResetMagHeading () const

void	runInAirYawReset ()

bool	canRealignYawUsingGps () const

void	runVelPosReset ()

void	selectMagAuto ()

void	check3DMagFusionSuitability ()

void	checkYawAngleObservability ()

void	checkMagBiasObservability ()

bool	isYawAngleObservable () const

bool	isMagBiasObservable () const

bool	canUse3DMagFusion () const

void	checkMagDeclRequired ()

void	checkMagInhibition ()

bool	shouldInhibitMag () const

void	checkMagFieldStrength ()

bool	isStrongMagneticDisturbance () const

bool	isMeasuredMatchingGpsMagStrength () const

bool	isMeasuredMatchingAverageMagStrength () const

void	runMagAndMagDeclFusions ()

void	run3DMagAndDeclFusions ()

void	controlRangeFinderFusion ()

void	controlAirDataFusion ()

void	controlBetaFusion ()

void	controlDragFusion ()

void	controlBaroFusion ()

void	controlVelPosFusion ()

void	controlAuxVelFusion ()

void	controlHeightSensorTimeouts ()

void	controlHeightFusion ()

void	checkRangeAidSuitability ()

bool	isRangeAidSuitable ()

void	updateRangeDataValidity ()

void	updateRangeDataStuck ()

void	setControlBaroHeight ()

void	setControlRangeHeight ()

void	setControlGPSHeight ()

void	setControlEVHeight ()

void	stopMagFusion ()

void	stopMag3DFusion ()

void	stopMagHdgFusion ()

void	startMagHdgFusion ()

void	startMag3DFusion ()

void	zeroRows (float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)

void	zeroCols (float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)

void	zeroOffDiag (float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)

void	setDiag (float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last, float variance)

float	calcOptFlowMeasVar ()

Vector3f	calcRotVecVariances ()

void	initialiseQuatCovariances (Vector3f &rot_vec_var)

void	resetMagRelatedCovariances ()

void	resetWindCovariance ()

void	resetWindStates ()

void	updateRangeDataContinuity ()

bool	isRangeDataContinuous ()

void	increaseQuatYawErrVariance (float yaw_variance)

void	loadMagCovData ()

void	saveMagCovData ()

void	clearMagCov ()

void	zeroMagCov ()

void	uncorrelateQuatStates ()

float	kahanSummation (float sum_previous, float input, float &accumulator) const

float	calculate_synthetic_mag_z_measurement (Vector3f mag_meas, Vector3f mag_earth_predicted)

Static Private Member Functions
static bool	isMeasuredMatchingExpected (float measured, float expected, float gate)

static constexpr float	sq (float var)

Private Attributes
struct {
uint8_t velNE_counter
	number of horizontal position reset events (allow to wrap if count exceeds 255) More...

uint8_t velD_counter
	number of vertical velocity reset events (allow to wrap if count exceeds 255) More...

uint8_t posNE_counter
	number of horizontal position reset events (allow to wrap if count exceeds 255) More...

uint8_t posD_counter
	number of vertical position reset events (allow to wrap if count exceeds 255) More...

uint8_t quat_counter
	number of quaternion reset events (allow to wrap if count exceeds 255) More...

Vector2f velNE_change
	North East velocity change due to last reset (m) More...

float velD_change
	Down velocity change due to last reset (m/sec) More...

Vector2f posNE_change
	North, East position change due to last reset (m) More...

float posD_change
	Down position change due to last reset (m) More...

Quatf quat_change
	quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion More...

}	_state_reset_status
	reset event monitoring structure containing velocity, position, height and yaw reset information More...

float	_dt_ekf_avg {FILTER_UPDATE_PERIOD_S}
	average update rate of the ekf More...

float	_dt_update {0.01f}
	delta time since last ekf update. This time can be used for filters which run at the same rate as the Ekf::update() function. (sec) More...

stateSample	_state {}
	state struct of the ekf running at the delayed time horizon More...

bool	_filter_initialised {false}
	true when the EKF sttes and covariances been initialised More...

bool	_earth_rate_initialised {false}
	true when we know the earth rotatin rate (requires GPS) More...

bool	_fuse_height {false}
	true when baro height data should be fused More...

bool	_fuse_pos {false}
	true when gps position data should be fused More...

bool	_fuse_hor_vel {false}
	true when gps horizontal velocity measurement should be fused More...

bool	_fuse_vert_vel {false}
	true when gps vertical velocity measurement should be fused More...

bool	_fuse_hor_vel_aux {false}
	true when auxiliary horizontal velocity measurement should be fused More...

float	_posObsNoiseNE {0.0f}
	1-STD observation noise used for the fusion of NE position data (m) More...

float	_posInnovGateNE {1.0f}
	Number of standard deviations used for the NE position fusion innovation consistency check. More...

Vector3f	_velObsVarNED
	1-STD observation noise variance used for the fusion of NED velocity data (m/sec)**2 More...

float	_hvelInnovGate {1.0f}
	Number of standard deviations used for the horizontal velocity fusion innovation consistency check. More...

float	_vvelInnovGate {1.0f}
	Number of standard deviations used for the vertical velocity fusion innovation consistency check. More...

bool	_fuse_hpos_as_odom {false}
	true when the NE position data is being fused using an odometry assumption More...

Vector3f	_pos_meas_prev
	previous value of NED position measurement fused using odometry assumption (m) More...

Vector2f	_hpos_pred_prev
	previous value of NE position state used by odometry fusion (m) More...

bool	_hpos_prev_available {false}
	true when previous values of the estimate and measurement are available for use More...

Vector3f	_ev_rot_vec_filt
	filtered rotation vector defining the rotation EV to EKF reference, initiliazied to zero rotation (rad) More...

Dcmf	_ev_rot_mat
	transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity More...

uint64_t	_ev_rot_last_time_us {0}
	previous time that the calculation of the EV to EKF rotation matrix was updated (uSec) More...

bool	_ev_rot_mat_initialised {0}
	_ev_rot_mat should only be initialised once in the beginning through the reset function More...

bool	_gps_data_ready {false}
	true when new GPS data has fallen behind the fusion time horizon and is available to be fused More...

bool	_mag_data_ready {false}
	true when new magnetometer data has fallen behind the fusion time horizon and is available to be fused More...

bool	_baro_data_ready {false}
	true when new baro height data has fallen behind the fusion time horizon and is available to be fused More...

bool	_range_data_ready {false}
	true when new range finder data has fallen behind the fusion time horizon and is available to be fused More...

bool	_flow_data_ready {false}
	true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon More...

bool	_ev_data_ready {false}
	true when new external vision system data has fallen behind the fusion time horizon and is available to be fused More...

bool	_tas_data_ready {false}
	true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused More...

bool	_flow_for_terrain_data_ready {false}

uint64_t	_time_last_fake_gps {0}
	same flag as "_flow_data_ready" but used for separate terrain estimator More...

uint64_t	_time_ins_deadreckon_start {0}
	amount of time we have been doing inertial only deadreckoning (uSec) More...

bool	_using_synthetic_position {false}
	true if we are using a synthetic position to constrain drift More...

uint64_t	_time_last_pos_fuse {0}
	time the last fusion of horizontal position measurements was performed (uSec) More...

uint64_t	_time_last_delpos_fuse {0}
	time the last fusion of incremental horizontal position measurements was performed (uSec) More...

uint64_t	_time_last_vel_fuse {0}
	time the last fusion of velocity measurements was performed (uSec) More...

uint64_t	_time_last_hgt_fuse {0}
	time the last fusion of height measurements was performed (uSec) More...

uint64_t	_time_last_of_fuse {0}
	time the last fusion of optical flow measurements were performed (uSec) More...

uint64_t	_time_last_arsp_fuse {0}
	time the last fusion of airspeed measurements were performed (uSec) More...

uint64_t	_time_last_beta_fuse {0}
	time the last fusion of synthetic sideslip measurements were performed (uSec) More...

uint64_t	_time_last_rng_ready {0}
	time the last range finder measurement was ready (uSec) More...

Vector2f	_last_known_posNE
	last known local NE position vector (m) More...

float	_imu_collection_time_adj {0.0f}
	the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec) More...

uint64_t	_time_acc_bias_check {0}
	last time the accel bias check passed (uSec) More...

uint64_t	_delta_time_baro_us {0}
	delta time between two consecutive delayed baro samples from the buffer (uSec) More...

uint64_t	_last_imu_bias_cov_reset_us {0}
	time the last reset of IMU delta angle and velocity state covariances was performed (uSec) More...

Vector3f	_earth_rate_NED
	earth rotation vector (NED) in rad/s More...

Dcmf	_R_to_earth
	transformation matrix from body frame to earth frame from last EKF prediction More...

Vector2f	_accel_lpf_NE
	Low pass filtered horizontal earth frame acceleration (m/sec**2) More...

float	_yaw_delta_ef {0.0f}
	Recent change in yaw angle measured about the earth frame D axis (rad) More...

float	_yaw_rate_lpf_ef {0.0f}
	Filtered angular rate about earth frame D axis (rad/sec) More...

bool	_mag_bias_observable {false}
	true when there is enough rotation to make magnetometer bias errors observable More...

bool	_yaw_angle_observable {false}
	true when there is enough horizontal acceleration to make yaw observable More...

uint64_t	_time_yaw_started {0}
	last system time in usec that a yaw rotation manoeuvre was detected More...

uint8_t	_num_bad_flight_yaw_events {0}
	number of times a bad heading has been detected in flight and required a yaw reset More...

uint64_t	_mag_use_not_inhibit_us {0}
	last system time in usec before magnetometer use was inhibited More...

bool	_mag_use_inhibit {false}
	true when magnetometer use is being inhibited More...

bool	_mag_use_inhibit_prev {false}
	true when magnetometer use was being inhibited the previous frame More...

bool	_mag_inhibit_yaw_reset_req {false}
	true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve. More...

float	_last_static_yaw {0.0f}
	last yaw angle recorded when on ground motion checks were passing (rad) More...

bool	_mag_yaw_reset_req {false}
	true when a reset of the yaw using the magnetometer data has been requested More...

bool	_mag_decl_cov_reset {false}
	true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event. More...

bool	_synthetic_mag_z_active {false}
	true if we are generating synthetic magnetometer Z measurements More...

float	P [_k_num_states][_k_num_states] {}
	state covariance matrix More...

Vector3f	_delta_vel_bias_var_accum
	kahan summation algorithm accumulator for delta velocity bias variance More...

Vector3f	_delta_angle_bias_var_accum
	kahan summation algorithm accumulator for delta angle bias variance More...

float	_vel_pos_innov [6] {}
	NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m) More...

float	_vel_pos_innov_var [6] {}
	NED velocity and position innovation variances: 0-2 vel ((m/sec)2), 3-5 pos (m2) More...

float	_aux_vel_innov [2] {}
	NE auxiliary velocity innovations: (m/sec) More...

float	_mag_innov [3] {}
	earth magnetic field innovations (Gauss) More...

float	_mag_innov_var [3] {}
	earth magnetic field innovation variance (Gauss**2) More...

float	_airspeed_innov {0.0f}
	airspeed measurement innovation (m/sec) More...

float	_airspeed_innov_var {0.0f}
	airspeed measurement innovation variance ((m/sec)**2) More...

float	_beta_innov {0.0f}
	synthetic sideslip measurement innovation (rad) More...

float	_beta_innov_var {0.0f}
	synthetic sideslip measurement innovation variance (rad**2) More...

float	_drag_innov [2] {}
	multirotor drag measurement innovation (m/sec**2) More...

float	_drag_innov_var [2] {}
	multirotor drag measurement innovation variance ((m/sec2)2) More...

float	_heading_innov {0.0f}
	heading measurement innovation (rad) More...

float	_heading_innov_var {0.0f}
	heading measurement innovation variance (rad**2) More...

float	_flow_innov [2] {}
	flow measurement innovation (rad/sec) More...

float	_flow_innov_var [2] {}
	flow innovation variance ((rad/sec)**2) More...

Vector3f	_flow_gyro_bias
	bias errors in optical flow sensor rate gyro outputs (rad/sec) More...

Vector3f	_imu_del_ang_of
	bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad) More...

float	_delta_time_of {0.0f}
	time in sec that _imu_del_ang_of was accumulated over (sec) More...

uint64_t	_time_bad_motion_us {0}
	last system time that on-ground motion exceeded limits (uSec) More...

uint64_t	_time_good_motion_us {0}
	last system time that on-ground motion was within limits (uSec) More...

bool	_inhibit_flow_use {false}
	true when use of optical flow and range finder is being inhibited More...

Vector2f	_flowRadXYcomp
	measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive More...

Vector3f	_delta_angle_corr
	delta angle correction vector (rad) More...

imuSample	_imu_down_sampled {}
	down sampled imu data (sensor rate -> filter update rate) More...

Quatf	_q_down_sampled
	down sampled quaternion (tracking delta angles between ekf update steps) More...

Vector3f	_vel_err_integ
	integral of velocity tracking error (m) More...

Vector3f	_pos_err_integ
	integral of position tracking error (m.s) More...

float	_output_tracking_error [3] {}
	contains the magnitude of the angle, velocity and position track errors (rad, m/s, m) More...

float	_gpsDriftVelN {0.0f}
	GPS north position derivative (m/sec) More...

float	_gpsDriftVelE {0.0f}
	GPS east position derivative (m/sec) More...

float	_gps_drift_velD {0.0f}
	GPS down position derivative (m/sec) More...

float	_gps_velD_diff_filt {0.0f}
	GPS filtered Down velocity (m/sec) More...

float	_gps_velN_filt {0.0f}
	GPS filtered North velocity (m/sec) More...

float	_gps_velE_filt {0.0f}
	GPS filtered East velocity (m/sec) More...

uint64_t	_last_gps_fail_us {0}
	last system time in usec that the GPS failed it's checks More...

uint64_t	_last_gps_pass_us {0}
	last system time in usec that the GPS passed it's checks More...

float	_gps_error_norm {1.0f}
	normalised gps error More...

uint32_t	_min_gps_health_time_us {10000000}
	GPS is marked as healthy only after this amount of time. More...

bool	_gps_checks_passed {false}

uint64_t	_last_gps_origin_time_us {0}
	true when all active GPS checks have passed More...

float	_gps_alt_ref {0.0f}
	WGS-84 height (m) More...

uint32_t	_hgt_counter {0}
	number of height samples read during initialisation More...

float	_rng_filt_state {0.0f}
	filtered height measurement (m) More...

uint32_t	_mag_counter {0}
	number of magnetometer samples read during initialisation More...

uint32_t	_ev_counter {0}
	number of external vision samples read during initialisation More...

uint64_t	_time_last_mag {0}
	measurement time of last magnetomter sample (uSec) More...

AlphaFilterVector3f	_mag_lpf
	filtered magnetometer measurement (Gauss) More...

Vector3f	_delVel_sum
	summed delta velocity (m/sec) More...

float	_hgt_sensor_offset {0.0f}
	set as necessary if desired to maintain the same height after a height reset (m) More...

float	_baro_hgt_offset {0.0f}
	baro height reading at the local NED origin (m) More...

float	_last_on_ground_posD {0.0f}
	last vertical position when the in_air status was false (m) More...

bool	_flt_mag_align_converging {false}
	true when the in-flight mag field post alignment convergence is being performd More...

uint64_t	_flt_mag_align_start_time {0}
	time that inflight magnetic field alignment started (uSec) More...

uint64_t	_time_last_mov_3d_mag_suitable {0}
	last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec) More...

float	_saved_mag_bf_variance [4] {}
	magnetic field state variances that have been saved for use at the next initialisation (Gauss**2) More...

float	_saved_mag_ef_covmat [2][2] {}
	NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2) More...

bool	_velpos_reset_request {false}
	true when a large yaw error has been fixed and a velocity and position state reset is required More...

gps_check_fail_status_u	_gps_check_fail_status {}

bool	_accel_bias_inhibit {false}
	true when the accel bias learning is being inhibited More...

Vector3f	_accel_vec_filt {}
	acceleration vector after application of a low pass filter (m/sec**2) More...

float	_accel_mag_filt {0.0f}
	acceleration magnitude after application of a decaying envelope filter (rad/sec) More...

float	_ang_rate_mag_filt {0.0f}
	angular rate magnitude after application of a decaying envelope filter (rad/sec) More...

Vector3f	_prev_dvel_bias_var
	saved delta velocity XYZ bias variances (m/sec)**2 More...

float	_terrain_vpos {0.0f}
	estimated vertical position of the terrain underneath the vehicle in local NED frame (m) More...

float	_terrain_var {1e4f}
	variance of terrain position estimate (m**2) More...

float	_hagl_innov {0.0f}
	innovation of the last height above terrain measurement (m) More...

float	_hagl_innov_var {0.0f}
	innovation variance for the last height above terrain measurement (m**2) More...

uint64_t	_time_last_hagl_fuse {0}
	last system time that the hagl measurement failed it's checks (uSec) More...

bool	_terrain_initialised {false}
	true when the terrain estimator has been initialized More...

float	_sin_tilt_rng {0.0f}
	sine of the range finder tilt rotation about the Y body axis More...

float	_cos_tilt_rng {0.0f}
	cosine of the range finder tilt rotation about the Y body axis More...

float	_R_rng_to_earth_2_2 {0.0f}
	2,2 element of the rotation matrix from sensor frame to earth frame More...

float	_dt_last_range_update_filt_us {0.0f}
	filtered value of the delta time elapsed since the last range measurement came into the filter (uSec) More...

bool	_hagl_valid {false}
	true when the height above ground estimate is valid More...

bool	_baro_hgt_faulty {false}
	true if valid baro data is unavailable for use More...

bool	_gps_hgt_intermittent {false}
	true if gps height into the buffer is intermittent More...

bool	_rng_hgt_valid {false}
	true if range finder sample retrieved from buffer is valid More...

int	_primary_hgt_source {VDIST_SENSOR_BARO}
	specifies primary source of height data More...

uint64_t	_time_bad_rng_signal_quality {0}
	timestamp at which range finder signal quality was 0 (used for hysteresis) More...

uint64_t	_time_bad_vert_accel {0}
	last time a bad vertical accel was detected (uSec) More...

uint64_t	_time_good_vert_accel {0}
	last time a good vertical accel was detected (uSec) More...

bool	_bad_vert_accel_detected {false}
	true when bad vertical accelerometer data has been detected More...

bool	_is_range_aid_suitable {false}
	true when range finder can be used in flight as the height reference instead of the primary height sensor More...

bool	_range_aid_mode_selected {false}
	true when range finder is being used as the height reference instead of the primary height sensor More...

float	_rng_stuck_min_val {0.0f}
	minimum value for new rng measurement when being stuck More...

float	_rng_stuck_max_val {0.0f}
	maximum value for new rng measurement when being stuck More...

float	_height_rate_lpf {0.0f}

Static Private Attributes
static constexpr uint8_t	_k_num_states {24}
	number of EKF states More...

Additional Inherited Members
Public Types inherited from EstimatorInterface
typedef AlphaFilter< Vector3f >	AlphaFilterVector3f

Static Public Attributes inherited from EstimatorInterface
static constexpr unsigned	FILTER_UPDATE_PERIOD_MS {8}

static constexpr float	FILTER_UPDATE_PERIOD_S {FILTER_UPDATE_PERIOD_MS * 0.001f}

Protected Member Functions inherited from EstimatorInterface
bool	initialise_interface (uint64_t timestamp)

void	unallocate_buffers ()

Vector3f	cross_product (const Vector3f &vecIn1, const Vector3f &vecIn2)

Matrix3f	quat_to_invrotmat (const Quatf &quat)

Protected Attributes inherited from EstimatorInterface
parameters	_params

uint8_t	_obs_buffer_length {0}

uint8_t	_imu_buffer_length {0}

unsigned	_min_obs_interval_us {0}

float	_dt_imu_avg {0.0f}

imuSample	_imu_sample_delayed {}

magSample	_mag_sample_delayed {}

baroSample	_baro_sample_delayed {}

gpsSample	_gps_sample_delayed {}

rangeSample	_range_sample_delayed {}

airspeedSample	_airspeed_sample_delayed {}

flowSample	_flow_sample_delayed {}

extVisionSample	_ev_sample_delayed {}

dragSample	_drag_sample_delayed {}

dragSample	_drag_down_sampled {}

auxVelSample	_auxvel_sample_delayed {}

uint8_t	_drag_sample_count {0}

float	_drag_sample_time_dt {0.0f}

float	_air_density {CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}

float	_rng_valid_min_val {0.0f}
	minimum distance that the rangefinder can measure (m) More...

float	_rng_valid_max_val {0.0f}
	maximum distance that the rangefinder can measure (m) More...

float	_flow_max_rate {0.0f}
	maximum angular flow rate that the optical flow sensor can measure (rad/s) More...

float	_flow_min_distance {0.0f}
	minimum distance that the optical flow sensor can operate at (m) More...

float	_flow_max_distance {0.0f}
	maximum distance that the optical flow sensor can operate at (m) More...

outputSample	_output_sample_delayed {}

outputSample	_output_new {}

outputVert	_output_vert_delayed {}

outputVert	_output_vert_new {}

imuSample	_imu_sample_new {}

Matrix3f	_R_to_earth_now

Vector3f	_vel_imu_rel_body_ned

Vector3f	_vel_deriv_ned

bool	_imu_updated {false}

bool	_initialised {false}

bool	_NED_origin_initialised {false}

bool	_gps_speed_valid {false}

float	_gps_origin_eph {0.0f}

float	_gps_origin_epv {0.0f}

float	_gps_alt_prev {0.0f}

float	_gps_yaw_offset {0.0f}

float	_yaw_test_ratio {0.0f}

float	_mag_test_ratio [3] {}

float	_vel_pos_test_ratio [6] {}

float	_tas_test_ratio {0.0f}

float	_terr_test_ratio {0.0f}

float	_beta_test_ratio {0.0f}

float	_drag_test_ratio [2] {}

innovation_fault_status_u	_innov_check_fail_status {}

bool	_is_dead_reckoning {false}

bool	_deadreckon_time_exceeded {true}

bool	_is_wind_dead_reckoning {false}

Vector3f	_delta_ang_prev

Vector3f	_delta_vel_prev

float	_vibe_metrics [3] {}

float	_gps_drift_metrics [3] {}

bool	_vehicle_at_rest {false}

uint64_t	_time_last_move_detect_us {0}

bool	_gps_drift_updated {false}

RingBuffer< imuSample >	_imu_buffer

RingBuffer< gpsSample >	_gps_buffer

RingBuffer< magSample >	_mag_buffer

RingBuffer< baroSample >	_baro_buffer

RingBuffer< rangeSample >	_range_buffer

RingBuffer< airspeedSample >	_airspeed_buffer

RingBuffer< flowSample >	_flow_buffer

RingBuffer< extVisionSample >	_ext_vision_buffer

RingBuffer< outputSample >	_output_buffer

RingBuffer< outputVert >	_output_vert_buffer

RingBuffer< dragSample >	_drag_buffer

RingBuffer< auxVelSample >	_auxvel_buffer

bool	_gps_buffer_fail {false}

bool	_mag_buffer_fail {false}

bool	_baro_buffer_fail {false}

bool	_range_buffer_fail {false}

bool	_airspeed_buffer_fail {false}

bool	_flow_buffer_fail {false}

bool	_ev_buffer_fail {false}

bool	_drag_buffer_fail {false}

bool	_auxvel_buffer_fail {false}

uint64_t	_time_last_imu {0}

uint64_t	_time_last_gps {0}

uint64_t	_time_last_mag {0}

uint64_t	_time_last_baro {0}

uint64_t	_time_last_range {0}

uint64_t	_time_last_airspeed {0}

uint64_t	_time_last_ext_vision {0}

uint64_t	_time_last_optflow {0}

uint64_t	_time_last_gnd_effect_on {0}

uint64_t	_time_last_auxvel {0}

fault_status_u	_fault_status {}

float	_mag_declination_gps {0.0f}

float	_mag_inclination_gps {0.0f}

float	_mag_strength_gps {0.0f}

filter_control_status_u	_control_status {}

filter_control_status_u	_control_status_prev {}

Detailed Description

Definition at line 47 of file ekf.h.

Constructor & Destructor Documentation

◆ Ekf()

Ekf::Ekf ( )

default

◆ ~Ekf()

virtual Ekf::~Ekf ( )

virtualdefault

Member Function Documentation

◆ alignOutputFilter()

void Ekf::alignOutputFilter ( )

private

Definition at line 378 of file ekf_helper.cpp.

References EstimatorInterface::_output_buffer, EstimatorInterface::_output_new, EstimatorInterface::_output_sample_delayed, _state, RingBuffer< data_type >::get_length(), estimator::outputSample::pos, estimator::stateSample::pos, estimator::outputSample::quat_nominal, estimator::stateSample::quat_nominal, estimator::outputSample::vel, and estimator::stateSample::vel.

Referenced by controlOpticalFlowFusion(), and initialiseFilter().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calcEarthRateNED()

void Ekf::calcEarthRateNED	(	Vector3f &	omega,
		float	lat_rad
	)		const

private

Definition at line 820 of file ekf_helper.cpp.

References CONSTANTS_EARTH_SPIN_RATE.

Referenced by predictState().

Here is the caller graph for this function:

◆ calcExtVisRotMat()

void Ekf::calcExtVisRotMat ( )

private

Definition at line 1620 of file ekf_helper.cpp.

References _ev_rot_last_time_us, _ev_rot_mat, _ev_rot_vec_filt, EstimatorInterface::_ev_sample_delayed, _state, EstimatorInterface::_time_last_imu, math::constrain(), f(), estimator::extVisionSample::quat, and estimator::stateSample::quat_nominal.

Referenced by controlExternalVisionFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calcOptFlowBodyRateComp()

bool Ekf::calcOptFlowBodyRateComp ( )

private

Definition at line 542 of file optflow_fusion.cpp.

References _delta_time_of, _flow_gyro_bias, EstimatorInterface::_flow_sample_delayed, _imu_del_ang_of, math::constrain(), estimator::flowSample::dt, f(), FLT_EPSILON, estimator::flowSample::gyroXYZ, and ISFINITE.

Referenced by controlOpticalFlowFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calcOptFlowMeasVar()

float Ekf::calcOptFlowMeasVar ( )

private

Definition at line 589 of file optflow_fusion.cpp.

References EstimatorInterface::_flow_sample_delayed, EstimatorInterface::_params, math::constrain(), f(), estimator::parameters::flow_noise, estimator::parameters::flow_noise_qual_min, estimator::parameters::flow_qual_min, estimator::flowSample::quality, and sq().

Referenced by fuseFlowForTerrain(), fuseOptFlow(), resetVelocity(), and sq().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calcRotVecVariances()

Vector3f Ekf::calcRotVecVariances ( )

private

Definition at line 1386 of file ekf_helper.cpp.

References _state, f(), P, estimator::stateSample::quat_nominal, t10, t11, t12, t13, t14, t15, t16, t17, t2, t3, t4, t5, t6, t7, t8, and t9.

Referenced by controlFusionModes(), and sq().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calculate_quaternion()

Quatf Ekf::calculate_quaternion ( ) const

Definition at line 680 of file ekf.cpp.

References _delta_angle_corr, _dt_ekf_avg, EstimatorInterface::_dt_imu_avg, EstimatorInterface::_imu_sample_new, EstimatorInterface::_output_new, _state, estimator::imuSample::delta_ang, estimator::stateSample::gyro_bias, and estimator::outputSample::quat_nominal.

Referenced by get_quat_reset(), and Ekf2::publish_attitude().

Here is the caller graph for this function:

◆ calculate_synthetic_mag_z_measurement()

float Ekf::calculate_synthetic_mag_z_measurement	(	Vector3f	mag_meas,
		Vector3f	mag_earth_predicted
	)

private

Definition at line 1006 of file mag_fusion.cpp.

References _R_to_earth, f(), math::max(), and sq().

Referenced by controlFusionModes(), and isRangeDataContinuous().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calculateOutputStates()

void Ekf::calculateOutputStates ( )

private

Definition at line 458 of file ekf.cpp.

Referenced by update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ canRealignYawUsingGps()

bool Ekf::canRealignYawUsingGps ( ) const

private

Definition at line 167 of file mag_control.cpp.

References EstimatorInterface::_control_status, estimator::filter_control_status_u::fixed_wing, and estimator::filter_control_status_u::flags.

Referenced by runInAirYawReset().

Here is the caller graph for this function:

◆ canResetMagHeading()

bool Ekf::canResetMagHeading ( ) const

private

Definition at line 149 of file mag_control.cpp.

References isStrongMagneticDisturbance().

Referenced by controlGpsFusion(), runInAirYawReset(), and runOnGroundYawReset().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ canRunMagFusion()

bool Ekf::canRunMagFusion ( ) const

private

Definition at line 108 of file mag_control.cpp.

References EstimatorInterface::_control_status, _mag_data_ready, estimator::filter_control_status_u::ev_yaw, estimator::filter_control_status_u::flags, and estimator::filter_control_status_u::gps_yaw.

Referenced by controlMagFusion().

Here is the caller graph for this function:

void Ekf::checkMagDeclRequired ( )

private

Definition at line 245 of file mag_control.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_params, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::gps, estimator::filter_control_status_u::mag_3D, estimator::filter_control_status_u::mag_dec, estimator::parameters::mag_declination_source, and MASK_FUSE_DECL.

Referenced by controlMagFusion().

Here is the caller graph for this function:

◆ checkMagFieldStrength()

void Ekf::checkMagFieldStrength ( )

private

Definition at line 285 of file mag_control.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_NED_origin_initialised, EstimatorInterface::_params, estimator::parameters::check_mag_strength, estimator::filter_control_status_u::flags, isMeasuredMatchingAverageMagStrength(), isMeasuredMatchingGpsMagStrength(), and estimator::filter_control_status_u::mag_field_disturbed.

Referenced by controlMagFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ checkMagInhibition()

void Ekf::checkMagInhibition ( )

private

Definition at line 256 of file mag_control.cpp.

References EstimatorInterface::_imu_sample_delayed, _mag_inhibit_yaw_reset_req, _mag_use_inhibit, _mag_use_not_inhibit_us, shouldInhibitMag(), and estimator::imuSample::time_us.

Referenced by controlMagFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ checkRangeAidSuitability()

void Ekf::checkRangeAidSuitability ( )

private

Definition at line 1166 of file control.cpp.

References EstimatorInterface::_control_status, _hagl_innov, _hagl_innov_var, _is_range_aid_suitable, EstimatorInterface::_params, _rng_hgt_valid, _state, _terrain_vpos, estimator::filter_control_status_u::ev_pos, estimator::filter_control_status_u::ev_vel, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::gps, estimator::filter_control_status_u::in_air, isTerrainEstimateValid(), estimator::parameters::max_hagl_for_range_aid, estimator::parameters::max_vel_for_range_aid, estimator::filter_control_status_u::opt_flow, estimator::stateSample::pos, estimator::parameters::range_aid_innov_gate, sq(), and estimator::stateSample::vel.

Referenced by controlHeightFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ checkYawAngleObservability()

void Ekf::checkYawAngleObservability ( )

private

Definition at line 197 of file mag_control.cpp.

References _accel_lpf_NE, EstimatorInterface::_control_status, EstimatorInterface::_params, _yaw_angle_observable, estimator::filter_control_status_u::ev_pos, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::gps, and estimator::parameters::mag_acc_gate.

Referenced by check3DMagFusionSuitability().

Here is the caller graph for this function:

◆ clearMagCov()

void Ekf::clearMagCov ( )

private

Definition at line 917 of file covariance.cpp.

References _mag_decl_cov_reset, and zeroMagCov().

Referenced by isRangeDataContinuous(), realignYawGPS(), resetMagHeading(), resetMagRelatedCovariances(), and stopMagFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ collect_gps()

bool Ekf::collect_gps ( const gps_message & gps )

overridevirtual

Implements EstimatorInterface.

Definition at line 59 of file gps_checks.cpp.

Referenced by position_covariances().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ collect_imu()

bool Ekf::collect_imu ( const imuSample & imu )

overridevirtual

Implements EstimatorInterface.

Definition at line 378 of file ekf.cpp.

References EstimatorInterface::_imu_buffer, _imu_collection_time_adj, _imu_down_sampled, EstimatorInterface::_imu_sample_delayed, EstimatorInterface::_imu_sample_new, EstimatorInterface::_imu_updated, EstimatorInterface::_min_obs_interval_us, EstimatorInterface::_obs_buffer_length, _q_down_sampled, math::constrain(), estimator::imuSample::delta_ang, estimator::imuSample::delta_ang_dt, estimator::imuSample::delta_vel, estimator::imuSample::delta_vel_dt, f(), EstimatorInterface::FILTER_UPDATE_PERIOD_S, RingBuffer< data_type >::get_oldest(), RingBuffer< data_type >::push(), and estimator::imuSample::time_us.

Referenced by position_covariances().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ constrainStates()

void Ekf::constrainStates ( )

private

Definition at line 784 of file ekf_helper.cpp.

References _dt_ekf_avg, EstimatorInterface::_params, _state, estimator::parameters::acc_bias_lim, estimator::stateSample::accel_bias, math::constrain(), f(), estimator::stateSample::gyro_bias, estimator::stateSample::mag_B, estimator::stateSample::mag_I, estimator::stateSample::pos, estimator::stateSample::quat_nominal, math::radians(), estimator::stateSample::vel, and estimator::stateSample::wind_vel.

Referenced by predictState().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ controlAirDataFusion()

void Ekf::controlAirDataFusion ( )

private

Definition at line 1199 of file control.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_params, _tas_data_ready, _time_last_arsp_fuse, _time_last_beta_fuse, EstimatorInterface::_time_last_imu, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::fuse_aspd, fuseAirspeed(), estimator::parameters::fusion_mode, estimator::filter_control_status_u::in_air, MASK_USE_DRAG, resetWindCovariance(), resetWindStates(), and estimator::filter_control_status_u::wind.

Referenced by controlFusionModes().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ controlAuxVelFusion()

void Ekf::controlAuxVelFusion ( )

private

Definition at line 1364 of file control.cpp.

void Ekf::controlExternalVisionFusion ( )

private

Definition at line 174 of file control.cpp.

Referenced by controlFusionModes().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ controlFusionModes()

void Ekf::controlFusionModes ( )

private

Definition at line 46 of file control.cpp.

Referenced by update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ controlGpsFusion()

void Ekf::controlGpsFusion ( )

private

Definition at line 576 of file control.cpp.

Referenced by controlFusionModes().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ controlHeightFusion()

void Ekf::controlHeightFusion ( )

private

Definition at line 996 of file control.cpp.

Referenced by controlFusionModes().

Here is the call graph for this function:

Here is the caller graph for this function:

void Ekf::controlRangeFinderFusion ( )

private

◆ controlVelPosFusion()

void Ekf::controlVelPosFusion ( )

private

Definition at line 1299 of file control.cpp.

Referenced by controlFusionModes().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ covariances()

matrix::SquareMatrix<float, 24> Ekf::covariances ( ) const

inline

Definition at line 129 of file ekf.h.

References P.

Referenced by covariances_diagonal(), orientation_covariances(), position_covariances(), and velocity_covariances().

Here is the caller graph for this function:

◆ covariances_diagonal()

matrix::Vector<float, 24> Ekf::covariances_diagonal ( ) const

inline

Definition at line 132 of file ekf.h.

References covariances().

Referenced by Ekf2::Run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fixCovarianceErrors()

void Ekf::fixCovarianceErrors ( )

private

Definition at line 728 of file covariance.cpp.

References _accel_bias_inhibit, EstimatorInterface::_control_status, _dt_ekf_avg, EstimatorInterface::_fault_status, EstimatorInterface::_params, _R_to_earth, _state, _time_acc_bias_check, EstimatorInterface::_time_last_imu, _vel_pos_innov, estimator::parameters::acc_bias_lim, estimator::stateSample::accel_bias, estimator::fault_status_u::bad_acc_bias, CONSTANTS_ONE_G, math::constrain(), ECL_WARN_TIMESTAMPED, f(), estimator::fault_status_u::flags, estimator::filter_control_status_u::flags, estimator::parameters::fusion_mode, estimator::filter_control_status_u::mag_3D, makeSymmetrical(), MASK_INHIBIT_ACC_BIAS, P, sq(), estimator::filter_control_status_u::wind, zeroCols(), and zeroRows().

Referenced by fuseAirspeed(), fuseDeclination(), fuseDrag(), fuseGpsAntYaw(), fuseHeading(), fuseMag(), fuseOptFlow(), fuseSideslip(), fuseVelPosHeight(), and predictCovariance().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fuse()

void Ekf::fuse	(	float *	K,
		float	innovation
	)

private

Definition at line 1201 of file ekf_helper.cpp.

References _state, estimator::stateSample::accel_bias, estimator::stateSample::gyro_bias, estimator::stateSample::mag_B, estimator::stateSample::mag_I, estimator::stateSample::pos, estimator::stateSample::quat_nominal, estimator::stateSample::vel, and estimator::stateSample::wind_vel.

Referenced by fuseAirspeed(), fuseDeclination(), fuseDrag(), fuseGpsAntYaw(), fuseHeading(), fuseMag(), fuseOptFlow(), fuseSideslip(), and fuseVelPosHeight().

Here is the caller graph for this function:

◆ fuseAirspeed()

void Ekf::fuseAirspeed ( )

private

Definition at line 47 of file airspeed_fusion.cpp.

References _airspeed_innov, _airspeed_innov_var, EstimatorInterface::_airspeed_sample_delayed, EstimatorInterface::_fault_status, EstimatorInterface::_innov_check_fail_status, EstimatorInterface::_is_wind_dead_reckoning, _k_num_states, EstimatorInterface::_params, _state, EstimatorInterface::_tas_test_ratio, _time_last_arsp_fuse, EstimatorInterface::_time_last_imu, estimator::fault_status_u::bad_airspeed, math::constrain(), estimator::airspeedSample::eas2tas, estimator::parameters::eas_noise, ECL_ERR_TIMESTAMPED, f(), fixCovarianceErrors(), estimator::fault_status_u::flags, estimator::innovation_fault_status_u::flags, fuse(), initialiseCovariance(), Kfusion, P, estimator::innovation_fault_status_u::reject_airspeed, resetWindCovariance(), resetWindStates(), sq(), estimator::parameters::tas_innov_gate, estimator::airspeedSample::true_airspeed, estimator::stateSample::vel, estimator::stateSample::wind_vel, zeroCols(), and zeroRows().

Referenced by controlAirDataFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fuseDeclination()

void Ekf::fuseDeclination ( float decl_sigma )

private

Definition at line 815 of file mag_fusion.cpp.

References EstimatorInterface::_fault_status, _k_num_states, _state, estimator::fault_status_u::bad_mag_decl, math::constrain(), f(), fixCovarianceErrors(), estimator::fault_status_u::flags, fuse(), getMagDeclination(), Kfusion, limitDeclination(), estimator::stateSample::mag_I, P, sq(), t10, t11, t12, t13, t14, t15, t18, t19, t2, t20, t21, t3, t4, t5, t6, t7, t8, t9, zeroCols(), and zeroRows().

Referenced by run3DMagAndDeclFusions().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fuseDrag()

void Ekf::fuseDrag ( )

private

Definition at line 46 of file drag_fusion.cpp.

References EstimatorInterface::_air_density, _drag_innov, _drag_innov_var, EstimatorInterface::_drag_sample_delayed, EstimatorInterface::_drag_test_ratio, _dt_ekf_avg, _k_num_states, EstimatorInterface::_params, _state, estimator::stateSample::accel_bias, estimator::dragSample::accelXY, estimator::parameters::bcoef_x, estimator::parameters::bcoef_y, estimator::parameters::drag_noise, f(), fixCovarianceErrors(), fuse(), Kfusion, P, estimator::stateSample::quat_nominal, EstimatorInterface::quat_to_invrotmat(), sq(), estimator::stateSample::vel, estimator::stateSample::wind_vel, zeroCols(), and zeroRows().

Referenced by controlDragFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fuseFlowForTerrain()

void Ekf::fuseFlowForTerrain ( )

private

Definition at line 185 of file terrain_estimator.cpp.

References _flow_gyro_bias, _flow_innov, _flow_innov_var, EstimatorInterface::_flow_sample_delayed, _flowRadXYcomp, EstimatorInterface::_params, _R_to_earth, _state, _terrain_var, _terrain_vpos, EstimatorInterface::_time_last_imu, _time_last_of_fuse, calcOptFlowMeasVar(), EstimatorInterface::cross_product(), estimator::flowSample::dt, f(), estimator::parameters::flow_innov_gate, estimator::parameters::flow_pos_body, estimator::flowSample::gyroXYZ, estimator::parameters::imu_pos_body, estimator::stateSample::pos, estimator::stateSample::quat_nominal, EstimatorInterface::quat_to_invrotmat(), estimator::parameters::rng_gnd_clearance, sq(), and estimator::stateSample::vel.

Referenced by runTerrainEstimator().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fuseGpsAntYaw()

void Ekf::fuseGpsAntYaw ( )

private

Definition at line 48 of file gps_yaw_fusion.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_fault_status, EstimatorInterface::_gps_sample_delayed, EstimatorInterface::_gps_yaw_offset, _heading_innov, _heading_innov_var, EstimatorInterface::_innov_check_fail_status, _k_num_states, EstimatorInterface::_mag_test_ratio, EstimatorInterface::_params, _R_to_earth, _state, EstimatorInterface::_yaw_test_ratio, estimator::fault_status_u::bad_hdg, math::constrain(), ECL_ERR_TIMESTAMPED, f(), fixCovarianceErrors(), estimator::fault_status_u::flags, estimator::innovation_fault_status_u::flags, estimator::filter_control_status_u::flags, fuse(), estimator::parameters::heading_innov_gate, estimator::filter_control_status_u::in_air, initialiseCovariance(), ISFINITE, Kfusion, estimator::parameters::mag_heading_noise, math::max(), P, estimator::stateSample::quat_nominal, math::radians(), estimator::innovation_fault_status_u::reject_yaw, sq(), t10, t11, t12, t13, t14, t15, t16, t17, t18, t19, t2, t20, t21, t22, t23, t24, t25, t26, t27, t28, t29, t3, t30, t31, t32, t33, t34, t35, t4, t5, t6, t7, t8, t9, estimator::filter_control_status_u::wind, matrix::wrap_pi(), estimator::gpsSample::yaw, zeroCols(), and zeroRows().

Referenced by controlGpsFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fuseHagl()

void Ekf::fuseHagl ( )

private

Definition at line 133 of file terrain_estimator.cpp.

References _hagl_innov, _hagl_innov_var, EstimatorInterface::_innov_check_fail_status, EstimatorInterface::_params, _R_rng_to_earth_2_2, EstimatorInterface::_range_sample_delayed, _state, EstimatorInterface::_terr_test_ratio, _terrain_var, _terrain_vpos, _time_last_hagl_fuse, EstimatorInterface::_time_last_imu, f(), estimator::innovation_fault_status_u::flags, P, estimator::stateSample::pos, estimator::parameters::range_cos_max_tilt, estimator::parameters::range_innov_gate, estimator::parameters::range_noise, estimator::parameters::range_noise_scaler, estimator::innovation_fault_status_u::reject_hagl, estimator::rangeSample::rng, sq(), and estimator::parameters::vehicle_variance_scaler.

Referenced by runTerrainEstimator().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fuseHeading()

void Ekf::fuseHeading ( )

private

Definition at line 438 of file mag_fusion.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_ev_sample_delayed, EstimatorInterface::_fault_status, _heading_innov, _heading_innov_var, EstimatorInterface::_innov_check_fail_status, _k_num_states, _last_static_yaw, EstimatorInterface::_mag_sample_delayed, EstimatorInterface::_mag_test_ratio, _mag_use_inhibit, _mag_use_inhibit_prev, EstimatorInterface::_params, _R_to_earth, _state, EstimatorInterface::_vehicle_at_rest, EstimatorInterface::_yaw_test_ratio, estimator::extVisionSample::angErr, estimator::fault_status_u::bad_hdg, math::constrain(), ECL_ERR_TIMESTAMPED, estimator::filter_control_status_u::ev_yaw, f(), fixCovarianceErrors(), estimator::fault_status_u::flags, estimator::innovation_fault_status_u::flags, estimator::filter_control_status_u::flags, fuse(), getMagDeclination(), estimator::parameters::heading_innov_gate, estimator::filter_control_status_u::in_air, initialiseCovariance(), Kfusion, estimator::magSample::mag, estimator::filter_control_status_u::mag_3D, estimator::stateSample::mag_B, estimator::filter_control_status_u::mag_hdg, estimator::parameters::mag_heading_noise, math::max(), P, estimator::extVisionSample::quat, estimator::stateSample::quat_nominal, estimator::innovation_fault_status_u::reject_yaw, sq(), t10, t11, t12, t13, t14, t2, t3, t4, t5, t6, t7, t8, t9, estimator::filter_control_status_u::wind, matrix::wrap_pi(), zeroCols(), and zeroRows().

Referenced by controlExternalVisionFusion(), and runMagAndMagDeclFusions().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fuseMag()

void Ekf::fuseMag ( )

private

Definition at line 47 of file mag_fusion.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_fault_status, _flt_mag_align_converging, EstimatorInterface::_innov_check_fail_status, _k_num_states, _mag_innov, _mag_innov_var, EstimatorInterface::_mag_sample_delayed, EstimatorInterface::_mag_test_ratio, EstimatorInterface::_params, _state, EstimatorInterface::_yaw_test_ratio, estimator::fault_status_u::bad_mag_x, estimator::fault_status_u::bad_mag_y, estimator::fault_status_u::bad_mag_z, ECL_ERR_TIMESTAMPED, f(), fixCovarianceErrors(), estimator::fault_status_u::flags, estimator::filter_control_status_u::flags, fuse(), Kfusion, limitDeclination(), estimator::magSample::mag, estimator::stateSample::mag_B, estimator::stateSample::mag_I, estimator::parameters::mag_innov_gate, estimator::parameters::mag_noise, math::max(), P, estimator::stateSample::quat_nominal, EstimatorInterface::quat_to_invrotmat(), resetMagRelatedCovariances(), sq(), estimator::filter_control_status_u::synthetic_mag_z, estimator::innovation_fault_status_u::value, zeroCols(), and zeroRows().

Referenced by run3DMagAndDeclFusions().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fuseOptFlow()

void Ekf::fuseOptFlow ( )

private

Definition at line 48 of file optflow_fusion.cpp.

References EstimatorInterface::_fault_status, _flow_gyro_bias, _flow_innov, _flow_innov_var, EstimatorInterface::_flow_max_rate, EstimatorInterface::_flow_sample_delayed, _flowRadXYcomp, EstimatorInterface::_innov_check_fail_status, _k_num_states, EstimatorInterface::_params, _R_to_earth, _state, _terrain_vpos, EstimatorInterface::_time_last_imu, _time_last_of_fuse, estimator::fault_status_u::bad_optflow_X, estimator::fault_status_u::bad_optflow_Y, calcOptFlowMeasVar(), EstimatorInterface::cross_product(), estimator::flowSample::dt, f(), fixCovarianceErrors(), estimator::fault_status_u::flags, estimator::parameters::flow_innov_gate, estimator::parameters::flow_pos_body, fuse(), estimator::flowSample::gyroXYZ, H_LOS, estimator::parameters::imu_pos_body, initialiseCovariance(), Kfusion, math::max(), P, estimator::stateSample::pos, estimator::stateSample::quat_nominal, EstimatorInterface::quat_to_invrotmat(), estimator::parameters::rng_gnd_clearance, sq(), t10, t11, t12, t13, t14, t15, t16, t17, t18, t19, t2, t20, t21, t22, t23, t24, t25, t26, t27, t28, t29, t3, t30, t31, t32, t33, t34, t35, t36, t37, t38, t39, t4, t40, t41, t42, t43, t44, t45, t46, t47, t48, t49, t5, t50, t51, t52, t53, t54, t55, t56, t57, t58, t59, t6, t60, t61, t62, t63, t64, t65, t66, t67, t68, t69, t7, t70, t71, t72, t73, t74, t75, t76, t77, t78, t79, t8, t80, t81, t82, t83, t84, t85, t86, t87, t88, t89, t9, t90, t91, t92, t93, t94, estimator::innovation_fault_status_u::value, estimator::stateSample::vel, zeroCols(), and zeroRows().

Referenced by controlOpticalFlowFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fuseSideslip()

void Ekf::fuseSideslip ( )

private

Definition at line 47 of file sideslip_fusion.cpp.

Referenced by controlBetaFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

void Ekf::get_ekf_ctrl_limits	(	float *	vxy_max,
		float *	vz_max,
		float *	hagl_min,
		float *	hagl_max
	)

overridevirtual

Implements EstimatorInterface.

Definition at line 1079 of file ekf_helper.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_flow_max_distance, EstimatorInterface::_flow_max_rate, EstimatorInterface::_flow_min_distance, EstimatorInterface::_params, EstimatorInterface::_rng_valid_max_val, EstimatorInterface::_rng_valid_min_val, _state, _terrain_vpos, estimator::filter_control_status_u::ev_pos, estimator::filter_control_status_u::ev_vel, f(), estimator::filter_control_status_u::flags, estimator::filter_control_status_u::gps, estimator::filter_control_status_u::opt_flow, estimator::stateSample::pos, estimator::parameters::range_aid, and estimator::filter_control_status_u::rng_hgt.

Referenced by position_covariances(), and Ekf2::Run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_ekf_gpos_accuracy()

void Ekf::get_ekf_gpos_accuracy	(	float *	ekf_eph,
		float *	ekf_epv
	)

overridevirtual

Implements EstimatorInterface.

Definition at line 1005 of file ekf_helper.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_gps_origin_eph, EstimatorInterface::_gps_origin_epv, EstimatorInterface::_is_dead_reckoning, _vel_pos_innov, estimator::filter_control_status_u::ev_pos, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::gps, math::max(), P, and sq().

Referenced by position_covariances(), and Ekf2::Run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_ekf_lpos_accuracy()

void Ekf::get_ekf_lpos_accuracy	(	float *	ekf_eph,
		float *	ekf_epv
	)

overridevirtual

Implements EstimatorInterface.

Definition at line 1024 of file ekf_helper.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_is_dead_reckoning, _vel_pos_innov, estimator::filter_control_status_u::ev_pos, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::gps, math::max(), P, and sq().

Referenced by position_covariances(), and Ekf2::Run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_ekf_origin()

bool Ekf::get_ekf_origin	(	uint64_t *	origin_time,
		map_projection_reference_s *	origin_pos,
		float *	origin_alt
	)

overridevirtual

Implements EstimatorInterface.

Definition at line 959 of file ekf_helper.cpp.

References _gps_alt_ref, _last_gps_origin_time_us, and EstimatorInterface::_NED_origin_initialised.

Referenced by position_covariances(), and Ekf2::Run().

Here is the caller graph for this function:

◆ get_ekf_soln_status()

void Ekf::get_ekf_soln_status ( uint16_t * status )

overridevirtual

Implements EstimatorInterface.

Definition at line 1178 of file ekf_helper.cpp.

Referenced by get_quat_reset(), and Ekf2::Run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_ekf_vel_accuracy()

void Ekf::get_ekf_vel_accuracy	(	float *	ekf_evh,
		float *	ekf_evv
	)

overridevirtual

Implements EstimatorInterface.

Definition at line 1041 of file ekf_helper.cpp.

References EstimatorInterface::_control_status, _flow_innov, EstimatorInterface::_is_dead_reckoning, EstimatorInterface::_params, _state, _terrain_vpos, _vel_pos_innov, estimator::filter_control_status_u::ev_pos, estimator::filter_control_status_u::ev_vel, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::gps, math::max(), estimator::filter_control_status_u::opt_flow, P, estimator::stateSample::pos, estimator::parameters::rng_gnd_clearance, and sq().

Referenced by position_covariances(), and Ekf2::Run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_ev2ekf_quaternion()

void Ekf::get_ev2ekf_quaternion ( float * quat )

overridevirtual

Implements EstimatorInterface.

Definition at line 1680 of file ekf_helper.cpp.

References _ev_rot_vec_filt.

Referenced by get_quat_reset(), and Ekf2::Run().

Here is the caller graph for this function:

◆ get_flow_innov()

void Ekf::get_flow_innov ( float flow_innov[2] )

overridevirtual

Implements EstimatorInterface.

Definition at line 518 of file optflow_fusion.cpp.

References _flow_innov.

Referenced by Ekf2::Run().

Here is the caller graph for this function:

◆ get_flow_innov_var()

void Ekf::get_flow_innov_var ( float flow_innov_var[2] )

overridevirtual

Implements EstimatorInterface.

Definition at line 524 of file optflow_fusion.cpp.

References _flow_innov_var.

Referenced by Ekf2::Run().

Here is the caller graph for this function:

◆ get_gps_check_status()

void Ekf::get_gps_check_status ( uint16_t * val )

overridevirtual

Implements EstimatorInterface.

Definition at line 896 of file ekf_helper.cpp.

References _gps_check_fail_status, and estimator::gps_check_fail_status_u::value.

Referenced by get_terrain_var(), and Ekf2::Run().

Here is the caller graph for this function:

◆ get_gps_drift_metrics()

bool Ekf::get_gps_drift_metrics	(	float	drift[3],
		bool *	blocked
	)

overridevirtual

Implements EstimatorInterface.

Definition at line 993 of file ekf_helper.cpp.

References EstimatorInterface::_gps_drift_metrics, EstimatorInterface::_gps_drift_updated, and EstimatorInterface::_vehicle_at_rest.

Referenced by position_covariances(), and Ekf2::Run().

Here is the caller graph for this function:

◆ get_gyro_bias()

void Ekf::get_gyro_bias ( float bias[3] )

overridevirtual

Implements EstimatorInterface.

Definition at line 948 of file ekf_helper.cpp.

References _dt_ekf_avg, _state, and estimator::stateSample::gyro_bias.

Referenced by get_terrain_var(), Ekf2::Run(), and TEST_F().

Here is the caller graph for this function:

◆ get_heading_innov()

void Ekf::get_heading_innov ( float * heading_innov )

overridevirtual

Implements EstimatorInterface.

Definition at line 859 of file ekf_helper.cpp.

References _heading_innov.

Referenced by Ekf2::Run().

Here is the caller graph for this function:

◆ get_heading_innov_var()

void Ekf::get_heading_innov_var ( float * heading_innov_var )

overridevirtual

Implements EstimatorInterface.

Definition at line 890 of file ekf_helper.cpp.

References _heading_innov_var.

Referenced by Ekf2::Run().

Here is the caller graph for this function:

◆ get_imu_vibe_metrics()

void Ekf::get_imu_vibe_metrics ( float vibe[3] )

overridevirtual

Implements EstimatorInterface.

Definition at line 980 of file ekf_helper.cpp.

References EstimatorInterface::_vibe_metrics.

Referenced by position_covariances(), and Ekf2::Run().

Here is the caller graph for this function:

◆ get_innovation_test_status()

void Ekf::get_innovation_test_status	(	uint16_t *	status,
		float *	mag,
		float *	vel,
		float *	pos,
		float *	hgt,
		float *	tas,
		float *	hagl,
		float *	beta
	)

overridevirtual

Implements EstimatorInterface.

Definition at line 1157 of file ekf_helper.cpp.

References EstimatorInterface::_beta_test_ratio, EstimatorInterface::_innov_check_fail_status, EstimatorInterface::_mag_test_ratio, EstimatorInterface::_tas_test_ratio, EstimatorInterface::_terr_test_ratio, EstimatorInterface::_vel_pos_test_ratio, EstimatorInterface::_yaw_test_ratio, math::max(), and estimator::innovation_fault_status_u::value.

Referenced by get_quat_reset(), and Ekf2::Run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_mag_innov()

void Ekf::get_mag_innov ( float mag_innov[3] )

overridevirtual

Implements EstimatorInterface.

Definition at line 841 of file ekf_helper.cpp.

References _mag_innov.

Referenced by Ekf2::Run().

Here is the caller graph for this function:

◆ get_mag_innov_var()

void Ekf::get_mag_innov_var ( float mag_innov_var[3] )

overridevirtual

Implements EstimatorInterface.

Definition at line 872 of file ekf_helper.cpp.

References _mag_innov_var.

Referenced by Ekf2::Run().

Here is the caller graph for this function:

◆ get_output_tracking_error()

void Ekf::get_output_tracking_error ( float error[3] )

overridevirtual

Implements EstimatorInterface.

Definition at line 969 of file ekf_helper.cpp.

References _output_tracking_error.

Referenced by position_covariances(), and Ekf2::Run().

Here is the caller graph for this function:

◆ get_pos_var()

void Ekf::get_pos_var ( Vector3f & pos_var )

overridevirtual

Implements EstimatorInterface.

Definition at line 121 of file covariance.cpp.

References P.

Referenced by position_covariances().

Here is the caller graph for this function:

◆ get_posD_reset()

void Ekf::get_posD_reset	(	float *	delta,
		uint8_t *	counter
	)

inlineoverridevirtual

Implements EstimatorInterface.

Definition at line 223 of file ekf.h.

References _state_reset_status.

Referenced by Ekf2::Run().

Here is the caller graph for this function:

◆ get_posNE_reset()

void Ekf::get_posNE_reset	(	float	delta[2],
		uint8_t *	counter
	)

inlineoverridevirtual

Implements EstimatorInterface.

Definition at line 229 of file ekf.h.

References _state_reset_status.

Referenced by Ekf2::Run().

Here is the caller graph for this function:

◆ get_quat_reset()

void Ekf::get_quat_reset	(	float	delta_quat[4],
		uint8_t *	counter
	)

inlineoverridevirtual

Implements EstimatorInterface.

Definition at line 243 of file ekf.h.

References _state_reset_status, calculate_quaternion(), get_ekf_soln_status(), get_ev2ekf_quaternion(), get_innovation_test_status(), and status.

Referenced by Ekf2::publish_attitude().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_state_delayed()

void Ekf::get_state_delayed ( float * state )

overridevirtual

Implements EstimatorInterface.

Definition at line 902 of file ekf_helper.cpp.

References _state, estimator::stateSample::accel_bias, estimator::stateSample::gyro_bias, estimator::stateSample::mag_B, estimator::stateSample::mag_I, estimator::stateSample::pos, estimator::stateSample::quat_nominal, estimator::stateSample::vel, and estimator::stateSample::wind_vel.

Referenced by Ekf2::Run().

Here is the caller graph for this function:

◆ get_terrain_var()

float Ekf::get_terrain_var ( ) const

inline

Definition at line 211 of file ekf.h.

References _terrain_var, get_accel_bias(), get_gps_check_status(), and get_gyro_bias().

Here is the call graph for this function:

void Ekf::getHaglInnov ( float * hagl_innov )

overridevirtual

Implements EstimatorInterface.

Definition at line 311 of file terrain_estimator.cpp.

References _hagl_innov.

◆ getHaglInnovVar()

void Ekf::getHaglInnovVar ( float * hagl_innov_var )

overridevirtual

Implements EstimatorInterface.

Definition at line 317 of file terrain_estimator.cpp.

References _hagl_innov_var.

◆ getMagDeclination()

float Ekf::getMagDeclination ( )

private

Definition at line 750 of file ekf_helper.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_mag_declination_gps, EstimatorInterface::_NED_origin_initialised, EstimatorInterface::_params, _state, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::mag_aligned_in_flight, estimator::parameters::mag_declination_deg, estimator::parameters::mag_declination_source, estimator::stateSample::mag_I, MASK_USE_GEO_DECL, and math::radians().

Referenced by fuseDeclination(), fuseHeading(), limitDeclination(), and resetMagHeading().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getTerrainVertPos()

void Ekf::getTerrainVertPos ( float * ret )

overridevirtual

Implements EstimatorInterface.

Definition at line 306 of file terrain_estimator.cpp.

References _terrain_vpos.

Referenced by position_covariances().

Here is the caller graph for this function:

◆ getTerrainVPos()

float Ekf::getTerrainVPos ( ) const

private

Definition at line 128 of file mag_control.cpp.

References _last_on_ground_posD, _terrain_vpos, and isTerrainEstimateValid().

Referenced by checkHaglYawResetReq().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ global_position_is_valid()

bool Ekf::global_position_is_valid ( )

overridevirtual

Implements EstimatorInterface.

Definition at line 1317 of file ekf_helper.cpp.

References EstimatorInterface::_deadreckon_time_exceeded, EstimatorInterface::_NED_origin_initialised, and _using_synthetic_position.

Referenced by position_covariances(), Ekf2::print_status(), and Ekf2::Run().

Here is the caller graph for this function:

◆ gps_is_good()

bool Ekf::gps_is_good ( const gps_message & gps )

private

Definition at line 116 of file gps_checks.cpp.

Referenced by collect_gps().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ increaseQuatYawErrVariance()

void Ekf::increaseQuatYawErrVariance ( float yaw_variance )

private

Definition at line 1692 of file ekf_helper.cpp.

References _state, f(), P, estimator::stateSample::quat_nominal, SG, SQ, and sq().

Referenced by controlExternalVisionFusion(), initialiseFilter(), isRangeDataContinuous(), realignYawGPS(), resetGpsAntYaw(), and resetMagHeading().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init()

bool Ekf::init ( uint64_t timestamp )

overridevirtual

Implements EstimatorInterface.

Definition at line 47 of file ekf.cpp.

References EstimatorInterface::initialise_interface(), and reset().

Referenced by EkfSamplingTestParametrized::SetUp(), EkfImuSamplingTestParametrized::SetUp(), and EkfInitializationTest::SetUp().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initHagl()

bool Ekf::initHagl ( )

private

Definition at line 46 of file terrain_estimator.cpp.

References EstimatorInterface::_control_status, _flow_for_terrain_data_ready, EstimatorInterface::_params, _R_rng_to_earth_2_2, EstimatorInterface::_range_buffer, _rng_hgt_valid, _state, _terrain_var, _terrain_vpos, _time_last_hagl_fuse, EstimatorInterface::_time_last_imu, f(), estimator::filter_control_status_u::flags, RingBuffer< data_type >::get_newest(), estimator::filter_control_status_u::in_air, estimator::stateSample::pos, estimator::parameters::range_cos_max_tilt, estimator::parameters::range_noise, estimator::rangeSample::rng, estimator::parameters::rng_gnd_clearance, sq(), and estimator::rangeSample::time_us.

Referenced by initialiseFilter(), and runTerrainEstimator().

Here is the call graph for this function:

Here is the caller graph for this function:

bool Ekf::isMagBiasObservable ( ) const

private

Definition at line 232 of file mag_control.cpp.

References _mag_bias_observable.

Referenced by check3DMagFusionSuitability().

Here is the caller graph for this function:

◆ isMeasuredMatchingAverageMagStrength()

bool Ekf::isMeasuredMatchingAverageMagStrength ( ) const

private

Definition at line 308 of file mag_control.cpp.

References EstimatorInterface::_mag_sample_delayed, isMeasuredMatchingExpected(), and estimator::magSample::mag.

Referenced by checkMagFieldStrength().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isMeasuredMatchingExpected()

bool Ekf::isMeasuredMatchingExpected	(	float	measured,
		float	expected,
		float	gate
	)

staticprivate

Definition at line 317 of file mag_control.cpp.

Referenced by isMeasuredMatchingAverageMagStrength(), and isMeasuredMatchingGpsMagStrength().

Here is the caller graph for this function:

◆ isMeasuredMatchingGpsMagStrength()

bool Ekf::isMeasuredMatchingGpsMagStrength ( ) const

private

Definition at line 302 of file mag_control.cpp.

References EstimatorInterface::_mag_sample_delayed, EstimatorInterface::_mag_strength_gps, isMeasuredMatchingExpected(), and estimator::magSample::mag.

Referenced by checkMagFieldStrength().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isRangeAidSuitable()

bool Ekf::isRangeAidSuitable ( )

inlineprivate

Definition at line 686 of file ekf.h.

References _is_range_aid_suitable, updateRangeDataStuck(), and updateRangeDataValidity().

Referenced by controlHeightFusion(), and controlOpticalFlowFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isRangeDataContinuous()

bool Ekf::isRangeDataContinuous ( )

inlineprivate

Definition at line 749 of file ekf.h.

References _dt_last_range_update_filt_us, calculate_synthetic_mag_z_measurement(), clearMagCov(), increaseQuatYawErrVariance(), kahanSummation(), loadMagCovData(), saveMagCovData(), uncorrelateQuatStates(), and zeroMagCov().

Referenced by updateRangeDataValidity().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isStrongMagneticDisturbance()

bool Ekf::isStrongMagneticDisturbance ( ) const

private

Definition at line 297 of file mag_control.cpp.

References EstimatorInterface::_control_status, estimator::filter_control_status_u::flags, and estimator::filter_control_status_u::mag_field_disturbed.

Referenced by canResetMagHeading(), and shouldInhibitMag().

Here is the caller graph for this function:

◆ isTerrainEstimateValid()

bool Ekf::isTerrainEstimateValid ( ) const

overridevirtual

Implements EstimatorInterface.

Definition at line 287 of file terrain_estimator.cpp.

References _hagl_valid.

Referenced by checkRangeAidSuitability(), controlHeightFusion(), controlOpticalFlowFusion(), get_ekf_soln_status(), getTerrainVPos(), and position_covariances().

Here is the caller graph for this function:

◆ isYawAngleObservable()

bool Ekf::isYawAngleObservable ( ) const

private

Definition at line 227 of file mag_control.cpp.

References _yaw_angle_observable.

Referenced by check3DMagFusionSuitability().

Here is the caller graph for this function:

◆ isYawResetAuthorized()

bool Ekf::isYawResetAuthorized ( ) const

private

Definition at line 144 of file mag_control.cpp.

References _mag_use_inhibit.

Referenced by runInAirYawReset(), and runOnGroundYawReset().

Here is the caller graph for this function:

◆ kahanSummation()

float Ekf::kahanSummation	(	float	sum_previous,
		float	input,
		float &	accumulator
	)		const

private

Definition at line 1762 of file ekf_helper.cpp.

Referenced by isRangeDataContinuous(), and predictCovariance().

Here is the caller graph for this function:

◆ limitDeclination()

void Ekf::limitDeclination ( )

private

Definition at line 955 of file mag_fusion.cpp.

References EstimatorInterface::_mag_declination_gps, EstimatorInterface::_mag_inclination_gps, EstimatorInterface::_mag_strength_gps, EstimatorInterface::_NED_origin_initialised, EstimatorInterface::_params, _state, f(), getMagDeclination(), estimator::parameters::mag_declination_deg, estimator::parameters::mag_declination_source, estimator::stateSample::mag_I, MASK_USE_GEO_DECL, and math::radians().

Referenced by fuseDeclination(), and fuseMag().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ loadMagCovData()

void Ekf::loadMagCovData ( )

private

Definition at line 1748 of file ekf_helper.cpp.

References _saved_mag_bf_variance, _saved_mag_ef_covmat, and P.

Referenced by isRangeDataContinuous(), and startMag3DFusion().

Here is the caller graph for this function:

◆ makeSymmetrical()

void Ekf::makeSymmetrical	(	float(&)	cov_mat[_k_num_states][_k_num_states],
		uint8_t	first,
		uint8_t	last
	)

private

Definition at line 773 of file ekf_helper.cpp.

Referenced by fixCovarianceErrors().

Here is the caller graph for this function:

◆ orientation_covariances()

matrix::SquareMatrix<float, 4> Ekf::orientation_covariances ( ) const

inline

Definition at line 135 of file ekf.h.

References covariances().

Here is the call graph for this function:

◆ position_covariances()

matrix::SquareMatrix<float, 3> Ekf::position_covariances ( ) const

inline

Definition at line 141 of file ekf.h.

References collect_gps(), collect_imu(), covariances(), get_ekf_ctrl_limits(), get_ekf_gpos_accuracy(), get_ekf_lpos_accuracy(), get_ekf_origin(), get_ekf_vel_accuracy(), get_gps_drift_metrics(), get_imu_vibe_metrics(), get_output_tracking_error(), get_pos_var(), get_vel_var(), getTerrainVertPos(), global_position_is_valid(), gps, isTerrainEstimateValid(), reset_imu_bias(), update_deadreckoning_status(), and updateTerrainValidity().

Here is the call graph for this function:

◆ predictCovariance()

void Ekf::predictCovariance ( )

private

Definition at line 135 of file covariance.cpp.

Referenced by update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ predictState()

void Ekf::predictState ( )

private

Definition at line 318 of file ekf.cpp.

References _accel_lpf_NE, _dt_ekf_avg, _earth_rate_initialised, _earth_rate_NED, EstimatorInterface::_imu_sample_delayed, EstimatorInterface::_NED_origin_initialised, _R_to_earth, _state, estimator::stateSample::accel_bias, calcEarthRateNED(), CONSTANTS_ONE_G, math::constrain(), constrainStates(), estimator::imuSample::delta_ang, estimator::imuSample::delta_ang_dt, estimator::imuSample::delta_vel, estimator::imuSample::delta_vel_dt, f(), EstimatorInterface::FILTER_UPDATE_PERIOD_S, estimator::stateSample::gyro_bias, estimator::stateSample::pos, estimator::stateSample::quat_nominal, and estimator::stateSample::vel.

Referenced by update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ realignYawGPS()

bool Ekf::realignYawGPS ( )

private

Definition at line 405 of file ekf_helper.cpp.

Referenced by controlGpsFusion(), and runInAirYawReset().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reset()

void Ekf::reset ( uint64_t timestamp )

overridevirtual

Implements EstimatorInterface.

Definition at line 55 of file ekf.cpp.

Referenced by init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reset_imu_bias()

bool Ekf::reset_imu_bias ( )

overridevirtual

Implements EstimatorInterface.

Definition at line 1122 of file ekf_helper.cpp.

References EstimatorInterface::_imu_sample_delayed, _last_imu_bias_cov_reset_us, EstimatorInterface::_params, _prev_dvel_bias_var, _state, estimator::stateSample::accel_bias, dt, EstimatorInterface::FILTER_UPDATE_PERIOD_S, estimator::stateSample::gyro_bias, P, sq(), estimator::parameters::switch_on_accel_bias, estimator::parameters::switch_on_gyro_bias, estimator::imuSample::time_us, zeroCols(), and zeroRows().

Referenced by position_covariances(), and Ekf2::Run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ resetExtVisRotMat()

void Ekf::resetExtVisRotMat ( )

private

Definition at line 1657 of file ekf_helper.cpp.

References _ev_rot_mat, _ev_rot_vec_filt, EstimatorInterface::_ev_sample_delayed, _state, f(), estimator::extVisionSample::quat, and estimator::stateSample::quat_nominal.

Referenced by controlExternalVisionFusion(), resetGpsAntYaw(), and resetMagHeading().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ resetGpsAntYaw()

bool Ekf::resetGpsAntYaw ( )

private

Definition at line 293 of file gps_yaw_fusion.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_gps_sample_delayed, EstimatorInterface::_gps_yaw_offset, EstimatorInterface::_output_buffer, EstimatorInterface::_output_new, EstimatorInterface::_params, _R_to_earth, _state, _state_reset_status, f(), estimator::filter_control_status_u::flags, estimator::parameters::fusion_mode, RingBuffer< data_type >::get_length(), increaseQuatYawErrVariance(), ISFINITE, estimator::parameters::mag_heading_noise, MASK_ROTATE_EV, MASK_USE_EVPOS, estimator::outputSample::quat_nominal, estimator::stateSample::quat_nominal, math::radians(), resetExtVisRotMat(), sq(), uncorrelateQuatStates(), matrix::wrap_pi(), estimator::gpsSample::yaw, and estimator::filter_control_status_u::yaw_align.

Referenced by controlGpsFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ resetHeight()

void Ekf::resetHeight ( )

private

Definition at line 214 of file ekf_helper.cpp.

Referenced by controlExternalVisionFusion(), controlHeightSensorTimeouts(), and initialiseFilter().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ resetMagHeading()

bool Ekf::resetMagHeading	(	const Vector3f &	mag_init,
		bool	increase_yaw_var = `true`,
		bool	update_buffer = `true`
	)

private

Definition at line 554 of file ekf_helper.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_ev_sample_delayed, _flt_mag_align_start_time, EstimatorInterface::_imu_sample_delayed, _mag_use_inhibit, EstimatorInterface::_output_buffer, EstimatorInterface::_output_new, EstimatorInterface::_params, _R_to_earth, _state, _state_reset_status, estimator::extVisionSample::angErr, clearMagCov(), estimator::filter_control_status_u::ev_yaw, f(), estimator::filter_control_status_u::flags, estimator::parameters::fusion_mode, RingBuffer< data_type >::get_length(), getMagDeclination(), increaseQuatYawErrVariance(), estimator::filter_control_status_u::mag_3D, MAG_FUSE_TYPE_3D, MAG_FUSE_TYPE_INDOOR, MAG_FUSE_TYPE_NONE, estimator::parameters::mag_fusion_type, estimator::parameters::mag_heading_noise, estimator::stateSample::mag_I, estimator::parameters::mag_noise, MASK_ROTATE_EV, P, estimator::extVisionSample::quat, estimator::outputSample::quat_nominal, estimator::stateSample::quat_nominal, resetExtVisRotMat(), saveMagCovData(), sq(), stopMagFusion(), estimator::imuSample::time_us, and uncorrelateQuatStates().

Referenced by controlFusionModes(), controlGpsFusion(), controlOpticalFlowFusion(), initialiseFilter(), realignYawGPS(), runInAirYawReset(), and runOnGroundYawReset().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ resetMagRelatedCovariances()

void Ekf::resetMagRelatedCovariances ( )

private

Definition at line 899 of file covariance.cpp.

References EstimatorInterface::_params, clearMagCov(), estimator::parameters::mag_noise, P, saveMagCovData(), sq(), zeroCols(), and zeroRows().

Referenced by fuseMag(), and sq().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ resetPosition()

bool Ekf::resetPosition ( )

private

Definition at line 139 of file ekf_helper.cpp.

References EstimatorInterface::_control_status, _ev_rot_mat, EstimatorInterface::_ev_sample_delayed, EstimatorInterface::_gps_sample_delayed, _hpos_prev_available, _last_known_posNE, EstimatorInterface::_output_buffer, EstimatorInterface::_output_new, EstimatorInterface::_params, _state, _state_reset_status, estimator::filter_control_status_u::ev_pos, estimator::filter_control_status_u::flags, estimator::parameters::fusion_mode, RingBuffer< data_type >::get_length(), estimator::filter_control_status_u::gps, estimator::gpsSample::hacc, estimator::filter_control_status_u::in_air, MASK_ROTATE_EV, estimator::filter_control_status_u::opt_flow, P, estimator::outputSample::pos, estimator::gpsSample::pos, estimator::extVisionSample::pos, estimator::stateSample::pos, estimator::parameters::pos_noaid_noise, estimator::extVisionSample::posErr, posNE_change, setDiag(), sq(), zeroCols(), and zeroRows().

Referenced by controlExternalVisionFusion(), controlGpsFusion(), controlOpticalFlowFusion(), controlVelPosFusion(), and runVelPosReset().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ resetStates()

void Ekf::resetStates ( )

overridevirtual

Implements EstimatorInterface.

Definition at line 99 of file ekf.cpp.

References EstimatorInterface::_output_new, _state, estimator::stateSample::accel_bias, estimator::stateSample::gyro_bias, estimator::stateSample::mag_B, estimator::stateSample::mag_I, estimator::outputSample::pos, estimator::stateSample::pos, estimator::outputSample::quat_nominal, estimator::stateSample::quat_nominal, estimator::outputSample::vel, estimator::stateSample::vel, and estimator::stateSample::wind_vel.

Referenced by resetStatesAndCovariances().

Here is the caller graph for this function:

◆ resetStatesAndCovariances()

void Ekf::resetStatesAndCovariances ( )

overridevirtual

Implements EstimatorInterface.

Definition at line 93 of file ekf.cpp.

References initialiseCovariance(), and resetStates().

Referenced by reset().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ resetVelocity()

bool Ekf::resetVelocity ( )

private

Definition at line 50 of file ekf_helper.cpp.

References EstimatorInterface::_control_status, _ev_rot_mat, EstimatorInterface::_ev_sample_delayed, EstimatorInterface::_flow_sample_delayed, _flowRadXYcomp, _gps_check_fail_status, EstimatorInterface::_gps_sample_delayed, EstimatorInterface::_output_buffer, EstimatorInterface::_output_new, EstimatorInterface::_params, _R_rng_to_earth_2_2, _R_to_earth, _state, _state_reset_status, _terrain_vpos, calcOptFlowMeasVar(), estimator::flowSample::dt, estimator::filter_control_status_u::ev_pos, estimator::filter_control_status_u::ev_vel, f(), estimator::filter_control_status_u::flags, estimator::parameters::fusion_mode, RingBuffer< data_type >::get_length(), estimator::filter_control_status_u::gps, MASK_ROTATE_EV, estimator::filter_control_status_u::opt_flow, P, estimator::stateSample::pos, estimator::parameters::rng_gnd_clearance, estimator::gpsSample::sacc, setDiag(), sq(), estimator::gps_check_fail_status_u::value, estimator::outputSample::vel, estimator::gpsSample::vel, estimator::extVisionSample::vel, estimator::stateSample::vel, estimator::extVisionSample::velErr, zeroCols(), zeroOffDiag(), and zeroRows().

Referenced by controlExternalVisionFusion(), controlGpsFusion(), controlOpticalFlowFusion(), controlVelPosFusion(), and runVelPosReset().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ resetWindCovariance()

void Ekf::resetWindCovariance ( )

private

Definition at line 929 of file covariance.cpp.

References EstimatorInterface::_airspeed_sample_delayed, EstimatorInterface::_imu_sample_delayed, EstimatorInterface::_params, _state, _tas_data_ready, math::constrain(), estimator::airspeedSample::eas2tas, estimator::parameters::eas_noise, f(), P, math::radians(), sq(), estimator::imuSample::time_us, estimator::airspeedSample::time_us, estimator::stateSample::wind_vel, zeroCols(), and zeroRows().

Referenced by controlAirDataFusion(), controlBetaFusion(), controlDragFusion(), fuseAirspeed(), fuseSideslip(), and sq().

Here is the call graph for this function:

Here is the caller graph for this function:

void Ekf::saveMagCovData ( )

private

Definition at line 1733 of file ekf_helper.cpp.

References _saved_mag_bf_variance, _saved_mag_ef_covmat, and P.

Referenced by initialiseCovariance(), isRangeDataContinuous(), realignYawGPS(), resetMagHeading(), resetMagRelatedCovariances(), and stopMag3DFusion().

Here is the caller graph for this function:

◆ selectMagAuto()

void Ekf::selectMagAuto ( )

void Ekf::setDiag	(	float(&)	cov_mat[_k_num_states][_k_num_states],
		uint8_t	first,
		uint8_t	last,
		float	variance
	)

private

Definition at line 1302 of file ekf_helper.cpp.

References zeroCols(), and zeroRows().

Referenced by controlGpsFusion(), resetPosition(), resetVelocity(), and sq().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ shouldInhibitMag()

bool Ekf::shouldInhibitMag ( ) const

private

Definition at line 269 of file mag_control.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_params, estimator::filter_control_status_u::ev_pos, estimator::filter_control_status_u::ev_vel, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::gps, isStrongMagneticDisturbance(), MAG_FUSE_TYPE_INDOOR, and estimator::parameters::mag_fusion_type.

Referenced by checkMagInhibition().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ sq()

static constexpr float Ekf::sq ( float var )

inlinestaticprivate

Definition at line 695 of file ekf.h.

References calcOptFlowMeasVar(), calcRotVecVariances(), initialiseQuatCovariances(), resetMagRelatedCovariances(), resetWindCovariance(), resetWindStates(), setControlBaroHeight(), setControlEVHeight(), setControlGPSHeight(), setControlRangeHeight(), setDiag(), startMag3DFusion(), startMagHdgFusion(), stopMag3DFusion(), stopMagFusion(), stopMagHdgFusion(), updateRangeDataContinuity(), zeroCols(), zeroOffDiag(), and zeroRows().

Referenced by calcOptFlowMeasVar(), calculate_synthetic_mag_z_measurement(), calculateOutputStates(), checkRangeAidSuitability(), controlExternalVisionFusion(), controlFusionModes(), controlGpsFusion(), controlHeightSensorTimeouts(), fixCovarianceErrors(), fuseAirspeed(), fuseDeclination(), fuseDrag(), fuseFlowForTerrain(), fuseGpsAntYaw(), fuseHagl(), fuseHeading(), fuseMag(), fuseOptFlow(), fuseSideslip(), fuseVelPosHeight(), get_ekf_gpos_accuracy(), get_ekf_lpos_accuracy(), get_ekf_vel_accuracy(), get_true_airspeed(), increaseQuatYawErrVariance(), initHagl(), initialiseCovariance(), initialiseFilter(), predictCovariance(), realignYawGPS(), reset_imu_bias(), resetGpsAntYaw(), resetHeight(), resetMagHeading(), resetMagRelatedCovariances(), resetPosition(), resetVelocity(), resetWindCovariance(), and runTerrainEstimator().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ startMag3DFusion()

void Ekf::startMag3DFusion ( )

private

Definition at line 1608 of file ekf_helper.cpp.

References EstimatorInterface::_control_status, estimator::filter_control_status_u::flags, loadMagCovData(), estimator::filter_control_status_u::mag_3D, stopMagHdgFusion(), and zeroMagCov().

Referenced by controlMagFusion(), selectMagAuto(), and sq().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ startMagHdgFusion()

void Ekf::startMagHdgFusion ( )

private

Definition at line 1602 of file ekf_helper.cpp.

References EstimatorInterface::_control_status, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::mag_hdg, and stopMag3DFusion().

Referenced by controlMagFusion(), selectMagAuto(), and sq().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stopMag3DFusion()

void Ekf::stopMag3DFusion ( )

private

Definition at line 1588 of file ekf_helper.cpp.

References EstimatorInterface::_control_status, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::mag_3D, and saveMagCovData().

Referenced by controlExternalVisionFusion(), controlGpsFusion(), sq(), startMagHdgFusion(), and stopMagFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stopMagFusion()

void Ekf::stopMagFusion ( )

private

Definition at line 1581 of file ekf_helper.cpp.

References clearMagCov(), stopMag3DFusion(), and stopMagHdgFusion().

Referenced by controlMagFusion(), resetMagHeading(), and sq().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stopMagHdgFusion()

void Ekf::stopMagHdgFusion ( )

private

Definition at line 1597 of file ekf_helper.cpp.

References EstimatorInterface::_control_status, estimator::filter_control_status_u::flags, and estimator::filter_control_status_u::mag_hdg.

Referenced by controlExternalVisionFusion(), controlGpsFusion(), sq(), startMag3DFusion(), and stopMagFusion().

Here is the caller graph for this function:

◆ uncorrelateQuatStates()

void Ekf::uncorrelateQuatStates ( )

private

Definition at line 1278 of file ekf_helper.cpp.

References P, zeroCols(), and zeroRows().

Referenced by controlExternalVisionFusion(), isRangeDataContinuous(), realignYawGPS(), resetGpsAntYaw(), and resetMagHeading().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ update()

bool Ekf::update ( )

overridevirtual

Implements EstimatorInterface.

Definition at line 117 of file ekf.cpp.

References _filter_initialised, EstimatorInterface::_imu_updated, calculateOutputStates(), controlFusionModes(), initialiseFilter(), predictCovariance(), predictState(), and runTerrainEstimator().

Referenced by Ekf2::Run(), and EkfInitializationTest::update_with_const_sensors().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ update_deadreckoning_status()

void Ekf::update_deadreckoning_status ( )

Definition at line 1325 of file ekf_helper.cpp.

References EstimatorInterface::_control_status, EstimatorInterface::_deadreckon_time_exceeded, EstimatorInterface::_is_dead_reckoning, EstimatorInterface::_is_wind_dead_reckoning, EstimatorInterface::_params, _time_ins_deadreckon_start, _time_last_arsp_fuse, _time_last_beta_fuse, _time_last_delpos_fuse, EstimatorInterface::_time_last_imu, _time_last_of_fuse, _time_last_pos_fuse, _time_last_vel_fuse, estimator::filter_control_status_u::ev_pos, estimator::filter_control_status_u::ev_vel, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::gps, estimator::parameters::no_aid_timeout_max, estimator::filter_control_status_u::opt_flow, estimator::parameters::valid_timeout_max, and estimator::filter_control_status_u::wind.

Referenced by controlFusionModes(), and position_covariances().

Here is the caller graph for this function:

◆ updateMagFilter()

void Ekf::updateMagFilter ( )

private

Definition at line 101 of file mag_control.cpp.

References _mag_data_ready, _mag_lpf, EstimatorInterface::_mag_sample_delayed, estimator::magSample::mag, and AlphaFilter< T >::update().

Referenced by controlMagFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

void Ekf::updateTerrainValidity ( )

Definition at line 292 of file terrain_estimator.cpp.

References EstimatorInterface::_control_status, _hagl_valid, _terrain_initialised, _time_last_hagl_fuse, EstimatorInterface::_time_last_imu, _time_last_of_fuse, estimator::filter_control_status_u::flags, and estimator::filter_control_status_u::opt_flow.

Referenced by position_covariances(), and runTerrainEstimator().

Here is the caller graph for this function:

◆ velocity_covariances()

matrix::SquareMatrix<float, 3> Ekf::velocity_covariances ( ) const

inline

Definition at line 138 of file ekf.h.

References covariances().

Here is the call graph for this function:

◆ zeroCols()

void Ekf::zeroCols	(	float(&)	cov_mat[_k_num_states][_k_num_states],
		uint8_t	first,
		uint8_t	last
	)

private

Definition at line 1249 of file ekf_helper.cpp.

Referenced by fixCovarianceErrors(), fuseAirspeed(), fuseDeclination(), fuseDrag(), fuseGpsAntYaw(), fuseHeading(), fuseMag(), fuseOptFlow(), fuseSideslip(), fuseVelPosHeight(), initialiseQuatCovariances(), predictCovariance(), reset_imu_bias(), resetHeight(), resetMagRelatedCovariances(), resetPosition(), resetVelocity(), resetWindCovariance(), setDiag(), sq(), uncorrelateQuatStates(), zeroMagCov(), and zeroOffDiag().

Here is the caller graph for this function:

◆ zeroMagCov()

void Ekf::zeroMagCov ( )

private

Definition at line 923 of file covariance.cpp.

References P, zeroCols(), and zeroRows().

Referenced by clearMagCov(), isRangeDataContinuous(), and startMag3DFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ zeroOffDiag()

void Ekf::zeroOffDiag	(	float(&)	cov_mat[_k_num_states][_k_num_states],
		uint8_t	first,
		uint8_t	last
	)

private

Definition at line 1258 of file ekf_helper.cpp.

References _k_num_states, zeroCols(), and zeroRows().

Referenced by resetVelocity(), and sq().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ zeroRows()

void Ekf::zeroRows	(	float(&)	cov_mat[_k_num_states][_k_num_states],
		uint8_t	first,
		uint8_t	last
	)

private

Definition at line 447 of file ekf.h.

Referenced by get_gps_check_status(), gps_is_good(), and resetVelocity().

private

last system time in usec that the GPS failed it's checks

Definition at line 417 of file ekf.h.

Referenced by controlGpsFusion(), and gps_is_good().

◆ _last_gps_origin_time_us

uint64_t Ekf::_last_gps_origin_time_us {0}

private

true when all active GPS checks have passed

Referenced by controlExternalVisionFusion(), get_posD_reset(), get_posNE_reset(), get_quat_reset(), get_velD_reset(), get_velNE_reset(), realignYawGPS(), resetGpsAntYaw(), resetHeight(), resetMagHeading(), resetPosition(), and resetVelocity().

uint64_t Ekf::_time_last_fake_gps {0}

private

same flag as "_flow_data_ready" but used for separate terrain estimator

last time we faked GPS position measurements to constrain tilt errors during operation without external aiding (uSec)

Definition at line 326 of file ekf.h.

Referenced by controlVelPosFusion().

◆ _time_last_hagl_fuse

uint64_t Ekf::_time_last_hagl_fuse {0}

private

last system time that the hagl measurement failed it's checks (uSec)

Definition at line 461 of file ekf.h.

Referenced by controlFusionModes(), controlOpticalFlowFusion(), fuseHagl(), initHagl(), initialiseFilter(), and updateTerrainValidity().

◆ _time_last_hgt_fuse

uint64_t Ekf::_time_last_hgt_fuse {0}

private

time the last fusion of height measurements was performed (uSec)

Definition at line 333 of file ekf.h.

Referenced by controlHeightFusion(), controlHeightSensorTimeouts(), fuseVelPosHeight(), and initialiseFilter().

float Ekf::P[_k_num_states][_k_num_states] {}

private

◆ posD_change

float Ekf::posD_change

Down position change due to last reset (m)

Definition at line 281 of file ekf.h.

◆ posD_counter

uint8_t Ekf::posD_counter

number of vertical position reset events (allow to wrap if count exceeds 255)

Definition at line 276 of file ekf.h.

◆ posNE_change

Vector2f Ekf::posNE_change

North, East position change due to last reset (m)

Definition at line 280 of file ekf.h.

Referenced by resetPosition().

◆ posNE_counter

uint8_t Ekf::posNE_counter

number of horizontal position reset events (allow to wrap if count exceeds 255)

Definition at line 275 of file ekf.h.

◆ quat_change

Quatf Ekf::quat_change

quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion

Definition at line 282 of file ekf.h.

◆ quat_counter

uint8_t Ekf::quat_counter

number of quaternion reset events (allow to wrap if count exceeds 255)

Definition at line 277 of file ekf.h.

◆ velD_change

float Ekf::velD_change

Down velocity change due to last reset (m/sec)

Definition at line 279 of file ekf.h.

◆ velD_counter

uint8_t Ekf::velD_counter

number of vertical velocity reset events (allow to wrap if count exceeds 255)

Definition at line 274 of file ekf.h.

◆ velNE_change

Vector2f Ekf::velNE_change

North East velocity change due to last reset (m)

Definition at line 278 of file ekf.h.

◆ velNE_counter

uint8_t Ekf::velNE_counter

number of horizontal position reset events (allow to wrap if count exceeds 255)

Definition at line 273 of file ekf.h.

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

src/lib/ecl/EKF/ekf.h
src/lib/ecl/EKF/airspeed_fusion.cpp
src/lib/ecl/EKF/control.cpp
src/lib/ecl/EKF/covariance.cpp
src/lib/ecl/EKF/drag_fusion.cpp
src/lib/ecl/EKF/ekf.cpp
src/lib/ecl/EKF/ekf_helper.cpp
src/lib/ecl/EKF/gps_checks.cpp
src/lib/ecl/EKF/gps_yaw_fusion.cpp
src/lib/ecl/EKF/mag_control.cpp
src/lib/ecl/EKF/mag_fusion.cpp
src/lib/ecl/EKF/optflow_fusion.cpp
src/lib/ecl/EKF/range_finder_checks.cpp
src/lib/ecl/EKF/sideslip_fusion.cpp
src/lib/ecl/EKF/terrain_estimator.cpp
src/lib/ecl/EKF/vel_pos_fusion.cpp

Public Member Functions

Private Member Functions

Static Private Member Functions

Private Attributes

Static Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ Ekf()

◆ ~Ekf()

Member Function Documentation

◆ alignOutputFilter()

◆ calcEarthRateNED()

◆ calcExtVisRotMat()

◆ calcOptFlowBodyRateComp()

◆ calcOptFlowMeasVar()

◆ calcRotVecVariances()

◆ calculate_quaternion()

◆ calculate_synthetic_mag_z_measurement()

◆ calculateOutputStates()

◆ canRealignYawUsingGps()

◆ canResetMagHeading()

◆ canRunMagFusion()

◆ canUse3DMagFusion()

◆ check3DMagFusionSuitability()

◆ checkHaglYawResetReq()

◆ checkMagBiasObservability()

◆ checkMagDeclRequired()

◆ checkMagFieldStrength()

◆ checkMagInhibition()

◆ checkRangeAidSuitability()

◆ checkYawAngleObservability()

◆ clearMagCov()

◆ collect_gps()

◆ collect_imu()

◆ constrainStates()

◆ controlAirDataFusion()

◆ controlAuxVelFusion()

◆ controlBaroFusion()

◆ controlBetaFusion()

◆ controlDragFusion()

◆ controlExternalVisionFusion()

◆ controlFusionModes()

◆ controlGpsFusion()

◆ controlHeightFusion()

◆ controlHeightSensorTimeouts()

◆ controlMagFusion()

◆ controlOpticalFlowFusion()

◆ controlRangeFinderFusion()

◆ controlVelPosFusion()

◆ covariances()

◆ covariances_diagonal()

◆ fixCovarianceErrors()

◆ fuse()

◆ fuseAirspeed()

◆ fuseDeclination()

◆ fuseDrag()

◆ fuseFlowForTerrain()

◆ fuseGpsAntYaw()

◆ fuseHagl()

◆ fuseHeading()

◆ fuseMag()

◆ fuseOptFlow()

◆ fuseSideslip()

◆ fuseVelPosHeight()

◆ get_accel_bias()

◆ get_airspeed_innov()

◆ get_airspeed_innov_var()

◆ get_aux_vel_innov()

◆ get_beta_innov()

◆ get_beta_innov_var()

◆ get_drag_innov()

◆ get_drag_innov_var()

◆ get_ekf_ctrl_limits()

◆ get_ekf_gpos_accuracy()

◆ get_ekf_lpos_accuracy()

◆ get_ekf_origin()

◆ get_ekf_soln_status()

◆ get_ekf_vel_accuracy()

◆ get_ev2ekf_quaternion()