#include <WindEstimator.hpp>

Collaboration diagram for WindEstimator:

Public Member Functions
	WindEstimator ()=default

	~WindEstimator ()=default

	WindEstimator (const WindEstimator &)=delete

WindEstimator &	operator= (const WindEstimator &)=delete

	WindEstimator (WindEstimator &&)=delete

WindEstimator &	operator= (WindEstimator &&)=delete

void	update (uint64_t time_now)

void	fuse_airspeed (uint64_t time_now, float true_airspeed, const matrix::Vector3f &velI, const matrix::Vector2f &velIvar)

void	fuse_beta (uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att)

void	get_wind (float wind[2])

bool	is_estimate_valid ()

bool	check_if_meas_is_rejected (uint64_t time_now, float innov, float innov_var, uint8_t gate_size, uint64_t &time_meas_rejected, bool &reinit_filter)

float	get_tas_scale ()

float	get_tas_innov ()

float	get_tas_innov_var ()

float	get_beta_innov ()

float	get_beta_innov_var ()

void	get_wind_var (float wind_var[2])

bool	get_wind_estimator_reset ()

void	set_wind_p_noise (float wind_sigma)

void	set_tas_scale_p_noise (float tas_scale_sigma)

void	set_tas_noise (float tas_sigma)

void	set_beta_noise (float beta_var)

void	set_tas_gate (uint8_t gate_size)

void	set_beta_gate (uint8_t gate_size)

void	enforce_airspeed_scale (float scale)

Private Types
enum	{ w_n = 0, w_e, tas }

Private Member Functions
bool	initialise (const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas)

void	run_sanity_checks ()

Private Attributes
matrix::Vector3f	_state
	state vector More...

matrix::Matrix3f	_P
	state covariance matrix More...

float	_tas_innov {0.0f}
	true airspeed innovation More...

float	_tas_innov_var {0.0f}
	true airspeed innovation variance More...

float	_beta_innov {0.0f}
	sideslip innovation More...

float	_beta_innov_var {0.0f}
	sideslip innovation variance More...

bool	_initialised {false}
	True: filter has been initialised. More...

float	_wind_p_var {0.1f}
	wind process noise variance More...

float	_tas_scale_p_var {0.0001f}
	true airspeed scale process noise variance More...

float	_tas_var {1.4f}
	true airspeed measurement noise variance More...

float	_beta_var {0.5f}
	sideslip measurement noise variance More...

uint8_t	_tas_gate {3}
	airspeed fusion gate size More...

uint8_t	_beta_gate {1}
	sideslip fusion gate size More...

uint64_t	_time_last_airspeed_fuse = 0
	timestamp of last airspeed fusion More...

uint64_t	_time_last_beta_fuse = 0
	timestamp of last sideslip fusion More...

uint64_t	_time_last_update = 0
	timestamp of last covariance prediction More...

uint64_t	_time_rejected_beta = 0
	timestamp of when sideslip measurements have consistently started to be rejected More...

uint64_t	_time_rejected_tas
	timestamp of when true airspeed measurements have consistently started to be rejected More...

bool	_wind_estimator_reset = false
	wind estimator was reset in this cycle More...

float	_enforced_airspeed_scale {-1.0f}
	by default we want to estimate the true airspeed scale factor (see enforce_airspeed_scale(...) ) More...

Detailed Description

Definition at line 45 of file WindEstimator.hpp.

Member Enumeration Documentation

◆ anonymous enum

anonymous enum

private

Enumerator
w_n
w_e
tas

Definition at line 98 of file WindEstimator.hpp.

Constructor & Destructor Documentation

◆ WindEstimator() [1/3]

WindEstimator::WindEstimator ( )

default

◆ ~WindEstimator()

WindEstimator::~WindEstimator ( )

default

◆ WindEstimator() [2/3]

WindEstimator::WindEstimator ( const WindEstimator & )

delete

◆ WindEstimator() [3/3]

WindEstimator::WindEstimator ( WindEstimator && )

delete

Member Function Documentation

◆ check_if_meas_is_rejected()

bool WindEstimator::check_if_meas_is_rejected	(	uint64_t	time_now,
		float	innov,
		float	innov_var,
		uint8_t	gate_size,
		uint64_t &	time_meas_rejected,
		bool &	reinit_filter
	)

Definition at line 353 of file WindEstimator.cpp.

Referenced by fuse_airspeed(), fuse_beta(), and is_estimate_valid().

Here is the caller graph for this function:

◆ enforce_airspeed_scale()

void WindEstimator::enforce_airspeed_scale ( float scale )

inline

Definition at line 95 of file WindEstimator.hpp.

References _enforced_airspeed_scale.

Referenced by AirspeedValidator::set_airspeed_scale_manual().

