fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 12:38:54 +08:00
Code Issues Packages Projects Releases Wiki Activity
44,072 Commits 852 Branches 161 Tags
e03aef616c95d42a316fef54228512b62466715d
Commit Graph
100 Commits
This Branch
This Branch
All Branches
Author SHA1 Message Date
bresch a50ef2eb5e ekf2-terrain: make terrain validity based on uncertainty 
When using optical flow for terrain aiding, we cannot assume that the
terrain estimate is valid if flow is fused as is can only be observed
during motion. When no direct observation is available, the terrain is
assumed to be valid if its 1sigma uncertainty is < 10% of the hagl.
 2024-06-26 11:05:38 +02:00
bresch a665764b0e ekf2: remove unused EKF2_TERR_MASK 2024-06-26 11:05:38 +02:00
bresch 7903ddf5df ekf2-terrain: terrain is not a separate estimator 2024-06-26 11:05:38 +02:00
bresch 9001c23926 ekf2: clean up hagl vs terrain naming 
Terrain is the state: terrain vertical position
Hagl (height above ground level) is the vertical distance between the
vertical position and the terrain vertical position
 2024-06-26 11:05:38 +02:00
bresch 68980b59e2 ekf2: add terrain state 2024-06-26 11:05:38 +02:00
bresch 0a665a526c ekf2: add mag type init 
In this mode, the mag is used to inilialize the heading.
During flight, the heading is predicted using gyros and corrected
by GNSS measurements if available.
 2024-06-03 13:28:12 +02:00
bresch 53210dd8f3 ekf2-mag: with NE aiding, constrain heading drift only before takeoff 
After takeoff, the heading is easily observable
 2024-05-31 10:38:17 -04:00
bresch 3dac9af09e ekf2-mag: do not reset on WMM change when NE aiding is active 
The mag field states are observed. No need to reset them.
 2024-05-31 10:38:17 -04:00
bresch ee765e7859 ekf2-mag: do not reset when NE aiding is active 2024-05-31 10:38:17 -04:00
bresch 6515935b52 ekf2-mag: do not limit the earth mag field estimate 
The EKF can recover from an initial bad earth mag field estimate.
Constraining the field is not necessary and can lead to an unpredicted
behavior of the filter.
Declination fusion is safe to run even when the horizontal field is 0
 2024-05-31 10:38:17 -04:00
bresch 774b6ed3b8 ekf2-mag: do not use yaw emergency estimator to reset mag states 
On slowly moving vehicles (e.g.: boats, rovers), the yaw estimator has
worse convergence than the main EKF. Resetting the mag states using the
yaw estimator as reference can lead to poor heading. Also, the EKF can
recover really well from initially incorrect mag states.
 2024-05-31 10:38:17 -04:00
bresch c3d984703c ekf2-mag: remove immediate declination fusion after reset 2024-05-31 10:38:17 -04:00
bresch a6007e4b93 ekf2-mag: turn around update_all_states condition 
Non-functional
 2024-05-31 10:38:17 -04:00
bresch c11c75d32e ekf2-mag: always add process noise until initial value 2024-05-31 10:38:17 -04:00
bresch 993782cffa ekf2: only trigger position timeout reset when hpos fusion is active 2024-05-29 20:49:14 -04:00
bresch f9e2ab8d44 msg-rates-setpoint: fix frame name NED -> FRD 2024-05-27 09:35:34 +02:00
bresch ccbcbbe268 wind_est_replay: report scale instead of inverse_scale 
The estimator internally estimates the scale inverse, but the interface
should be the scale as "airspeed_corrected = scale * airspeed"
 2024-05-24 17:25:39 +02:00
bresch ea39032b45 mag_ctrl: combine common conditions for mag_hdg and mag_3d 2024-05-17 11:19:04 +02:00
bresch d796009302 mag_ctrl: do not fuse synthetic mag but do not zero the innovation 2024-05-17 11:19:04 +02:00
bresch b42799fac2 wind_est_replay: allow setting the initial scale factor 2024-05-17 09:17:08 +02:00
bresch 440465702e wind_est_replay: fix cov matrix format and data indexing 2024-05-17 09:17:08 +02:00
bresch 9dc7719d4a ekf2: Only reset to GNSS heading if necessary 
When North-East (e.g.: GNSS pos/vel) aiding is active, the heading
estimate is constrained and consistent with the vel/pos aiding. Reset to
GNSS heading should only occur if no N-E aiding is active or if the
filter is not yes aligned. Otherwise, just wait for the consistency
check to pass again (will pass at some point if the heading uncertainty
of the filter is getting too high).
 2024-04-29 07:22:01 -07:00
