Commit Graph
100 Commits
Author SHA1 Message Date
bresch b30c2c22b3 ekf2: do not run rng kinematic consistency check for fixed-wings
As they are always moving horizontally, the check doesn't make sense
for fixed-wings.
Also don't run the check while on ground to prevent getting a failed
check during pre-takeoff manipulation.
2022-07-12 13:54:30 -04:00
bresch 8e789bcc7f ekf2_post-processing: use estimator_status_flags instead of bitmasks 2022-05-24 10:30:52 -04:00
bresch 1f399e5b33 ekf2: use explicit flags instead of bitmask position
This prevents bitmask mismatch when a new flag is inserted
2022-05-23 14:57:17 -04:00
bresch f8ff34f82d ekf2: optimize KHP computation
Calculating K(HP) takes less operations than (KH)P because K and H are
vectors.

Without considering the sparsity optimization:
- KH (24*24 operations) is then a 24x24 matrix an it takes
24^3 operations to multiply it with P. Total: 14400 op

- HP (24*(24+24-1) operations) is a row vector
and it takes 24 operations to left-multiply it by K. Total:1152 op
2022-05-06 10:15:26 -04:00
bresch db0274e19b ekf: robustify bad_acc_vertical check
when the vertical pos or vel innov ratio is above the threshold, the
other one needs to be significant too and not just positive to trigger
the failure
2022-04-25 12:58:18 -04:00
bresch e9c07fac6f EKF: move python tuning tools to EKF module 2022-04-21 13:39:26 +02:00
bresch 49bc5082e7 Tools: add baro pressure coefficient tuning script 2022-04-21 13:39:26 +02:00
bresch ea7009546b mc wind: rename MC wind estimor tuning script 2022-04-21 13:39:26 +02:00
bresch 3c6b72c33b commander: improve set_in_air_position
- set local home using global pos and global home
- set local home using GNSS pos and global home
- set global home using global ref of local frame and local home
2022-04-19 15:29:25 +02:00
bresch e9a2b3f260 geofence: remove unused function parameters 2022-04-19 15:29:25 +02:00
bresch 3f3a4ac5c1 navigator: home_positionvalid -> global_home_position_valid
This is to make clear that the relevant part of the home position
message for navigator is the global one. Local home position isn't
required as everything is done in global coordinates.
The specific home_alt_valid is used when lat/lon are not used
2022-04-19 15:29:25 +02:00
bresch d2f2ba59a4 commander: refactor home position setter
- always try to set local or global home position when possible
- set global home with GNSS position if global pos from EKF isn't
  available
- reset home when significantly moved from home before takeoff (checking
  lpos or gpos or GNSS)
- reset home on takeoff transition
- reset home on mavlink arm command
- remove "home required accuracy" parameters, rely on validity flags
2022-04-19 15:29:25 +02:00
bresch 37cafe7dcd ekf rng kin: reduce minimum rng variance 2022-04-13 11:41:48 +02:00
bresch 4994649500 ekf rng kin: increase default gate size
The user needs to tune the range finder noise parameters properly and we
shouldn't need such a small gate
2022-04-13 11:41:48 +02:00
bresch 1fbe04986f ekf rng finder consistency: simplify class member names 2022-04-13 11:41:48 +02:00
bresch 4c03f0bc50 ekf: make range finder kin consistency gate tunable by parameter 2022-04-13 11:41:48 +02:00
bresch 079a5e92ba ekf: run rng consistency check only when not horizontally moving
The check assumes a non-moving terrain height
2022-04-13 11:41:48 +02:00
bresch d903613c9c ekf: add logging for rng kinematic consistency check 2022-04-13 11:41:48 +02:00
bresch 8693ad15a7 ekf: add logging of range finder consistency check 2022-04-13 11:41:48 +02:00
bresch baf9cc9597 ekf: use uint64_t for time variables 2022-04-13 11:41:48 +02:00
bresch 9fc331b7ea ekf: requires kinematically consistent range finder data to continue terrain aiding 2022-04-13 11:41:48 +02:00
bresch 78211f9dbb ekf: improve rng consistency check
To pass from invalid to valid:
- time hysteresis
- some vertical velocity
- test ratio < 1
- low-passed signed test ratio < 1