Here is the caller graph for this function:

◆ fuse_airspeed()

void WindEstimator::fuse_airspeed	(	uint64_t	time_now,
		float	true_airspeed,
		const matrix::Vector3f &	velI,
		const matrix::Vector2f &	velIvar
	)

Definition at line 140 of file WindEstimator.cpp.

References _initialised, _P, _state, _tas_gate, _tas_innov, _tas_innov_var, _tas_var, _time_last_airspeed_fuse, _time_rejected_tas, check_if_meas_is_rejected(), f(), initialise(), math::max(), run_sanity_checks(), tas, matrix::Matrix< Type, M, N >::transpose(), w_e, and w_n.

Referenced by AirspeedValidator::update_wind_estimator().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fuse_beta()

void WindEstimator::fuse_beta	(	uint64_t	time_now,
		const matrix::Vector3f &	velI,
		const matrix::Quatf &	q_att
	)

Definition at line 216 of file WindEstimator.cpp.

References _beta_gate, _beta_innov, _beta_innov_var, _beta_var, _initialised, _P, _state, _time_last_beta_fuse, _time_rejected_beta, check_if_meas_is_rejected(), f(), initialise(), matrix::Vector< Type, M >::length(), run_sanity_checks(), tas, matrix::Matrix< Type, M, N >::transpose(), w_e, and w_n.

Referenced by AirspeedValidator::update_wind_estimator(), and AirspeedModule::update_wind_estimator_sideslip().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_beta_innov()

float WindEstimator::get_beta_innov ( )

inline

Definition at line 77 of file WindEstimator.hpp.

References _beta_innov.

Referenced by AirspeedValidator::get_wind_estimator_states(), and AirspeedModule::update_wind_estimator_sideslip().

Here is the caller graph for this function:

◆ get_beta_innov_var()

float WindEstimator::get_beta_innov_var ( )

inline

Definition at line 78 of file WindEstimator.hpp.

References _beta_innov_var.

Referenced by AirspeedValidator::get_wind_estimator_states(), and AirspeedModule::update_wind_estimator_sideslip().

Here is the caller graph for this function:

◆ get_tas_innov()

float WindEstimator::get_tas_innov ( )

inline

Definition at line 75 of file WindEstimator.hpp.

References _tas_innov.

Referenced by AirspeedValidator::check_airspeed_innovation(), AirspeedValidator::get_wind_estimator_states(), and AirspeedModule::update_wind_estimator_sideslip().

Here is the caller graph for this function:

◆ get_tas_innov_var()

float WindEstimator::get_tas_innov_var ( )

inline

Definition at line 76 of file WindEstimator.hpp.

References _tas_innov_var.

Referenced by AirspeedValidator::check_airspeed_innovation(), AirspeedValidator::get_wind_estimator_states(), and AirspeedModule::update_wind_estimator_sideslip().

Here is the caller graph for this function:

◆ get_tas_scale()

float WindEstimator::get_tas_scale ( )

inline

Definition at line 74 of file WindEstimator.hpp.

References _state, and tas.

Referenced by AirspeedValidator::get_wind_estimator_states(), AirspeedValidator::update_EAS_scale(), and AirspeedModule::update_wind_estimator_sideslip().

Here is the caller graph for this function:

◆ get_wind()

void WindEstimator::get_wind ( float wind[2] )

inline

Definition at line 63 of file WindEstimator.hpp.

References _state, w_e, and w_n.

Referenced by AirspeedValidator::get_wind_estimator_states(), and AirspeedModule::update_wind_estimator_sideslip().

Here is the caller graph for this function:

◆ get_wind_estimator_reset()

bool WindEstimator::get_wind_estimator_reset ( )

inline

Definition at line 84 of file WindEstimator.hpp.

References _wind_estimator_reset.

Referenced by AirspeedValidator::check_airspeed_innovation().

Here is the caller graph for this function:

◆ get_wind_var()

void WindEstimator::get_wind_var ( float wind_var[2] )

inline

Definition at line 79 of file WindEstimator.hpp.

References _P.

Referenced by AirspeedValidator::get_wind_estimator_states(), and AirspeedModule::update_wind_estimator_sideslip().

Here is the caller graph for this function:

◆ initialise()

bool WindEstimator::initialise	(	const matrix::Vector3f &	velI,
		const matrix::Vector2f &	velIvar,
		const float	tas_meas
	)

private

Definition at line 42 of file WindEstimator.cpp.

References _P, _state, _time_rejected_beta, _time_rejected_tas, _wind_estimator_reset, f(), matrix::Matrix< Type, M, N >::setZero(), tas, matrix::Matrix< Type, M, N >::transpose(), w_e, and w_n.