bresch 7cefc3172a estimatorCheck: get param only if handle is valid 2024-04-25 14:48:23 -04:00
bresch b508df39a2 imu consistency: don't scale param threshold 2024-04-24 15:51:07 -04:00
bresch 8bf1cf0b15 ekf2_params: reduce "short" description 2024-04-24 15:09:57 -04:00
bresch 4cf51e5833 baro comp: set hpf optional 2024-04-22 15:59:50 +02:00
bresch a8a67fbf8f ekf2: set horizon using specific parameter 
Some sensors can have their delay included in the timestamp. In this
case, the buffer cannot be sized using the _DELAY parameter.
 2024-04-17 09:16:16 -04:00
bresch 7a973eddce ekf2: migrate param to yaml 2024-04-16 11:14:13 -04:00
bresch 42a16bd91d order yaml fileds 2024-04-16 11:14:13 -04:00
bresch 5f61e3b785 ekf2: update change indicator 2024-04-05 22:26:03 -04:00
bresch b79d3854e4 ekf2: remove option to continuously fuse mag declination 
Declination fusion is only used when not observable (no global aiding).
 2024-04-05 22:26:03 -04:00
bresch dae246d7e2 ekf2: do not continuously use mag decl fusion when GNSS fusion is active 
This prevents over-constraining the heading from mag fusion. An
incorrect mag yaw rotation can be absorbed as a declination error.
 2024-04-05 22:26:03 -04:00
bresch 2f51db7284 ekf2: limit mag heading fusion to prevent heading overconfidence 2024-04-05 22:26:03 -04:00
bresch 51883fe5d4 ekf2: integrate mag heading into mag 3D 2024-04-05 22:26:03 -04:00
bresch 0c9e4013d2 ekf2: improve tilt leveling speed 
Starting with no yaw uncertainty makes the tilt more observable when
using fake position fusion during the quasi-stationary alignment phase.
 2024-04-05 22:26:03 -04:00
bresch 12fefbcfbd ekf2: use global definition of quaternion error 2024-04-05 22:26:03 -04:00
bresch 62b8db153b mpc: fix PositionControl unit test 
The unit test assumes the position controller is in "decoupled" mode
 2024-03-20 14:16:12 +01:00
bresch 638e17d551 ekf: update change indicator 2024-03-20 14:13:49 +01:00
bresch 6d819343aa ekf2: fix direct state measurement update for suboptimal K case 
The duration of a unit test had to be increased because the incorrect
covariance matrix update, was making the unit test passing faster
(over-optimistic variance).
 2024-03-20 14:13:49 +01:00
bresch cb2bb2e098 ekf2: add no gyro bias estimate test case 
This makes the ekf unstable and creates NANs during initialization
 2024-03-20 14:13:49 +01:00
bresch c9221b91ad ekf2: fix gnss yaw unit test 2024-03-20 14:04:19 +01:00
bresch 2e6dd243af mpc: add possibility to generate tilt using full 3D accel 
Using full 3D acceleration provides better horizontal acceleration
tracking but also creates a sometimes unwanted behavior because the tilt
is directly coupled with the vertical acceleration setpoint.
 2024-03-19 12:29:34 -04:00
bresch 7c6ecd95a8 mc_wind_estimator_tuning: optionally use GNSS velocity 
Sometimes GNSS is logged but not used
 2024-03-14 21:06:24 +01:00
bresch b2f1122372 ekf2: remove old yaw 321 and 312 derivations 2024-03-14 11:33:45 -04:00
bresch ee63f3e664 update change indicator 2024-03-14 11:33:45 -04:00
bresch e3f67d5c1a ekf2: new yaw derivation 
Instead of euler angles, compute measurement jacobian using a small
global perturbation around the vertical axis
 2024-03-14 11:33:45 -04:00