To pass from valid to invalid:
- low-passed signed test ratio >= 1
2022-04-13 11:41:48 +02:00
bresch b1ea2e4e15 ekf: use same gate for innov and innov sequence monitoring 2022-04-13 11:41:48 +02:00
bresch f96287b80a ekf: access member variable without getter 2022-04-13 11:41:48 +02:00
bresch 904bf8ef9f ekf: add range finder kinematic consistency check
At each new valid range measurement, the time derivative of the distance
to the ground is computed and compared with the estimated velocity.
The average of a normalized innovation squared statistic check is used
to detect a bias in the derivative of distance measurement,
indicating that the distance measurements are kinematically inconsistent
with the filter.
2022-04-13 11:41:48 +02:00
bresch 064518f57a ekf: extract range finder noise computation 2022-04-13 11:41:48 +02:00
bresch 76a59d5c66 Tools: add drag fusion tuning script 2022-04-11 09:56:01 -04:00
bresch f3f09c1344 PreFlightCheck: add data timeout detection for mag, baro accel and gyro 2022-03-16 11:46:01 +01:00
bresch 75aa11c955 PreFlightCheck: refactor sensors preflight checks 2022-03-16 11:46:01 +01:00
bresch b1c1163ee4 PreFlightCheck: remove unused device_id argument 2022-03-16 11:46:01 +01:00
bresch 5f1b577b6d PreFlightCheck: mark sensor required if used by an EKF2 instance 2022-03-16 11:46:01 +01:00
bresch 58bd3d0c60 cmake: use elif -> elseif 2022-03-11 11:59:20 +01:00
bresch 182980526f commander: allow rearming grace period for arming switch only 2022-03-09 08:42:21 +01:00
bresch 4f0a959244 gps_hgt: add and improve unit tests 2022-02-22 11:49:25 +01:00
bresch edabfd2f0e ekf_hgt: call specific height reset function instead of generic one
- Also use the delayed current data instead of newest that might not be
available (gps buffer is sometimes empty if the dt between samples is
larger than the delayed horizon).

- Separate "baro fault" from "baro intermittent": intermittent is a
  temporary failure and should prevent from switching to baro right now,
  but "fault" means that it should never be used anymore

- In case of height timeout, check for metrics but not for consistency
  as the filter is likely to have diverged already.