Referenced by fuse_airspeed(), and fuse_beta().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ is_estimate_valid()

bool WindEstimator::is_estimate_valid ( )

inline

Definition at line 69 of file WindEstimator.hpp.

References _initialised, and check_if_meas_is_rejected().

Referenced by AirspeedValidator::update_EAS_scale().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ operator=() [1/2]

WindEstimator& WindEstimator::operator= ( const WindEstimator & )

delete

◆ operator=() [2/2]

WindEstimator& WindEstimator::operator= ( WindEstimator && )

delete

◆ run_sanity_checks()

void WindEstimator::run_sanity_checks ( )

private

Definition at line 310 of file WindEstimator.cpp.

References _enforced_airspeed_scale, _initialised, _P, _state, f(), ISFINITE, math::max(), tas, w_e, and w_n.

Referenced by fuse_airspeed(), and fuse_beta().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_beta_gate()

void WindEstimator::set_beta_gate ( uint8_t gate_size )

inline

Definition at line 91 of file WindEstimator.hpp.

References _beta_gate.

Referenced by AirspeedModule::update_params().

Here is the caller graph for this function:

◆ set_beta_noise()

void WindEstimator::set_beta_noise ( float beta_var )

inline

Definition at line 89 of file WindEstimator.hpp.

References _beta_var.

Referenced by AirspeedModule::update_params().

Here is the caller graph for this function:

◆ set_tas_gate()

void WindEstimator::set_tas_gate ( uint8_t gate_size )

inline

Definition at line 90 of file WindEstimator.hpp.

References _tas_gate.

Referenced by AirspeedModule::update_params().

Here is the caller graph for this function:

◆ set_tas_noise()

void WindEstimator::set_tas_noise ( float tas_sigma )

inline

Definition at line 88 of file WindEstimator.hpp.

References _tas_var.

Referenced by AirspeedModule::update_params().

Here is the caller graph for this function:

◆ set_tas_scale_p_noise()

void WindEstimator::set_tas_scale_p_noise ( float tas_scale_sigma )

inline

Definition at line 87 of file WindEstimator.hpp.

References _tas_scale_p_var.

Referenced by AirspeedModule::update_params().

Here is the caller graph for this function:

◆ set_wind_p_noise()

void WindEstimator::set_wind_p_noise ( float wind_sigma )

inline

Definition at line 86 of file WindEstimator.hpp.

References _wind_p_var.

Referenced by AirspeedModule::update_params().

Here is the caller graph for this function:

◆ update()

void WindEstimator::update ( uint64_t time_now )

Definition at line 97 of file WindEstimator.cpp.

References _initialised, _P, _tas_scale_p_var, _time_last_update, _wind_estimator_reset, _wind_p_var, dt, and f().

Referenced by AirspeedValidator::update_wind_estimator(), and AirspeedModule::update_wind_estimator_sideslip().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ _beta_gate

uint8_t WindEstimator::_beta_gate {1}

private

sideslip fusion gate size

Definition at line 120 of file WindEstimator.hpp.

Referenced by fuse_beta(), and set_beta_gate().

◆ _beta_innov

float WindEstimator::_beta_innov {0.0f}

private

sideslip innovation

Definition at line 110 of file WindEstimator.hpp.

Referenced by fuse_beta(), and get_beta_innov().

◆ _beta_innov_var

float WindEstimator::_beta_innov_var {0.0f}

private

sideslip innovation variance

Definition at line 111 of file WindEstimator.hpp.

Referenced by fuse_beta(), and get_beta_innov_var().

◆ _beta_var

float WindEstimator::_beta_var {0.5f}

private

sideslip measurement noise variance

Definition at line 118 of file WindEstimator.hpp.

Referenced by fuse_beta(), and set_beta_noise().

◆ _enforced_airspeed_scale

float WindEstimator::_enforced_airspeed_scale {-1.0f}

private

by default we want to estimate the true airspeed scale factor (see enforce_airspeed_scale(...) )

Definition at line 130 of file WindEstimator.hpp.

Referenced by enforce_airspeed_scale(), and run_sanity_checks().

◆ _initialised

bool WindEstimator::_initialised {false}

private

True: filter has been initialised.

Definition at line 113 of file WindEstimator.hpp.

Referenced by fuse_airspeed(), fuse_beta(), is_estimate_valid(), run_sanity_checks(), and update().

◆ _P

matrix::Matrix3f WindEstimator::_P

private

state covariance matrix

Definition at line 105 of file WindEstimator.hpp.

Referenced by fuse_airspeed(), fuse_beta(), get_wind_var(), initialise(), run_sanity_checks(), and update().

◆ _state

matrix::Vector3f WindEstimator::_state

private

state vector

Definition at line 104 of file WindEstimator.hpp.

