fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 04:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity
1,782 Commits 852 Branches 161 Tags
a4fe2b1e7206c97f1498de2ac875eb671cc45a38
Commit Graph
332 Commits
Author SHA1 Message Date
bresch 6248646bff EV yaw: move EV yaw reset out of resetMagHeading function 
Also remove the unnecessary 2nd "manual reset" done in the start function
 2021-05-12 08:33:07 +10:00
bresch 48722a2a92 control: inhibit EV yaw from restarting when GPS aid wants to start 2021-05-12 08:33:07 +10:00
bresch 51197a5d82 GNSS yaw selection logic refactor 2021-05-12 08:33:07 +10:00
Morten Fyhn Amundsen bb950a1550 Expose terrain vpos reset counter 2021-03-18 14:25:07 +01:00
Daniel Agar 63f64b57c1 EKF: set global origin method 
EKF: add convergence after global origin reset test
 2021-02-19 08:59:47 -05:00
bresch 04844a04ed ekf control: move time_last_in_air/on_ground out of GPS control logic 2021-02-18 10:11:51 +01:00
bresch b00d4a9237 mag fusion: improve on ground forced mag fusion logic 
While a yaw test ration > 1 rejects the mag data in air, the fusion is
forced with a limited innovation on ground to fix gyro bias and local
magnetic effects problems pre-takeoff. However, the same logic is
applied after landing and immediately after the land detector detects
landing. In some cases, this can disturb the other EKF states which can
lead to a state change of the land detector.
To avoid such a loop, the start of this forced yaw fusion is delayed by
5 seconds after landing.
 2021-02-18 10:11:51 +01:00
