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