Referenced by fuse_airspeed(), fuse_beta(), get_tas_scale(), get_wind(), initialise(), and run_sanity_checks().

◆ _tas_gate

uint8_t WindEstimator::_tas_gate {3}

private

airspeed fusion gate size

Definition at line 119 of file WindEstimator.hpp.

Referenced by fuse_airspeed(), and set_tas_gate().

◆ _tas_innov

float WindEstimator::_tas_innov {0.0f}

private

true airspeed innovation

Definition at line 107 of file WindEstimator.hpp.

Referenced by fuse_airspeed(), and get_tas_innov().

◆ _tas_innov_var

float WindEstimator::_tas_innov_var {0.0f}

private

true airspeed innovation variance

Definition at line 108 of file WindEstimator.hpp.

Referenced by fuse_airspeed(), and get_tas_innov_var().

◆ _tas_scale_p_var

float WindEstimator::_tas_scale_p_var {0.0001f}

private

true airspeed scale process noise variance

Definition at line 116 of file WindEstimator.hpp.

Referenced by set_tas_scale_p_noise(), and update().

◆ _tas_var

float WindEstimator::_tas_var {1.4f}

private

true airspeed measurement noise variance

Definition at line 117 of file WindEstimator.hpp.

Referenced by fuse_airspeed(), and set_tas_noise().

◆ _time_last_airspeed_fuse

uint64_t WindEstimator::_time_last_airspeed_fuse = 0

private

timestamp of last airspeed fusion

Definition at line 122 of file WindEstimator.hpp.

Referenced by fuse_airspeed().

◆ _time_last_beta_fuse

uint64_t WindEstimator::_time_last_beta_fuse = 0

private

timestamp of last sideslip fusion

Definition at line 123 of file WindEstimator.hpp.

Referenced by fuse_beta().

◆ _time_last_update

uint64_t WindEstimator::_time_last_update = 0

private

timestamp of last covariance prediction

Definition at line 124 of file WindEstimator.hpp.

Referenced by update().

◆ _time_rejected_beta

uint64_t WindEstimator::_time_rejected_beta = 0

private

timestamp of when sideslip measurements have consistently started to be rejected

Definition at line 125 of file WindEstimator.hpp.

Referenced by fuse_beta(), and initialise().

◆ _time_rejected_tas

uint64_t WindEstimator::_time_rejected_tas

private

Initial value:

=

0

timestamp of when true airspeed measurements have consistently started to be rejected

Definition at line 126 of file WindEstimator.hpp.

Referenced by fuse_airspeed(), and initialise().

◆ _wind_estimator_reset

bool WindEstimator::_wind_estimator_reset = false

private

wind estimator was reset in this cycle

Definition at line 129 of file WindEstimator.hpp.

Referenced by get_wind_estimator_reset(), initialise(), and update().

◆ _wind_p_var

float WindEstimator::_wind_p_var {0.1f}

private

wind process noise variance

Definition at line 115 of file WindEstimator.hpp.

Referenced by set_wind_p_noise(), and update().

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

src/lib/ecl/airdata/WindEstimator.hpp
src/lib/ecl/airdata/WindEstimator.cpp

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Detailed Description

Member Enumeration Documentation

◆ anonymous enum

Constructor & Destructor Documentation

◆ WindEstimator() [1/3]

◆ ~WindEstimator()

◆ WindEstimator() [2/3]

◆ WindEstimator() [3/3]

Member Function Documentation

◆ check_if_meas_is_rejected()

◆ enforce_airspeed_scale()

◆ fuse_airspeed()

◆ fuse_beta()

◆ get_beta_innov()

◆ get_beta_innov_var()

◆ get_tas_innov()

◆ get_tas_innov_var()

◆ get_tas_scale()

◆ get_wind()

◆ get_wind_estimator_reset()

◆ get_wind_var()

◆ initialise()

◆ is_estimate_valid()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ run_sanity_checks()

◆ set_beta_gate()

◆ set_beta_noise()

◆ set_tas_gate()

◆ set_tas_noise()

◆ set_tas_scale_p_noise()

◆ set_wind_p_noise()

◆ update()

Member Data Documentation

◆ _beta_gate

◆ _beta_innov

◆ _beta_innov_var

◆ _beta_var

◆ _enforced_airspeed_scale

◆ _initialised

◆ _P

◆ _state

◆ _tas_gate

◆ _tas_innov

◆ _tas_innov_var

◆ _tas_scale_p_var

◆ _tas_var

◆ _time_last_airspeed_fuse

◆ _time_last_beta_fuse

◆ _time_last_update

◆ _time_rejected_beta

◆ _time_rejected_tas

◆ _wind_estimator_reset

◆ _wind_p_var