2022-02-22 11:49:25 +01:00
bresch cba73585e1 use recorded last sensor timestamp for intermittent check 2022-02-22 11:49:25 +01:00
bresch 493e35b72e ekf_terr: reset rng fault when on ground 2022-02-11 10:57:45 -05:00
bresch 502ec7ef46 ekf_terr: fix unit test
the flags were valid ony because they were based on timeouts and when
starting the unit test, it takes a couple of seconds to actually timeout
2022-02-11 10:57:45 -05:00
bresch 2fb7b35a8b ekf2_terr: refactor terrain estimator - flow aiding 2022-02-11 10:57:45 -05:00
bresch 33fd1849e0 ekf2_terr: refactor terrain estimator - rng aiding 2022-02-11 10:57:45 -05:00
bresch 5818974f0f ekf: add range finder "faulty" status
When delclared faulty, the range finder cannot be used anymore
2022-02-11 10:57:45 -05:00
bresch a3b2550f07 mc_auto: only check for offtrack, infront and behind in XY-plane
This fixes the issue when changing the altitude during a goto for
example, where the vehicle was going backwards and upwards to reach the
closest point to the line. Now the vehicle simply goes towards the
target waypoint.
2022-02-11 16:14:48 +01:00
bresch 917910f3e2 ekf: pass a float by copy instead of a constant ref 2022-02-02 17:43:25 -05:00
bresch 3077f27821 ekf: use float instead of Vector2f for innov gate in pos/vel fusion
We always use the same innovation gate for X and Y anyway
2022-02-02 17:43:25 -05:00
bresch d20e46567e ekf2: improve flow for terrain unit test 2022-01-25 15:07:48 +01:00
bresch 1502d77df2 ekf: refactor fake position control logic 2022-01-25 15:07:48 +01:00
bresch a2260e53da ekf2: replace yaw failure detection criteria
The existing logic using the angle between velocity vectors failed to
determine a yaw failure in practice because the state velocity is often
too small compared to its uncertainty to be used. In all the failures
reported, the yaw emergency estimator converged properly and yaw reset
would have fixed the issue.
A much simpler check using the yaw difference between the main EKF
and the emergency estimator is now used to tell if the vel/pos update
failure is most likely caused by a wrong heading.
2022-01-20 11:54:24 +01:00
bresch e89e3c1b0c ekf2: extract logic to test yaw emergency estimate quality 2022-01-19 09:05:41 -05:00
bresch ea80c5027e ekf2: split yaw estimator state getter into several functions 2022-01-19 09:05:41 -05:00
bresch ffebd8e771 ekf2: initialize cos tilt variable assuming no tilt 2022-01-19 09:04:53 -05:00
bresch 953c90d3a6 failure detector: change imbalanced propeller metric
Use standard deviations instead of variances to get a linear metric
2022-01-18 12:24:35 -05:00
bresch 408cf011b2 failure detector: fix dt computation 2022-01-18 12:24:35 -05:00
bresch 4fc7348582 mpc: leave room for altitude error control during descent and descent
Since the same parameter is used to generate the trajectory and to
saturate the controller, there is no additional space for the output of
the position controller once it is filled with the feedforward, letting
the altitude grow uncontrolled.
2022-01-12 13:33:48 +01:00
bresch ea7d2334c9 mpc: add parameter for ascent/descent speed in auto modes 2022-01-12 13:33:48 +01:00
bresch 57fa9c545a PosControl: fix hover update equation
The integrator now absorbs properly the change in hover thrust
2022-01-08 15:42:10 +01:00
bresch d094fbbd70 ekf2: fix delta angle coning metric 2022-01-04 09:28:48 -05:00
bresch 67a893ac6d WelfordMean: remove unnecessary case for first sample
setting the mean to the sample value and then adding 0 is the same as
setting the mean to zero and then add the sample value divided by 1
2021-12-13 11:42:48 -05:00
bresch 5468841aa5 WelfordMean: add unit test 2021-12-13 11:42:48 -05:00
bresch 66290de745 ekf: improve range primary height source
allow range fusion to start with fake measurements if the sensor is
providing bad measurements because too close to the ground
2021-12-10 15:18:10 +01:00
bresch 9147d5ea5c ekf2: use ground as ekf altitude datum when in range primary hgt mode 2021-12-10 15:18:10 +01:00
bresch 5e790da634 ekf2: reset the timeout timer in the reset function directly 2021-12-10 15:18:10 +01:00
bresch 0d874cf00a ekf2: move aiding start details in dedicated function
When an aiding source needs to be started, simply call the corresponding
starting function and any required reset is handled in there.
2021-12-10 15:18:10 +01:00
bresch c947562183 ekf: get rid of intermediate variable "fuse_height" 2021-12-07 09:16:21 +01:00
bresch 9651f78b0a ekf: centralize the height fusion decision 2021-12-07 09:16:21 +01:00
bresch d583ef6b79 ekf2: move height fusion code to separate source file 2021-12-07 09:16:21 +01:00
bresch fc155bc57e Ekf2Selector: declare filter unhealthy when high test ratio
Request a switch to another instance when the combined test ratio is too high.
2021-11-18 13:29:01 +01:00
bresch 43fb076828 AngularAcceleration: explicitly disable filter when cutoff <= 0 2021-11-18 09:25:47 +01:00
bresch 4a14a8bc7f AutoLineSV: Always publish a valid yaw setpoint
In MPC_YAW_MODE 3 (along trajectory), the flight task is asked to
produce a yaw setpoint. If the previous yaw value was NAN (e.g.: during
early takeoff), the setpoint should then be reset to the current
estimate.
2021-11-16 18:09:37 -05:00
bresch f751dd2242 FlightTask: set yaw_setpoint to NAN when yaw should not be controlled 2021-11-16 18:09:37 -05:00
bresch 2213343240 ft altitude: use longerThan to save a sqrt operation 2021-11-16 18:09:37 -05:00
bresch 394fa953b8 EKF: get status on yaw alignement in flight
1s after the in air mag reset is performed, the final yaw alignment is
complete and the yaw estimate can be used for yaw control.
2021-11-16 18:09:37 -05:00
bresch d47f9f155a MC mixer: replace multirotor_motor_limits by control_allocator_status
CA: fix saturation computation
Since the CA matrix is normalized, the same scale applied to be used when using the effectiveness matrix