bresch 0d0978b3b9 ekf2: update change indicator 2024-03-07 11:06:31 -05:00
bresch 0639f5370c ekf2: fix mag and wind covariance prediction 2024-03-07 11:06:31 -05:00
bresch 2bacb4b65d ekf2: update change indicator 2024-03-07 15:11:47 +01:00
bresch 421f13e4b5 ekf2: fix joseph covariance update for Schmidt-Kalman filter 
If part of the Kalman gain is zeroed, the first step of the joseph
update does not produce a symmetrical matrix.
 2024-03-07 15:11:47 +01:00
bresch 6f9a378247 yaw_est: force set gyro bias when at rest 
The gyro bias estimate from EFK2 is really good when at rest and should
be used by the yaw estimator to prevent heading drifts due to poor
heading observability.
 2024-03-05 14:00:06 -05:00
bresch 28db3e1c8c ekf2: update change indicator 2024-02-27 12:33:43 -05:00
bresch e9d43015ce ekf2: fix unit tests failing due to mag fusion changes 2024-02-27 12:33:43 -05:00
bresch b46fc9a67d ekf2 sensor_sim: set correct world mag field 2024-02-27 12:33:43 -05:00
bresch b80f15f7b5 ekf2-mag_auto: always use mag 3D after takeoff 2024-02-27 12:33:43 -05:00
bresch 051baec9c4 ekf2: allow wind dead-reckoning after manual position reset 
Reset velocity using airspeed and start navigating
 2024-02-27 09:34:05 -05:00
bresch 08a2a6c836 update EKF2 change indicator 2024-02-26 12:32:59 -05:00
bresch d501d8e1d4 ekf2: use Joseph stabilized update in direct state observations 2024-02-26 12:32:59 -05:00
bresch 9d9766c6cf ekf2: use Joseph stabilized covariance update 2024-02-26 12:32:59 -05:00
bresch 37a40d3fc2 baro static pressure compensation tuning: remove dependency to baro bias 
`estimator_baro_bias` requires to have GNSS and baro hgt active and GNSS as the reference. This is quite restrictive. Instead, we can simply use a high-passed version of the baro error.
 2024-02-23 10:07:13 +01:00
bresch c221da27a7 ekf2: set attitude validity flag using centralized function 2024-02-20 11:33:30 -05:00
bresch 17d55dddd6 ekf2-drag: do not generate Kalman gain to save flash 2024-02-06 12:16:33 +01:00
bresch 1efb08375a ekf2: let drag fusion affect the complete state vector 
This improves tilt estimation and can extend the inertial dead-reckoning
validity period
 2024-02-06 12:16:33 +01:00
bresch 9c02e384e6 ekf2-agp: follow measurement reset 2024-01-30 11:23:55 -05:00
bresch 5d9081b0dd ekf2-agp: ensure logging of AGP aid_src topic 2024-01-30 11:23:55 -05:00
bresch 4268759d4a ekf2-agp: reset to measurement on fusion timeout 2024-01-30 11:23:55 -05:00
bresch 80f20e619c ekf2: zvup sequential fusion 2024-01-29 12:12:37 -05:00
bresch 0bf9ccdcc9 sys-id: implement linear and log sine sweeps 2024-01-24 12:18:23 -05:00
Mathieu Brescianiandbresch cb396a6339 ekf2: fix computation of tilt and yaw variances add them to logging 
Co-authored-by: bresch <bresch@users.noreply.github.com>
 2024-01-24 12:14:15 -05:00
bresch da28d9a7f2 ekf2-grav: rename g-force unit to g0 to avoid confusion with grams 2024-01-24 13:26:25 +01:00
bresch 7c7a3c117a ekf2-gravity: nomalize gravity fusion and proper sequential fusion 2024-01-18 20:39:16 -05:00
bresch d624fbba07 ekf2-grav: lower gate to reject real acceleration more effectively 2024-01-18 20:39:16 -05:00
bresch c28972d15e ekf2-grav: only use filtered accel norm to start/stop the fusion 
Using the raw data makes the swith too sensitive to noise
 2024-01-18 20:39:16 -05:00
bresch 8158a14eff fw: cleanup and make use of Vector3 more intensively 2024-01-17 14:44:11 +01:00
bresch c1b139dea1 atune: reset param on start 
This prevents a race condition where autotune cannot start because the param was already set to 1
 2023-12-21 13:52:47 +01:00
