Commit Graph

1345 Commits

Author SHA1 Message Date
kritz 01495ede97 Clean initialiseFilter function (#687)
* Clean initialiseFilter

* Add const qualifiers
2019-12-17 13:36:28 +01:00
kritz 532c9abd4a Expand test framework and test cases (#685)
* Fix comment

* Ekf wrapper for testing

Add utility function for accessing information in the ekf object

* Add step function for Gps sensor

* Add RangeFinder and Flow to simulated sensors

* Add first fusion logic tests

* Add units to function name

* Use EXPECT_TRUE

* Adding missing qualifiers

* Improve EXPECT_ calls

* Improve naming
2019-12-17 11:35:45 +01:00
kritz 6c25ac5731 Tilt Initialisation: Average Filter -> LowPass Filter (#686)
* Tilt Initialisation: Average Filter -> LowPass Filter

* Add _is_first_imu_sample variable

* Remove not needed comments
2019-12-17 11:01:25 +01:00
Daniel Agar 98a1aae494 EKF: use Matrix cross product 2019-12-17 09:35:08 +01:00
kamilritz 0831c158f2 Move time update to sensor base class 2019-12-16 13:40:01 +01:00
kamilritz 1832bedd13 run_seconds 2019-12-16 13:40:01 +01:00
kamilritz 39b369d90b make ekf a shared pointer 2019-12-16 13:40:01 +01:00
kamilritz f13f2f8452 use shared pointer 2019-12-16 13:40:01 +01:00
kamilritz 00cd720a66 Change folder name 2019-12-16 13:40:01 +01:00
kamilritz b9dd1e54c5 Change file names 2019-12-16 13:40:01 +01:00
kamilritz 2d2edd90e3 ECL Unit Test Clean Up 2019-12-16 13:40:01 +01:00
kamilritz 6b25dbd6c7 SensorSimulator 2019-12-16 13:40:01 +01:00
kamilritz 85e0e6e89c Remove resetStates(AndCovariances) function 2019-12-13 16:50:56 +01:00
kamilritz 64652f523b Improve quality of comments 2019-12-12 08:48:10 +01:00
kamilritz 1bf09fd370 Remove EKF prefix from logged messages 2019-12-12 08:48:10 +01:00
kamilritz 88c4929c96 Use helper function to switch to gps height 2019-12-11 17:23:45 +02:00
kamilritz 042d9b6615 Remove dangerous _primary_hgt_source variable 2019-12-11 17:23:45 +02:00
kamilritz 66f707ede0 Remove non enterable if statement 2019-12-11 17:23:45 +02:00
kamilritz cd030e7cd2 Remove switch to ev hgt during filter initialization 2019-12-11 17:23:45 +02:00
kamilritz 4bc6ae69ae Remove if statement that can not be entered 2019-12-11 17:23:45 +02:00
kamilritz 6941777e17 Remove obsolete vision sample counter during initialization 2019-12-11 17:23:45 +02:00
kamilritz 181303488c Log height innovation sensor specific 2019-12-05 20:09:34 +11:00
kamilritz f32fce28a3 Fix gps vertical observation variance 2019-12-05 20:09:34 +11:00
kamilritz ecd199d5d0 Fix vel pos test ratio 2019-12-05 20:09:34 +11:00
kamilritz 2df2dede2f Fix variance vs stdDev bug
(cherry picked from commit 0faea89da2a3326d070a6ecca93ec52ec3e29591)
2019-12-05 20:09:34 +11:00
kamilritz 3923c5fecb Add get*innovation* functions as overide 2019-12-05 20:09:34 +11:00
kamilritz 07e804676c Rename IMU biases 2019-12-05 20:09:34 +11:00
kamilritz dae8c2f8dc Group velocity and position as a 3d vector 2019-12-05 20:09:34 +11:00
kamilritz beedf1ce4f Add print statement to reset vel or pos function 2019-12-05 20:09:34 +11:00
kamilritz 5c038a3b43 Add stop*Fusion function 2019-12-05 20:09:34 +11:00
kamilritz d5dc6bb8ea Clean get*innov* interface 2019-12-05 20:09:34 +11:00
kamilritz 86b9079bdc Refactoring velPos fusion 2019-12-05 20:09:34 +11:00
bresch 6b5f011bc2 ekf: use PDOP instead of GDOP as the TDOP (part of GDOP) is usually not available
PDOP is the position dillution of precision and is given by sqrt(VDOP^2+HDOP^2)
2019-11-25 17:18:40 -05:00
Daniel Agar 362a2dfa08 remove unnecessary <cmath> include 2019-11-22 11:16:51 -05:00
RomanBapst 33a2bd0292 added python script for derivation of initial wind covariance matrix
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2019-11-20 16:28:34 +11:00
bresch 811ec81246 parameters: change check_mag_strength from bool to int32_t to comply with the standard interface 2019-11-11 15:29:52 +01:00
Mathieu Bresciani c7bdf25663 [ekf] controlMagFusion refactor and mag field strength check (#662)
* ekf_control: Inhibit mag fusion when field magnitude is large
Move mag inhibition check in separate function

* ekf_control: pull out of functionalities out of controlMagFusion
- yaw abd mag bias observability checks
- mag 3D conditions
- load mag covariances
- set and clear mag control modes

* ekf_control: refactor mag heading/3D start/stop.
Move mag declination, mag 3d and mag heading fusion out of the main function

* ekf_control: extract mag yaw reset and mag declination fusion requirements

* ekf_control: use WMM in isStronMagneticField for mag fusion inhibition
- Correct units of WMM strength table

* ekf_control: extract mag_state_only functionality of AUTOFW (VTOL custom)
Also split inAirYawReset from onGroundYawReset

* ekf_control: extract mag automatic selection
- transform if-else into switch-case for parameter fusion type selection

* ekf_control: extract run3DMagAndDeclFusion, reorganize functions, fix
flag naming in Test script

* ekf_control: do not run mag fusion if tilt is not aligned.
Reset some variables on ground even if mag fusion is not running yet. It
could be that it runs later so we need to make sure that those variables
are properly set.

* ekf_control: move controlMagFusion and related functions to mag_control.cpp

* ekf control: check for validity of mag strength from WMM and falls back
to average earth mag field with larger gate if not valid

* ekf control: remove evyaw check for mag inhibition

* ekf control: change nested ternary operator into if-else if

* Ekf: create AlphaFilter template class for simple low-pass filtering
0.1/0.9 type low-pass filters are commonly used to smooth data, this
class is meant to abstract the computation of this filter

* ekf control: reset heading using mag_lpf data to avoid resetting on an outlier
fixes ecl issue #525

* ekf control: replace mag_states_only flag with mag_field_disturbed and
add parameter to enable or disable mag field strength check

* ekf control: remove AUTOFW mag fusion type as not needed This was implemented for VTOL but did not solve the problem and should not be used anymore

* ekf control: use start/stop mag functions everywhere instead of setting the flag

* ekf control: Run mag fusion depending on yaw_align instead of tilt_align
as there is no reason to fuse mag when the ekf isn't aligned

* AlphaFilter: add test for float and Vector3f
2019-11-08 16:02:59 +01:00
bresch a6840655e8 ekf: Extract resetState from main reset and reinitialize
the covariances in the init function
2019-11-05 09:07:25 +01:00
bresch bf48004fb9 ekf: move "reset" part of the init function into a separate function
add getter for the vehicle at rest flag
2019-11-05 09:07:25 +01:00
TSC21 7eb9118673 Jenkins: update container tags; use px4-dev-base-bionic 2019-11-03 10:29:15 -05:00
kamilritz d76b704225 Only inlcude gtest if standalone build 2019-11-02 09:46:18 -04:00
kamilritz 7c1e38d3ad Make it build with Firmware 2019-10-28 11:04:38 +01:00
kamilritz d79199c863 Remove swig and python test related things 2019-10-28 11:04:38 +01:00
kamilritz d88e242a60 pytest are replaced in gtests 2019-10-28 11:04:38 +01:00
kamilritz 71be26efc6 Port RingBuffer Test to GTests 2019-10-28 11:04:38 +01:00
kamilritz cac5f3fd6d GTest and Coverage cleanup and Basic EKF GTest 2019-10-28 11:04:38 +01:00
Martina Rivizzigno fcea13eef8 add gtest, temp disable swig
(cherry picked from commit 4008d163d77f503e6acbd7ddc1d87ac6d479c897)
2019-10-28 11:04:38 +01:00
RomanBapst 9b4b24ee71 removed unused definition
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2019-10-24 07:55:37 +11:00
RomanBapst 0e3a0b8659 range_finder_checks: parametrised signal quality hysteresis duration
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2019-10-24 07:55:37 +11:00
RomanBapst 16d1e15b51 addressed review comments
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2019-10-24 07:55:37 +11:00