The failsafe from rng height to baro height should occur after a reset
to baro triggered by the controlHeightSensorTimeouts function and not in
the fusion selector.
Until now, the EKF was fusing baro measurements between rng updates if
the range finder ODR was slower than the EKF update rate. This is not
the case anymore as the height reset occurs after 5 seconds.
The unit test has been extended to show this behavior.
- 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.
- allocate IMU and output buffers on construction according to defaults
- determine buffer max time delay based on configuration parameters
- reorder flowSample and extVisionSample to minimize padding
- adjust parameter defaults to match PX4-Autopilot
* 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>
* added python script with ekf derivation (WIP)
Signed-off-by: RomanBapst <bapstroman@gmail.com>
* worked on c code auto-generation
Signed-off-by: RomanBapst <bapstroman@gmail.com>
* save before variable name change
Signed-off-by: RomanBapst <bapstroman@gmail.com>
* changed symbol names
Signed-off-by: RomanBapst <bapstroman@gmail.com>
* added codegeneration class
Signed-off-by: RomanBapst <bapstroman@gmail.com>
* improve 3D mag fusion derivation
Signed-off-by: RomanBapst <bapstroman@gmail.com>
* EKF: Extend ekf sympy derivation to include all observation types
* EKF: Add custom ecl::powf function for integer powers
* EKF: Convert ekf covariance prediction to use sympy output
* EKF: Add test program to compare sympy and matlab covariance prediction
Also tests ecl::powf(x,exp) function
* EKF: simplify ecl::powf function
* Generate code to subfolder generated/
* Add printouts for showing code generation progress
* Move generated covariance code to generated folder
* Upgrade code generation to python3
* main.py: Remove unused create_symbols function
& making code more compact
* main.py: move main part into function
* Code generation: fix passing wrong rotation matrix to yaw_observation ()
* EKF: Amend generated code filename for consistency
* Move ecl::powf function test to unit tests
* EKF: Use updated ecl:powf functionality in test program
* Move ecl::powf to utils.hpp
* Update ecl::powf test
* Update output change indication
* test: update expected output for change indicator
* test: update expected output for change indicator again
Co-authored-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: kamilritz <kritz@ethz.ch>
Minor consistency fixes for the copyright header and update the tables to current. PX4Buildbot will periodically update the tables automatically from this point.
- updated table to 2 bytes (int16) per element and scaled the inclination/declination/strength tables to use most of the range without being too awkward
- tables have been extended to include the full latitude range
- expanded the API slightly to offer declination/inclination in both degrees/radians and the magnetic strength in Gauss and Tesla
- generated some simple testing that verifies interpolation between points
When the antennas are not parallel to the x body axis, the GPS message
contains the angular offset but the data is already corrected in the
driver. EKF2 should then not add this offset during the initialisation.
This fixes the cases where the yaw message from the GNSS receiver would
take more time than the vel/pos. The estimator should wait and not
immediately fall back to an other aiding source after 5sec.
If it never comes, it will never fall back, but this is ok since the
user wants to fly with GPS aiding an not with something else.
If the user selects GPS yaw fusion but that there is no GPS yaw data in
the GPS message or if the fusion is rejected for some time, the GPS yaw
data is declared faulty and the fusion is stopped to allow an other
source of yaw aiding to start.
This is how it is also done in ekf2_main. Otherwise, this leads to
multiple integration of the same IMU data due to asynchronous sensor
updates triggering a prediction step between IMU updates.
Fix unit tests that broke because of this fix
The `_deadreckon_time_exceeded` flag is used in
`local_position_is_valid()`. This means that
`_params.valid_timeout_max` after startup, in my observed case 5
seconds, the local position switche from valid to invalid and then after
a while back to valid again.
With this fix, the local position is flagged invalid from boot and gets validated after the first aiding event.
Co-authored-by: Julian Oes <julian@oes.ch>
* ekf: disable xy accel bias learning before takeoff
As those biases are usually poorly observable before takeoff because
they are almost perpendicular to the gravity vector, learning is often
driven by noise and numerical issues. This results in incorrect bias
learning before takeoff when the drone is static on ground for a long
period of time.
* ekf: update unit test and change indicator
This is a non-functional change required to select accel bias estimation
per axis selection. The intent is then to disable the learning before
takeoff of the components that are poorly observable.
* Support vision velocity expressed in body frame
* Use switch statement for vision velocity frame
* Robustify vision velocity frame test
* Increase lower bound on vision velocity noise to 0.05 m/s
* EKF: Improve covariance prediction stability
Eliminates collapse of vertical velocity state variance due to rounding errors that can occur under some operating conditions.
* EKF: Fix typo
* test: Fix initialisation test cases
Provide sufficient time for variances to stabilise and fix calculation of reference quaternion for alignment.
* test: Allow for accumulated rounding error in IMU sampling test
* test: Allow sufficient time for quaternion variances to reduce after initial alignment
* test: Increase allowance for tilt alignment delay and inertial nav errors
* test: Increase allowance for tilt alignment delay and inertial nav errors
* adpat reset velocity test
* test: update change indication file
* test: Adjust tests to handle alignment time and prediction errors
* README.md: Add documentation for change indicator test
