fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 01:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity
41,673 Commits 852 Branches 161 Tags
41abf695a85dfbce354346a55b0dbe80336faef1
Commit Graph

888 Commits

Author SHA1 Message Date
bresch 41abf695a8 ekf2: use more efficient covariance update computation 
KSK' is faster to compute than KHP and is mathematically equivalent
 2022-12-12 12:31:54 -05:00
Daniel Agar 8cf13d50a8 ekf2: EKFGSF_yaw minor cleanup (#20510) 
- don't store unnecessary IMU copy in class, pass it through where required
 - remove custom pi constants
 - remove unused fields from ahrs_ekf_gsf (vel_NE, fuse_gps, accel_dt)
 - explicitly initialize everything in header
 - apply PX4 code style
 - set GPS velocity before yaw estimator update, not after
 2022-12-09 13:24:00 -05:00
Daniel Agar f3884d5835 Update world_magnetic_model to latest Fri Dec 9 11:14:09 UTC 2022 
Co-authored-by: PX4 BuildBot <bot@px4.io>
 2022-12-09 11:40:30 -05:00
Daniel Agar 8eb2a0a3ec ekf2: delete orientation_covariances_euler() 
- usage replaced with now public calcRotVecVariances()
 2022-12-07 12:41:27 -05:00
Mathieu Bresciani a187bb3b80 ekf2: predict covariance before predicting state (#20715) 
* To predict the covariance, the jacobian from the previous to the current state is required.
 2022-12-07 09:34:19 -05:00
bresch 5155346d60 ekf2: do not use gnss data when no lock 2022-11-30 08:50:34 -05:00
bresch 5d7faefa84 ekf2: don't add invalid GNSS samples to the buffer 2022-11-30 00:22:15 -05:00
bresch 60c448ce3a ekf2: GNSS yaw, use reported yaw accuracy when available 2022-11-22 09:59:31 -05:00
bresch b25bc1b982 ekf2: GNSS yaw, update->fuse pattern 2022-11-22 09:59:31 -05:00
bresch 4cb327fba3 ekf2: remove old GPS yaw autogenerated code 2022-11-22 09:59:31 -05:00
bresch 16a3fee54f ekf2_test: check behavior of GNSS yaw fusion at singularities 2022-11-22 09:59:31 -05:00
bresch 4116de31ad ekf2: compare GNSS yaw fusion sympy vs symforce 2022-11-22 09:59:31 -05:00
bresch eff2c6adcf ekf2: migrate GNSS yaw fusion to SymForce 2022-11-22 09:59:31 -05:00
Daniel Agar da82757bf6 ekf2: accumulate multiple vel/pos/orientation reset deltas per update 
- if in a single EKF update there are multiple resets we need to track the accumulated delta so the change consumed by the controllers is correct
 2022-11-15 13:20:54 -05:00
bresch 22420a7bf1 ekf2: do not fuse ZVU if other velocity source is active 2022-11-15 11:17:07 -05:00
bresch c67f03f383 ekf2: update change indicator 2022-11-14 11:51:32 -05:00
bresch f319cc528b ekf2: remove old flow fusion generated code 2022-11-14 11:51:32 -05:00
bresch 2a83dbf81d ekf2_test: compare flow fusion sympy vs symforce 2022-11-14 11:51:32 -05:00
bresch 93564baccf ekf2_flow: recalculate innovation after fusing 1st axis 
The state changed so we need to recalculate the innovation of the 2nd
axis after fusing the 1st one
 2022-11-14 11:51:32 -05:00
bresch 4dbdf23346 ekf2: update _R_to_earth when the state quat changed through fusion 2022-11-14 11:51:32 -05:00
bresch 28458340e6 ekf2_flow: check innov variance health after fusing 1st axis 2022-11-14 11:51:32 -05:00
bresch 73a8c388e8 ekf2: fuse by looping through axes 2022-11-14 11:51:32 -05:00
bresch b54a4417fa ekf2: migrate flow fusion to SymForce 2022-11-14 11:51:32 -05:00
Daniel Agar 639d1ddca2 ekf2: update flags (in_air, at_rest, etc) on delayed time horizon (#20576) 
- in a lot of cases this won't really matter, although you can see it in logs (especially with things like vehicle_at_rest), but it's something we might as well do properly

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
 2022-11-14 11:35:40 -05:00
bresch 7d1f1d0f84 ekf2: replace macro constants by typed constexpr 2022-11-14 10:45:23 -05:00
bresch 06702da003 ekf2: add GNSS yaw max interval 
yaw data usually comes at lower rate than vel/pos
 2022-11-14 10:45:23 -05:00
bresch 9834c7917b ekf2: use yaw emergency estimator more aggressively 
During the whole flight, if the difference between the yaw estimate from
EKF2 and the emergency estimator is large and that the GNSS velocity
fusion is failing continuously, immediately reset to the emergency yaw
estimate.
 2022-11-14 10:45:23 -05:00
Daniel Agar bd5bc9d207 ekf2: cleanup terrain estimator init (shouldn't be valid by default) 
- with Ekf::initialiseFilter() resetting _time_last_hagl_fuse the terrain estimate was technically valid regardless of any range or flow data availability and as a result optical flow fusion is able to start and stay active
 - only consider terrain estimate valid based on control status flags and recent fusion
 2022-11-14 09:29:46 -05:00
Daniel Agar 84d1435880 ekf2: ensure minimum output buffer sizing 
- buffer at least 2 samples for the IMU output predictor buffers
 - dropping below 2 becomes problematic for the minimum observation
interval calculation and the vertical output buffer trapezoidal
integration
 2022-11-09 09:49:35 -05:00
Daniel Agar 5239993c88 ekf2: move EV vel to new state machine, introduce EKF2_EV_CTRL param 2022-11-08 11:46:41 -05:00
Daniel Agar 688dae1108 ekf2: add new EKF2_EV_QMIN parameter 2022-11-08 11:46:41 -05:00
Daniel Agar 298cc61e07 ekf2: push fuse beta config into backend 2022-11-02 10:09:26 -04:00
Daniel Agar a242a0210e Update world_magnetic_model to latest Mon 24 Oct 2022 09:29:11 PM EDT 2022-10-25 09:20:01 -04:00
Daniel Agar 6f861ba889 ekf2: pos/vel reset helpers pass new variance 
- optical flow velocity reset use already computed _flow_vel_ne
 2022-10-24 10:59:58 -04:00
Daniel Agar ed558e199f ekf2: remove realignYawGPS() (replaced with yaw estimator) 2022-10-21 09:01:30 -04:00
bresch 96e7ea7a08 ekf2: remove old mag declination auto-code 2022-10-20 18:16:25 -04:00
bresch f0a0a3e545 ekf2_test: compare mag decl fusion sympy vs symforce 2022-10-20 18:16:25 -04:00
bresch 2f3ea88099 ekf2: migrate mag declination to SymForce 2022-10-20 18:16:25 -04:00
Daniel Agar 5030b21d2e ekf2: replace quatToInverseRotMat if only used once 2022-10-20 14:15:32 -04:00
Daniel Agar fb3adc3faa ekf2: move baro compensation to delayed time horizon and add validity check 
- this removes an unnecessary virtual call and simplifies things a bit

Co-authored-by: bresch <brescianimathieu@gmail.com>
 2022-10-20 09:17:55 -04:00
Daniel Agar cea185268e msg ROS2 compatibility, microdds_client improvements (timesync, reduced code size, added topics, etc), fastrtps purge 
- update all msgs to be directly compatible with ROS2
 - microdds_client improvements
   - timesync
   - reduced code size
   - add to most default builds if we can afford it
   - lots of other little changes
 - purge fastrtps (I tried to save this multiple times, but kept hitting roadblocks)
 2022-10-19 19:36:47 -04:00
Daniel Agar f9509b442c ekf2: height aid source consistency (#20405) 
- working towards keeping all height source (baro/ev/gnss/rng) handling as consistent as possible, possibly refactoring these out into separate classes later
 2022-10-19 15:35:07 -04:00
Matthias Grob 5ca28dd6dc Use isAllFinite() in all places that check finiteness on entire vectors or matrices 2022-10-19 19:09:20 +02:00
bresch f77d7d9413 [AUTO COMMIT] update change indication 2022-10-19 17:34:31 +02:00
bresch cf9859965a ekf2: remove old yaw fusion auto-code 2022-10-19 17:34:31 +02:00
bresch e33215b61c ekf2_test: compare yaw fusion sympy vs symforce 2022-10-19 17:34:31 +02:00
bresch 42f683fa64 ekf2_test: test using positive definite covariance matrix 
A covariance matrix needs to be positive definite
 2022-10-19 17:34:31 +02:00
bresch 10f9ac148f ekf2: migrate mag yaw fusion to SymForce 2022-10-19 17:34:31 +02:00
bresch a41b6f416e ekf2_test: verify heading innov variance in all orientations 2022-10-19 17:34:31 +02:00
bresch f11908a266 ekf2: generate yaw fusion jacobians using symforce 2022-10-19 17:34:31 +02:00