MCRateControl: use control_allocator_status to get saturation info
2021-11-09 10:35:10 -05:00
bresch 1e94512719 FD: use flags union instead of bitmask 2021-11-05 09:45:52 -04:00
bresch 3f1025fb1e FD: add dedicated topic to log more internal states
log imbalanced propeller check metric

add failure_detector_status message
2021-11-05 09:45:52 -04:00
bresch b8ed457371 Commander: trigger failsafe action if imbalanced propeller detected 2021-11-05 09:45:52 -04:00
bresch 5dfb8e594a FD: add imbalanced propeller check 2021-11-05 09:45:52 -04:00
bresch bf2fb70d67 vehicleIMU: compute and log accel variance 2021-11-05 09:45:52 -04:00
bresch 9e54c6d1aa ekf2: move generic functions to control.cpp
these functions aren't specific to GPS fusion
2021-11-01 13:59:34 +01:00
bresch e90734881b ekf2_test: add more GPS fusion control tests 2021-11-01 13:59:34 +01:00
bresch 8aae39e82a ekf2: move GPS control logic to separate source file 2021-11-01 13:59:34 +01:00
bresch 689ab12845 ekf2: refactor gps vel/pos fusion control logic 2021-11-01 13:59:34 +01:00
bresch 9afc390552 ekf2: move gps yaw reset in starting function 2021-11-01 13:59:34 +01:00
bresch d0f89f7fff ekf2: refactor wind reset functions 2021-10-26 10:18:56 +02:00
bresch 456dfcb4b9 ekf2: update getter for true airspeed 2021-10-26 10:18:56 +02:00
bresch 3927c183de ekf2_test: adjust airspeed unit test
an airpseed of > 2m/s is required to start the fusion (set by param)
fw mode is also required

Given the larger estimated windspeed after those changes, the change of
static pressure is larger and the height estimate takes more time to
reach the final value
2021-10-26 10:18:56 +02:00
bresch 6e8f0e92ff ekf2: refactor airspeed fusion control logic 2021-10-26 10:18:56 +02:00
bresch 8873e92c7c ekf: force fallback to baro if GPS is stopped while in GPS height mode
Otherwise, no height aiding source is used
2021-10-26 10:05:28 +02:00
bresch 0a140ec59a ekf2_test: add GPS height to baro fallback 2021-10-26 10:05:28 +02:00
bresch f4c21cedd9 ekf2_test: use motion_planning for dynamic yaw emergency test 2021-10-25 18:06:38 -04:00
bresch 340a2caa8e ekf2_test: use motion_planning library
The VelocitySmoothing class from the motion_planning library is used to
generate trajectories in order to test the EKF convergence during motion
2021-10-25 18:06:38 -04:00
bresch 6ec9ab11f2 add fw auto-tune module to board configs 2021-10-25 13:41:31 +02:00
bresch 95e2941b17 fw att: inject system identification signal to controller 2021-10-25 13:41:31 +02:00
bresch 6af0856558 add FF to FW rate controllers 2021-10-25 13:41:31 +02:00
bresch 55f0860c31 fw atune: add fixed-wing auto-tuning module 2021-10-25 13:41:31 +02:00
bresch 8dfdb1e3db compute and publish fixed-wing control power 2021-10-25 13:41:31 +02:00
bresch 78fe6e2152 VelocitySmoothing: fix issue when delta vel is the same on all axes
When the change in velocity is exactly the same on several axes, the T1
of the recumputed trajectories after time sync was set to 0 because it
was skipping both if and else if.
2021-10-22 10:54:36 +02:00
Mathieu Bresciani 56b0c46444 ekf2: improve optical flow angular rate compensation 2021-10-12 13:17:29 -04:00
bresch ba1b7f3a07 CA pseudo inverse: set all small elements in CA matrix to zero
This avoids problems in the sequencial desaturation method where vectors
of the CA matrix are used to desaturate the motors.
2021-10-08 09:35:40 +02:00
bresch d1a2d6e1aa h480_ctrlalloc: tune rate controller 2021-10-08 09:35:40 +02:00
bresch 984a698760 matrix: update to include min/max of slices 2021-10-08 09:35:40 +02:00