45 #include <mathlib/mathlib.h> 119 bool updated =
false;
233 if (hgt_count_fail || mag_count_fail) {
264 Eulerf euler_init(roll, pitch, 0.0
f);
336 dq.from_axis_angle(corrected_delta_ang);
397 Dcmf delta_R(delta_q.inversed());
481 dq.from_axis_angle(delta_angle);
554 if (q_error(0) >= 0.0
f) {
561 const Vector3f delta_ang_error{scalar * q_error(1), scalar * q_error(2), scalar * q_error(3)};
566 const float att_gain = 0.5f *
_dt_imu_avg / time_delay;
570 _delta_angle_corr = delta_ang_error * att_gain;
607 const float vel_d_correction = pos_d_err * pos_gain + vel_d_err * pos_gain * 1.1f;
622 const uint8_t index_next = (index + 1) % size;
628 current_state.
vel_d += vel_d_correction;
631 next_state.
vel_d += vel_d_correction;
637 index = (index + 1) % size;
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Vector3f _vel_err_integ
integral of velocity tracking error (m)
float _cos_tilt_rng
cosine of the range finder tilt rotation about the Y body axis
struct estimator::filter_control_status_u::@60 flags
void setControlBaroHeight()
bool pop_first_older_than(const uint64_t ×tamp, data_type *sample)
Vector3f delta_ang
delta angle in body frame (integrated gyro measurements) (rad)
void calculateOutputStates()
Adapter / shim layer for system calls needed by ECL.
stateSample _state
state struct of the ekf running at the delayed time horizon
uint32_t yaw_align
1 - true if the filter yaw alignment is complete
float mag_heading_noise
measurement noise used for simple heading fusion (rad)
bool initialiseFilter(void)
RingBuffer< outputSample > _output_buffer
Quatf calculate_quaternion() const
float _yaw_rate_lpf_ef
Filtered angular rate about earth frame D axis (rad/sec)
bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var=true, bool update_buffer=true)
#define MAG_FUSE_TYPE_3D
Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised...
imuSample _imu_sample_new
RingBuffer< extVisionSample > _ext_vision_buffer
const data_type & get_newest()
Vector3f accel_bias
delta velocity bias estimate in m/s
float _gps_alt_ref
WGS-84 height (m)
float _yaw_delta_ef
Recent change in yaw angle measured about the earth frame D axis (rad)
uint64_t time_us
timestamp of the measurement (uSec)
Vector2f _accel_lpf_NE
Low pass filtered horizontal earth frame acceleration (m/sec**2)
float vel_Tau
velocity state correction time constant (1/sec)
uint32_t rng_hgt
10 - true when range finder height is being fused as a primary height reference
void initialiseCovariance()
Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2)
Vector3f mag_I
NED earth magnetic field in gauss.
uint32_t baro_hgt
9 - true when baro height is being fused as a primary height reference
RingBuffer< outputVert > _output_vert_buffer
float pos_Tau
position state correction time constant (1/sec)
Vector3f _vel_imu_rel_body_ned
int32_t vdist_sensor_type
selects the primary source for height data
imuSample _imu_down_sampled
down sampled imu data (sensor rate -> filter update rate)
static constexpr float CONSTANTS_ONE_G
float vel_d
D velocity calculated using alternative algorithm (m/sec)
const T & getState() const
bool collect_imu(const imuSample &imu) override
bool _NED_origin_initialised
uint64_t _time_last_of_fuse
time the last fusion of optical flow measurements were performed (uSec)
uint64_t _time_last_hgt_fuse
time the last fusion of height measurements was performed (uSec)
float _imu_collection_time_adj
the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PE...
unsigned _min_obs_interval_us
void controlFusionModes()
AlphaFilterVector3f _mag_lpf
filtered magnetometer measurement (Gauss)
Vector3f _prev_dvel_bias_var
saved delta velocity XYZ bias variances (m/sec)**2
Dcmf _R_to_earth
transformation matrix from body frame to earth frame from last EKF prediction
void calcEarthRateNED(Vector3f &omega, float lat_rad) const
uint64_t time_us
timestamp of the measurement (uSec)
outputVert _output_vert_delayed
uint64_t _time_last_pos_fuse
time the last fusion of horizontal position measurements was performed (uSec)
int _primary_hgt_source
specifies primary source of height data
baroSample _baro_sample_delayed
const data_type & get_oldest()
bool _terrain_initialised
true when the terrain estimator has been initialized
uint64_t time_us
timestamp of the measurement (uSec)
Vector3f pos
NED position estimate in earth frame (m/sec)
float _output_tracking_error[3]
contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
#define VDIST_SENSOR_EV
Use external vision.
void reset(uint64_t timestamp) override
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
uint32_t _mag_counter
number of magnetometer samples read during initialisation
float rng_gnd_clearance
minimum valid value for range when on ground (m)
void update(const T &input, float tau, float dt)
uint32_t _ev_counter
number of external vision samples read during initialisation
filter_control_status_u _control_status
RingBuffer< baroSample > _baro_buffer
Vector3f _earth_rate_NED
earth rotation vector (NED) in rad/s
Vector3f vel
NED velocity in earth frame in m/s.
uint32_t ev_hgt
14 - true when height data from external vision measurements is being fused
float _gps_drift_velD
GPS down position derivative (m/sec)
int32_t mag_fusion_type
integer used to specify the type of magnetometer fusion used
static constexpr float sq(float var)
RingBuffer< magSample > _mag_buffer
uint32_t ev_yaw
13 - true when yaw data from external vision measurements is being fused
fault_status_u _fault_status
float hgt
pressure altitude above sea level (m)
void increaseQuatYawErrVariance(float yaw_variance)
imuSample _imu_sample_delayed
Quatf quat_nominal
quaternion defining the rotation from body to earth frame
filter_control_status_u _control_status_prev
Vector3f pos
NED position in earth frame in m.
void runTerrainEstimator()
innovation_fault_status_u _innov_check_fail_status
float rng_sens_pitch
Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero...
Vector3< float > Vector3f
extVisionSample _ev_sample_delayed
float _accel_mag_filt
acceleration magnitude after application of a decaying envelope filter (rad/sec)
float _R_rng_to_earth_2_2
2,2 element of the rotation matrix from sensor frame to earth frame
bool _filter_initialised
true when the EKF sttes and covariances been initialised
Vector3f mag_B
magnetometer bias estimate in body frame in gauss
constexpr _Tp max(_Tp a, _Tp b)
float _sin_tilt_rng
sine of the range finder tilt rotation about the Y body axis
uint64_t time_us
timestamp of the measurement (uSec)
Quaternion< float > Quatf
uint64_t time_us
timestamp of the measurement (uSec)
Vector3f vel
NED velocity estimate in earth frame (m/sec)
float vel_d_integ
Integral of vel_d (m)
RingBuffer< imuSample > _imu_buffer
uint64_t _time_last_delpos_fuse
time the last fusion of incremental horizontal position measurements was performed (uSec) ...
Vector3f _delVel_sum
summed delta velocity (m/sec)
Vector3f imu_pos_body
xyz position of IMU in body frame (m)
AxisAngle< float > AxisAnglef
uint8_t get_oldest_index() const
uint32_t gps_hgt
11 - true when GPS height is being fused as a primary height reference
float angErr
1-Sigma angular error (rad)
outputVert _output_vert_new
uint64_t _time_last_vel_fuse
time the last fusion of velocity measurements was performed (uSec)
float _ang_rate_mag_filt
angular rate magnitude after application of a decaying envelope filter (rad/sec)
float _baro_hgt_offset
baro height reading at the local NED origin (m)
uint32_t _hgt_counter
number of height samples read during initialisation
bool init(uint64_t timestamp) override
static constexpr float FILTER_UPDATE_PERIOD_S
Quatf _q_down_sampled
down sampled quaternion (tracking delta angles between ekf update steps)
Vector2f wind_vel
wind velocity in m/s
bool _earth_rate_initialised
true when we know the earth rotatin rate (requires GPS)
Vector3f mag
NED magnetometer body frame measurements (Gauss)
Vector3f _delta_angle_corr
delta angle correction vector (rad)
Vector3f delta_vel
delta velocity in body frame (integrated accelerometer measurements) (m/sec)
bool initialise_interface(uint64_t timestamp)
uint64_t _time_last_hagl_fuse
last system time that the hagl measurement failed it's checks (uSec)
Vector3f gyro_bias
delta angle bias estimate in rad
Quatf quat_nominal
nominal quaternion describing vehicle attitude
float _dt_ekf_avg
average update rate of the ekf
float delta_ang_dt
delta angle integration period (sec)
Class for core functions for ekf attitude and position estimator.
Vector3f _pos_err_integ
integral of position tracking error (m.s)
#define ECL_INFO_TIMESTAMPED
outputSample _output_sample_delayed
float delta_vel_dt
delta velocity integration period (sec)
uint8_t get_length() const
float _rng_filt_state
filtered height measurement (m)
uint8_t _obs_buffer_length
void resetStates() override
void resetStatesAndCovariances() override
magSample _mag_sample_delayed
void push(const data_type &sample)