457ce90541
ekf2: run simplified GNSS checks after initial fix
...
This prevents stopping GNSS fusion on slightly degraded solution when
tight checks are set
2025-06-06 15:59:56 +02:00
e487d59521
Vehicle_odometry: protect angular_velocity field against aliasing
...
Accumulate delta-angles between each publication to get the correct
angular velocity between two attitudes
2025-06-04 09:01:33 +02:00
0e32b155f3
ekf2: simplify names of scoped variables
2025-06-03 09:22:32 +02:00
c33d79cfb4
ekf2: move gnss_checks to their own class
2025-06-03 09:22:32 +02:00
2dbce4d958
ekf2: replace defines with enum class
2025-06-03 09:22:32 +02:00
5332010b13
ekf2: move on ground GNSS checks to separate function
2025-06-03 09:22:32 +02:00
6b8bf80423
ekf2-mag: never skip post-takeoff mag yaw reset
2025-05-06 11:24:31 -04:00
f0fdf0b53b
EKF2: distinguish airspeed source and use synthetic for wind
...
Enable wind-dead-reckoning with airspeed source to synthetic airspeed
2025-05-02 13:37:18 +02:00
e7250bc8d5
ekf2: do not let mag heading and declination update xy gyro biases
...
This can destabilize the tilt estimate when the mag field is disturbed
2025-05-02 10:23:43 +02:00
a40377e544
ekf2: always update IMU filters
2025-05-02 10:23:43 +02:00
6a105bcbdb
ekf2-grav: accelerate tilt alignment
2025-05-02 10:23:43 +02:00
c59101e8ed
ekf2-gravity: do not estimate accel bias when gravity fusion is active
...
Gravity fusion uses the bias corrected accelerometer data to correct the
tilt estimate. We should not continue to estimate the accel bias when
this is active as it creates an unwanted feedback loop.
2025-05-02 10:23:43 +02:00
ee30b70a3c
ekf2-gravity: start based on accel LPF instead of peak hold
...
This prevent rapid switching in presence of noise and the innovation
filter is good at rejecting spikes
2025-05-02 10:23:43 +02:00
6ec6f1b297
ekf2-decl: do not always update tilt
2025-05-02 10:23:43 +02:00
9404783c99
EKF2: more accurate unaided yaw calculation
...
to fix the yaw unlocking in StickYaw
when giving a lot of roll and pitch input
2025-04-22 15:55:38 +02:00
e35c1f430c
EKF-AGP: only reset lat/lon when starting
2025-04-01 16:42:26 +03:00
f73c7977dd
ekf2-flow: limit minimum flow hagl
2025-03-31 11:34:25 +02:00
53bdceb895
ekf2-flow: check test ratio on Y axis separately
2025-03-31 11:34:25 +02:00
cdab0cb6e4
ekf2-flow: use same measurement prediction as in jacobian derivation
...
Also avoid double division in flow prediction
2025-03-31 11:34:25 +02:00
82a482ec0b
ekf2: reset heading when mag calibration changed
2025-03-26 22:32:51 -04:00
9ac03f03eb
ekf2: consider GNSS vel as horizontal velocity aiding
2025-03-14 18:22:00 +01:00
2aaa54037c
ekf2: split gnss pos/vel flags
...
They can be selected independently in the control parameter, so there is
no reason why they should share the same flag.
2025-03-11 14:14:40 +01:00
7e0d04e446
ekf2: fix flow derivation for negative hagl
...
The partial derivative of the flow observation with respect
to the terrain and height states involve calculating the derivative
of 1/x, which is -1/x^2. This function is even, making the estimate
diverge in case x (the height above ground) is negative.
The solution is to take the derivative of 1/|x|,
which is -x/|x|^3 and is an odd function; preserving the sign of x.
Note that for positive values of x, the expression reduces to -1/x^2
2025-03-10 17:11:53 -04:00
5e06ab1430
ekf-agp: do not reset to AGP if GNSS fusion is active
2025-02-18 14:35:10 +01:00
2b75d2e738
run fog check independently from stuck check
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2025-02-11 09:08:25 +01:00
8cedef2dc4
further cleanup
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2025-02-11 09:08:25 +01:00
2c4d38e303
fixed mistake
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2025-02-11 09:08:25 +01:00
806500fc4a
further improve logic
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2025-02-11 09:08:25 +01:00
73ade6d05c
range finder: remove potential deadlock
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2025-02-11 09:08:25 +01:00
4df65c133e
add cs_baro_fault to switch to fallback baro if available ( #24260 )
2025-01-28 10:37:16 +01:00
caaae6ed51
ekf2: allow sideslip fusion to always start with airspeed fusion
...
This allow sideslip fusion to start during VTOL front transition or even
on a multirotor with a vertical stabilizer and an airspeed sensor for example.
2025-01-22 09:33:16 +01:00
4a5aa1e947
Fix max-hagl restriction to position/altitude control ( #23667 )
...
* fix max-hagl restriction to position/altitude control
* max hagl vel restriction in ManAcc position mode
* use interpolate func, change naming
* simplyfied vertical vel limitation
* move velocity-constraint adjustment to StickAccelXY
2025-01-20 15:50:21 +01:00
b1ca0495e2
ekf2: yaw estimator additional validity checks
2025-01-20 09:45:41 +01:00
c99cb6e94b
ekf2: do not auto-generate sideslip measurement jacobian
...
This is to trade a bit of CPU load for more flash space.
2025-01-15 11:05:50 -05:00
12a9087e92
ekf2: constrain max variance by zero innovation update
...
Clipping the variance of the covariance matrix has a destabilizing
effect as it increases the correlation between the states.
2025-01-14 21:42:33 -05:00
ab70ae3252
ekf2: fix GNSS drift false alarm
...
In SIH, the GNSS signal is zero-mean, but apparently not symmetric. The
issue is that saturating such a signal creates an artificial bias. This
made the check fail as the bias was above the threshold.
2025-01-08 21:40:25 -05:00
1abc27dd1f
remove fixed-wing condition from airspeed fusion condition
...
- enables airspeed fusion to begin during transition already
- airspeed fusion is started based on airspeed being above defined threshold
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2025-01-07 16:19:33 +01:00
b1773df441
ekf2: fix comments about primary height sources
...
Having the xxx_hgt flag set only means that the sensor is fused for
altitude/height aiding. It's not necessarily the height reference.
2025-01-07 10:23:13 +01:00
4fe6d69966
local position acceleration: use mean value between two publications ( #24105 )
...
To avoid aliasing on the ned acceleration, add an accumulation of acceleration to improve the downsampling
2025-01-03 12:43:39 +01:00
c1589ddb18
ekf2-rng: do not continuously reset terrain in rng height ref
...
When the height reference datum is ground level (range finder height
ref), the terrain state is not updated through fusion and should stay at
a constant altitude.
2025-01-03 12:25:16 +01:00
480c232bfd
ekf2: use alpha filter class
2024-12-18 14:11:08 +01:00
fa5a781e20
AlphaFilter: set time constant instead of alpha
...
Then the update function can set the dt at every iteration if needed
2024-12-18 14:11:08 +01:00
4a73195007
yaw_est: store attitude as quaternion instead of DCM
...
This saves flash and makes code simpler
2024-12-18 09:33:11 +01:00
6969e5b6b4
ekf2: do not pre-compute airspeed Kalman gain
...
The generated code is not much faster than the simple matrix-vector
multiplication
2024-12-17 22:32:16 -05:00
f9140fcd50
ekf2: set baro bias when GNSS is alt ref
...
Do this even when GNSS altitude fusion is disabled.
2024-12-13 14:55:46 +01:00
fad9ae855d
EKF2: reset aid src when resetting state to measurement
...
Filtered innovations and test ratios can be large before the reset and
would trigger pre-flight warnings
2024-12-13 14:26:57 +01:00
cc92979b06
ekf-agp: fix timeout
2024-12-12 15:49:04 +01:00
8626019ae0
EKF2: reset global position using variance
2024-12-03 13:04:25 +01:00
b6658df169
lla: move to lib directory
2024-11-29 14:21:29 +01:00
7cf42727fb
lla: add functions to convert from and to ECEF
2024-11-29 14:21:29 +01:00
bresch