bresch b8c81f6281 gps_blending: fallback to secondary if primary has no fix 2023-12-20 16:35:30 -05:00
bresch 094048ed04 gps_blending: fix selection rapid switching 
Once a timeout of the primary instance is detected, a fallback is only
allowed until the primary receiver is regained.
 2023-12-20 16:35:30 -05:00
bresch 36eb319834 ekf2-yaw_est: split imu and velocity updates 2023-12-18 18:11:20 +01:00
bresch bba30663cc update change indicator 
Caused by the GNSS checks being performed at the delayed-time horizon
 2023-12-18 18:11:20 +01:00
bresch a653073d4f ekf2: perform GNSS checks at delayed-time horizon 
- never fuse a measurement that is not passing the checks
- cleanup and simplify GNSS vel/pos control logic
 2023-12-18 18:11:20 +01:00
bresch 5ca22df55c ekf2-pos bias estimator: use enum 2023-12-05 11:22:59 -05:00
bresch a3515a2474 ekf2: scope HeightSensor enum 2023-12-05 11:22:59 -05:00
bresch 6bd1145006 ekf2: scope RngCtrl enum 2023-12-05 11:22:59 -05:00
bresch 1df52df27d ekf2: scope GnssCtrl enum 2023-12-05 11:22:59 -05:00
bresch 97423136d1 ekf2-AGP: scope control enum 2023-12-05 11:22:59 -05:00
breschandDaniel Agar fe7988672f ekf2: auxiliary position fusion 
Co-authored-by: Daniel Agar <daniel@agar.ca>
 2023-12-01 11:50:22 +01:00
bresch e568b4a1d7 replay: increase buffer size 
required to parse larger topics
 2023-11-21 21:56:34 -05:00
bresch 6320ca64a9 ekf2-terr: allow range fusion before takeoff 2023-11-21 21:56:34 -05:00
bresch 0f3378e194 ekf2: fix terrain range finder logging 2023-11-21 21:56:34 -05:00
bresch 1c3a1183c8 ekf2-flow: refactor control logic 
Use flow rates instead of integrals in backend. This allows us to delay
the data to the mitpoint integration time and simplifies the code in
general.
Gyro compensation can still be done in EKF2 if needed, but the
flow module normally already appends the correct gyro data to the flow
message.
 2023-11-21 21:56:34 -05:00
bresch c7672fc68c ekf2-params: delta vel/ang -> accel/gyro 2023-11-20 20:10:16 -05:00
bresch f2482d7157 ekf-pdf-report: plot bias using bias msg instead of state vector 2023-11-20 20:10:16 -05:00
bresch 2110eae25d atune: initialize filter if not already initialized 2023-11-16 23:14:17 -05:00
Mathieu Brescianiandbresch efa12ad224 EKF2 simplify covariance propagation (#22344) 
* ekf2-derivation: optimize before generating cpp code

* update EKF2 change indicator

Slight changes due to simplifications done in the covariance prediction

---------

Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
 2023-11-14 11:22:16 -05:00
bresch 00568985c0 yaw_est: use error-state covariance prediction 
Convergence improvements in high yaw rate conditions
 2023-11-13 11:37:27 -05:00
bresch 3acc29410a failsafe: disarm if battery failure is detected during spoolup 2023-11-07 17:38:06 -05:00
bresch a989e5338c ekf2: reset globlal position uncertainty when GNSS is fused 
There is no reason to keep an uncertainty on the origin as it is then
already contained in the local position estimate when GNSS data is fused
in the filter.
 2023-11-02 13:11:40 -04:00
Mathieu Brescianiandbresch 0d6c2c8ce9 EKF2: Error-State Kalman Filter (#22262) 
* ekf derivation: change to error state formulation
* ekf2: update auto-generated code for error-state
* ekf2: adjust ekf2 code for error state formulation
* ekf2_tests: adjust unit tests for error-state EKF
* update change indicator for error-state EKF
* ekf2_derivation: allow disabling mag and wind states

---------

Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
 2023-10-31 10:02:18 -04:00
bresch eed2870fd8 ekf2: fix optical_flow_vel publication 
We should otherwise call this publication before the aid_src publisher
that sets the timestamp. Having it separate avoids this ordering
constraint.
 2023-10-25 09:59:56 -05:00
bresch 1c9373e83b update baro static pressure compensation tuning script 
Field name changed in vehicle_gps_position
 2023-10-16 09:45:28 -04:00