Daniel Agar 310f415175 EKF add const state reset status access 2021-02-16 10:33:43 -05:00
isidroas 0c5291d194 Heading reset to magnetometer from GPS or EV (#969) 2021-02-16 14:37:34 +01:00
Mathieu Bresciani d4258cc66d yaw emergency: check angle between vel observation and estimate (#975) 
use thresholds based on state and obs variance

* [AUTO COMMIT] update change indication
 2021-02-15 10:31:16 +01:00
Paul Riseborough 6f664abc4a EKF: Non functional changes arising from review 2021-02-09 19:55:32 +11:00
Paul Riseborough 9c89fa3b29 EKF: Use IMU clipping to adjudicate bad accel data check 2021-02-09 19:55:32 +11:00
bresch cd38621dd6 flow terrain: use dedicated variable for last fusion time 
This avoids confusion with the flow "for velocity" fusion in the main
EKF
 2021-02-08 23:26:33 -05:00
Paul Riseborough efb78deef9 EKF: Set position fusion gate option false by default 2021-01-21 20:55:42 +11:00
Paul Riseborough 556a195320 EKF: Improve handling of non position mode large position innovations 
These changes fix a bug that allowed IMU gyro errors that cause large tilt errors prior to start of aiding not resulting in large innovation test ratios.
 2021-01-21 20:55:42 +11:00
Paul Riseborough dd259a288f EKF: Don't modify accel bias states and variances when inhibited 2021-01-09 11:03:31 -05:00
Daniel Agar a8e0e82858 EKF: update mag LPF immediately 2020-12-17 10:46:04 +01:00
Daniel Agar 1237087d70 AlphaFilter add constructor that takes alpha and use in EKF 2020-12-16 13:55:59 -05:00
bresch b0cf45e2d2 gps_alt: rename getGpsAltVar -> getGpsHeightVariance 2020-12-13 14:39:50 +11:00
bresch 02369cd415 gps_alt: extract measurement variance computation and saturation 2020-12-13 14:39:50 +11:00
Mathieu Bresciani 8f3df7a97b flow: restructure optical flow control logic (#928) 
- extract motion checks performed on ground
- move all non-timing check to controlOpticalFlowFusion. This simplifies and standardirzes the setOpticalFlowData function. Furthermore, in some cases (SITL, PX4Flow), the dt is forced to 0 when the quality is 0 (which is usual on ground) so the ekf needs to ignore those values on ground to initialize properly the flow fusion.
- add filter convergence test
- move check for dt time max in setOpticalFlowData
- in the simulator, do not update dt as this is the sensor integration
period.
 2020-12-09 11:33:00 +01:00
Paul Riseborough 44ebfb8c43 EKF: Don't report local position as valid when in a non-position mode 2020-12-08 18:03:04 -05:00
Daniel Agar da7d41e78a EKF: add mag bias reset helper and update IMU bias reset to match (#924) 2020-12-08 12:16:59 -05:00
Daniel Agar a21092804a EKF: remove virtual getters from estimator_interface 2020-11-02 09:35:47 -05:00
Daniel Agar 48a8992caf EKF: move small simple getters to header 
- return by const reference where possible
 2020-10-30 09:48:45 -04:00
Daniel Agar defb35d8f5 EKF: pass imuSample by const reference 2020-10-30 09:47:22 -04:00
Daniel Agar d85e24d3ca EKF: Ekf class is final 2020-10-27 08:53:58 -04:00
Daniel Agar 5ea8824439 EKF: inline simple getters 2020-10-27 08:53:58 -04:00
Daniel Agar 6e99ebd928 EKF: add fault status bit for bad vertical accel data 2020-10-26 17:37:15 +01:00
Mathieu Bresciani dd3ffc4192 Optical flow: compute velociy from corrected flow data for logging (#920) 
This is really useful when debugging optical flow data
 2020-10-26 13:27:52 +01:00
Paul Riseborough 7c81350c7a EKF: Don't yaw reset if not yaw induced nav failure 2020-10-06 20:50:52 +11:00
bresch 5879eaaf78 ekf: fix variable length array error 
Even if the non_zeros() function is static constexpr, gcc (10.2) does not like it.
Using sizeof...(Idxs) fixes the error.
 2020-09-25 11:33:29 +02:00
Kamil Ritz fdc86c247a add functions to compute yaw (321 and 312 sequence) 
from quaternion and rotation matrix
 2020-08-27 09:51:56 +10:00
kamilritz 404edde7f3 Add measurementUpdate function 2020-08-24 18:04:57 +10:00
kamilritz da9bfe4316 Update interface of applyCorrectionsToOuputStates 2020-08-16 10:00:47 +10:00
Paul Riseboroughandkamilritz 21cc46edd7 EKF: Convert magnetic field observation methods to use SymPy generated code (#879) 
* EKF: Add comparison test for mag field fusion generated code

* EKF: convert mag field fusion to use SymPy generated code

* EKF: Add test comparison program for yaw fusion equations

* Stop setting 0 to 0

* Reduce if/else statement to only if

* EKF: more accurate implementation for sequential fusion of magnetometer data

* test: update change indicator

* Use matrix::SparseVector<float, 24, ...> for observation jacobian

* Adapt the auto code generation to allow for different bracket styles

* Add auto generated code for mag fusion

* Add generic computation of KHP

* Apply generic computation of KHP to mag fusion

* tests: update change indicator

* tests: update change indicator

Co-authored-by: kamilritz <kritz@ethz.ch>
 2020-08-11 18:57:22 +10:00
kamilritz 310b989c9a refactor start of gps into separate function 2020-08-07 10:02:47 +02:00
kamilritz 7eb2b08eed Yaw measurement jacobian to Vector4f 2020-08-04 09:57:00 +02:00
kamilritz 15d360f446 saved_mag_ef_cov to Squarematrix2f 2020-08-04 09:57:00 +02:00
kamilritz 7609bc69b2 drag innov member variables to Vector2f 2020-08-04 09:57:00 +02:00
kritz 88c52aba5e Refactor ev fusion start into helper functions (#872) 2020-07-27 11:42:52 +02:00
kamilritz 89bfcc2167 Introduce checkAndFixCovarianceUpdate(KHP) function 2020-07-16 18:25:06 +02:00
kamilritz b5e1397c0f Add const modifier 
const modifier


Add missing const modifier
 2020-07-16 18:25:06 +02:00
kamilritz 796afd5f98 Rename yaw_use_inhibit -> is_yaw_fusion_inhibited 
Temp
 2020-07-16 18:25:06 +02:00
kamilritz b0458fbded Refactor output buffer updates into separate functions 2020-07-16 18:25:06 +02:00
kritz 9eea44f4ab Return type of resets (#859) 
* Reset position/velocity return type is void

* Delete not needed comments
 2020-07-02 14:53:59 +02:00
bresch b0f79caf34 GPS yaw: Extract and refactor GPS yaw control logic 
- Rename GPS yaw fusion functions:
resetGpsAntYaw -> resetYawToGps
fuseGpsAntYaw -> fuseGpsYaw

- Move last fusion time in reset function
 2020-07-02 13:35:13 +02:00
kamilritz 16a00eae0b Make Kfusion a Vector24f 2020-07-02 09:02:24 +02:00
kamilritz 552bf824ea Add typedef for Matrix 24 types 2020-07-02 09:02:24 +02:00
kamilritz 5356077a32 Make flow_innov/-var a matrix Vector2f 2020-06-26 08:26:26 +02:00