dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity
843 Commits 514 Branches 136 Tags 234 MiB
Commit Graph

634 Commits

Author SHA1 Message Date
Roman Bapst 6344fa0e8c limit maximum rates of all measurement at which they are stored into the buffers 2016-04-19 12:47:57 +02:00
Roman Bapst 00c8821006 disable airspeed fusion for now. Needs more testing and implementation of side slip fusion 2016-04-19 11:18:45 +02:00
Paul Riseborough 2fa8a11a29 EKF: replace unnecessary memcopy 2016-04-19 19:10:41 +10:00
Roman Bapst 99fc61c27c ekf2 airspeed fusion: 
- finished logic for fusion
- fixed bug where previous control status was set in the wrong location
 2016-04-19 09:53:31 +02:00
Paul Riseborough 82920da232 EKF: strengthen checking of local position status 
provides immediate status reporting when dropping out of optical flow mode
 2016-04-19 17:38:22 +10:00
Paul Riseborough 31bf342fc1 EKF: publish GPS check status 2016-04-19 17:38:22 +10:00
Paul Riseborough 6fa13c7806 EKF: publish control mode status 2016-04-19 17:37:27 +10:00
Roman Bapst 60abf07bee added function to return accelerometer bias 2016-04-18 17:52:16 +02:00
Roman Bapst f32303de69 added function to return accelerometer bias 2016-04-18 17:00:17 +02:00
Lorenz Meier 3455931617 EKF init: Fix initialization statement 2016-04-17 19:29:23 +02:00
Daniel Agar 861c13f1fe cmake STACK -> STACK_MAIN 2016-04-16 21:46:50 -04:00
Paul Riseborough ea38aa130f EKF: update default time delay parameter values 2016-04-14 08:53:59 +10:00
Paul Riseborough 8c55e36ca9 EKF: use common value for gravity 2016-04-12 11:14:31 +10:00
Paul Riseborough 163c08a3ac EKF: Improve output observer documentation 2016-04-12 11:14:31 +10:00
Paul Riseborough 2dcc6e2053 EKF: Improve accuracy of state prediction 
Use an a common estimator value for gravity
Use average orientation across update interval when rotating delta velocities
 2016-04-12 11:14:31 +10:00
Paul Riseborough 5bf02517a7 EKF: Rationalise use of rotation matrices and improve efficiency 2016-04-12 11:14:31 +10:00
Paul Riseborough e10093854a EKF: correct outputs for IMU offset 2016-04-12 11:14:31 +10:00
Paul Riseborough b46053415f EKF: Compensate optical flow data for sensor position offset 2016-04-12 11:14:31 +10:00
Paul Riseborough 48b105b748 EKF: correct range finder data for sensor position offset 2016-04-12 11:14:31 +10:00
Paul Riseborough e89dbb9f63 EKF: correct GPS data for antenna position offset 2016-04-12 11:14:31 +10:00
Paul Riseborough 3580940e10 EKF: Add sensor position offset parameters 2016-04-12 11:14:31 +10:00
Paul Riseborough 006b6b58e4 EKF: fix bug in status print statement 2016-04-12 10:38:50 +10:00
Paul Riseborough 74078cde94 EKF: reset state variance when performing a height reset 
Set vertical position and velocity variances using known sensor error characteristics if we have reset the states to the sensor readings.
 2016-04-11 19:40:27 +10:00
Paul Riseborough f4f108d57d EKF: Reset vertical velocity when performing a height reset 2016-04-11 19:25:24 +10:00
Lorenz Meier f86ef34782 EKF: Fix error message which lied 2016-04-05 19:06:35 -07:00
Paul Riseborough a352c2f4e3 EKF: Fix posix build error 
Memset cannot be used on a class like this. Setting the time elements to zero achieves the desired result.
 2016-04-05 18:57:21 -07:00
Lorenz Meier 6b3ad03419 Merge pull request #86 from CarlOlsson/small_airspeed_fix 
Small airspeed fix
 2016-04-05 16:38:37 -07:00
Paul Riseborough 430d4b1cf8 EKF: ensure data in buffers is zero at startup 2016-04-05 22:23:37 +02:00
Paul Riseborough 470098e182 EKF: update default tuning parameters 
Parameters needed to be changed when covariance prediction error was corrected.
The magnetic earth and body field process noise now use separate parameters to help with tuning
 2016-04-05 22:23:37 +02:00
Paul Riseborough eaf94935f0 EKF: Fix bug in initialisation of height and magnetic field 
This prevents zero data being used to form the initial height and magnetic field.
Do not start sampling initial values until non-zero time values are retrieved from the buffer.
 2016-04-05 22:23:37 +02:00
Paul Riseborough 03eac2f25e EKF: Apply covariance prediction derivation changes 2016-04-05 22:23:37 +02:00
CarlOlsson 8678a939e2 removed reinitialization of intermediate variables 2016-04-05 14:18:47 +02:00
Roman Bapst 2632c930f7 Merge pull request #85 from CarlOlsson/add_error_reporting_tas 
added fault status reporting
 2016-04-04 18:52:06 +02:00
CarlOlsson eee6f1048f Changed vtaspred threshold 2016-04-04 16:27:06 +02:00
CarlOlsson 3ad5b52aea removed comment 2016-04-04 16:26:46 +02:00
CarlOlsson 616725410d updated calculation 2016-04-04 16:26:13 +02:00
CarlOlsson ec9902d802 added fault status reporting 2016-04-04 16:21:12 +02:00
CarlOlsson 1ea26b406a change name to true_airspeed 2016-04-04 16:10:52 +02:00
Roman Bapst 27b894540e put airspeed fusion logic but don't actually call it yet 2016-03-30 17:01:03 +02:00
Roman Bapst 1b7115dec9 increased airspeed fusion rate to 12.5Hz 2016-03-30 17:01:03 +02:00
CarlOlsson f5a9afd278 moved init 2016-03-30 17:01:03 +02:00
CarlOlsson f990d99790 fixed bug 2016-03-30 17:01:03 +02:00
CarlOlsson 4301e1105b added commas 2016-03-30 17:01:03 +02:00
CarlOlsson 81fc086b76 adopted ekf_interface.h 2016-03-30 17:01:03 +02:00
CarlOlsson d440c883b5 adopted ekf_interface.cpp 2016-03-30 17:01:03 +02:00
CarlOlsson f8878d41dc adopted ekf_helper.cpp 2016-03-30 17:01:03 +02:00
CarlOlsson 6bb2f7638c adopted ekf.h 2016-03-30 17:01:03 +02:00
CarlOlsson 0918fa04c7 adopted ekf.cpp 2016-03-30 17:01:03 +02:00
CarlOlsson 92abf93c8f adopted common.h 2016-03-30 17:01:03 +02:00
CarlOlsson 0fb1e0578b added function for fusing airspeed 2016-03-30 17:01:03 +02:00
Paul Riseborough 400a6e12ba EKF: Ensure all data in buffers is initialised 
This is a defensive change to prevent introduction of NaN's into the filter if data is read from the incorrect place in the buffer.
 2016-03-20 15:14:16 +11:00
Paul Riseborough 064a0e4dbc EKF: Don't use GPS to set position noise when not using GPS 2016-03-16 20:12:36 +11:00
Paul Riseborough c23d72ba29 EKF: Ensure filter control modes are correctly initialised 2016-03-16 17:22:27 +11:00
Paul Riseborough 9f3b1351f7 EKF: Don't initialise velocity to GPS on initial alignment 
For initial alignment the velocity and position should start at zero
 2016-03-16 17:20:57 +11:00
Paul Riseborough 687fcc70be EKF: Explicitly define type conversion for GPS height 2016-03-16 17:18:20 +11:00
Paul Riseborough e334a5dc57 EKF: Add check for NaN's on attitude states 2016-03-16 13:43:21 +11:00
Paul Riseborough 6b2e2dba90 EKF: Add GPS height option and improve height recovery 2016-03-16 10:40:51 +11:00
Paul Riseborough 63b0cf4360 EKF: Fix baro height offset bug 2016-03-14 15:51:15 +11:00
Paul Riseborough 49023f3d7e EKF: fix travis build error 2016-03-13 21:56:28 +11:00
Paul Riseborough 7677a162aa EKF: Don't start the output observer before the main filter has initialised 
This prevents the possibility of output transients if alignment is delayed.
 2016-03-13 21:17:51 +11:00
Paul Riseborough d2407c3463 EKF: code style updates 2016-03-13 21:17:51 +11:00
Paul Riseborough c58ab3e256 EKF: Enable fallback to baro alt when using range finder for height 2016-03-13 21:17:51 +11:00
Paul Riseborough 370f643f42 EKF: Enable use of range finder for primary height source 2016-03-13 18:44:34 +11:00
Paul Riseborough 109e0e6dfc EKF: When commencing GPS aiding, don't reset local position and velocity if using optical flow 2016-03-11 11:03:43 +11:00
Paul Riseborough 26238bc2f5 EKF: Allow for change in position when defining the WGS-84 origin position 
This allows GPS aiding to commence later in flight without step changes in local position output
 2016-03-11 11:03:43 +11:00
Paul Riseborough e0fcce1463 EKF: Make position and velocity reset publish success 
Some users of the position and velocity reset functions will need to know if the reset has been successful.
 2016-03-11 11:03:43 +11:00
Paul Riseborough 48f980b054 EKF: Fix syntax causing posix build to fail 2016-03-11 11:03:43 +11:00
Paul Riseborough eab0ef4266 EKF: Update default process noise for terrain estimator 2016-03-11 11:03:43 +11:00
Paul Riseborough 962fd0aaf2 EKF: Adjust terrain process noise for gradient effect 2016-03-11 11:03:43 +11:00
Paul Riseborough 1a2da887ab EKF: Fix bug in calculation of terrain estimator Kalman gain 2016-03-11 11:03:43 +11:00
Paul Riseborough cc5512905a EKF: prevent optical flow, GPS and baro fusion from blocking each other 2016-03-11 11:03:43 +11:00
Paul Riseborough ffebaf384f EKF: Set initial optical flow fusion monitor outputs to zero 2016-03-11 11:03:43 +11:00
Paul Riseborough 5acd1cbac4 EKF: Make definitions of parameters clearer for external use 2016-03-11 11:03:43 +11:00
Paul Riseborough dd1d58bab5 EKF: Remove unnecessary matrix operations from optical flow fusion 
The updated formulation means that H_LOS[][8] is always zero, so these operations are no longer required.
 2016-03-11 11:03:43 +11:00
Paul Riseborough b3b0f1347a EKF: Make normal GPS mode the default 2016-03-11 11:03:43 +11:00
Paul Riseborough d97d308ca7 EKF: Add control of optical flow and range finder fusion 2016-03-11 11:03:43 +11:00
Paul Riseborough 836fe39070 EKF: Update external interface functions to support optical flow 2016-03-11 11:03:43 +11:00
Paul Riseborough 2ff338048d EKF: Add support for range-finder fusion as primary height reference 2016-03-11 11:03:43 +11:00
Paul Riseborough 2c2850c0ce EKF: Add functions to get position and velocity state variance 2016-03-11 11:03:43 +11:00
Paul Riseborough 32b03819ef EKF: Add function to calculate global position validity 2016-03-11 11:03:43 +11:00
Paul Riseborough 270451e17b EKF: Update height reset to support range finder height use 2016-03-11 11:03:43 +11:00
Paul Riseborough dca186c6e8 EKF: Add required declarations for optical flow 2016-03-11 11:03:43 +11:00
Paul Riseborough 122dd9c531 EKF: Add source file for optical flow LOS rate fusion 2016-03-11 11:03:43 +11:00
Paul Riseborough 82cbfafb34 EKF: Add source file for terrain vertical position estimator 
Implements a single state Kalman filter to estimate terrain vertical position relative to the NED origin.
 2016-03-11 11:03:43 +11:00
mcsauder 6a61f9ba77 Remove a define that might not be required for compilations in other scnearios outside of C99 restrictions to minimize Pull Request modifications. 2016-03-05 02:17:55 -07:00
mcsauder 115c87a0ea Remove an include that was unnecessary. 2016-03-05 02:15:04 -07:00
mcsauder 1aee1b895a Uncomment commented lines that removed Eigen namespaces and dependencies. 2016-03-05 02:12:26 -07:00
mcsauder f9be23933b Correct deg to rad conversion inversion. Add additional functionality to mathlib to allow standalone compile without Limits.cpp and Limits.hpp files from PX4. 2016-03-05 01:58:58 -07:00
Paul Riseborough 5242af84af EKF: Standardise covariance update and use static arrays for large matrices 2016-03-05 08:58:19 +11:00
Paul Riseborough 1414d591ce EKF: Fix bug in declination fusion indexing 2016-03-05 08:43:33 +11:00
Roman Bapst b2744f965d use in_air flag from land detector to determine if we are on the ground 2016-03-03 10:53:39 +01:00
Paul Riseborough 569886a4fc EKF: Fix bug in velocity and position innovation consistency checks 2016-03-03 17:15:22 +11:00
mcsauder f9e3db0504 Move variable initializations from header file to constructor to allow C99 compliance. 2016-03-02 08:42:38 +01:00
Paul Riseborough d8627c6aaf EKF: Reset the vertical position states if height fusion times out 2016-03-02 08:39:07 +01:00
Paul Riseborough f7a53d69f2 EKF: Improve height reset function 
Add a method to reset the vertical velocity to enable in-flight resets
Reset to GPS height if baro height is not available.
 2016-03-02 08:39:07 +01:00
Paul Riseborough a6da73fa83 EKF: Add missing div0 protection 2016-03-01 18:03:35 +11:00
Paul Riseborough d5e47d21db EKF: miscellaneous comment and format changes 2016-03-01 15:35:45 +11:00
Paul Riseborough 7c83ac4b6d EKF: Use heading fusion as default for startup and ground operation 2016-03-01 15:35:17 +11:00
Paul Riseborough 57d4064d7b EKF: Increase default heading observation noise 
Improves robustness when operating in a bad magnetic field environment
 2016-03-01 15:28:53 +11:00
Paul Riseborough 97df006a6a EKF: Update direct heading fusion 
Adds a 312 Euler rotation sequence option for magnetic heading fusion.
Switches between it and the normal 321 sequence option depending on orientation.
 2016-03-01 15:25:22 +11:00
Paul Riseborough 2c9a814de1 Merge pull request #64 from mcsauder/master 
Additional filter variable initializations required to fully reset the filter variables
 2016-02-26 23:06:56 +11:00
Paul Riseborough 78d6f6941c Merge pull request #61 from PX4/pr-ImprovedAccuracyReporting 
EKF: Improve Position Accuracy Reporting
 2016-02-26 22:48:44 +11:00
mcsauder 5fb48a2e7b Merge remote-tracking branch 'upstream/master' 2016-02-25 22:51:58 -07:00
mcsauder 5fec0df70d Additional initializations required to reset complimentary filter values if the state estimate ever diverges and requires re-initiailization. 2016-02-25 22:51:14 -07:00
Roman Bapst 9192ced7bb do not reset output attitude state after heading reset to avoid jumps in attitude 2016-02-25 18:20:29 +01:00
Paul Riseborough cd0cac066a EKF: Calculate and publish horizontal and vertical position accuracy 
This calculation takes into account the uncertainty of the origin.
Dead reckoning status is also published
 2016-02-25 10:38:07 +11:00
Paul Riseborough a30830a7a9 EKF: Scale position observation noise with GPS quality 
This allows the filter to adapt to variations in GPs quality.
The range of adjustment in observation noise is limited to the range between a lower limit set by the GPS observation noise parameter and an upper limit set by the no-aiding observation noise.
 2016-02-25 10:16:32 +11:00
Paul Riseborough f55a0bff53 EKF: Fix code style 2016-02-25 08:17:50 +11:00
Paul Riseborough 5b5bddebea EKF: Always reset yaw and mag field states on entry into 3-axis mag fusion mode 2016-02-25 08:16:42 +11:00
Paul Riseborough 2d09a5f3ac EKF: Don't reset yaw and mag field states when not necessary 2016-02-25 08:16:42 +11:00
Paul Riseborough 380db7ebef EKF: Reset angle error covariance after yaw and mag field reset 
The yaw angle could have changed by a significant amount making the correlations invalid.
Setting angle variances to zero prevents the initial kick in angles due to 3D fusion starting
 2016-02-25 08:16:42 +11:00
Paul Riseborough 0ad5329caf EKF: Update comments in heading fusion to clarify calculation of magnetic heading 2016-02-24 12:06:11 +11:00
Paul Riseborough 36affe3cd8 EKF: Fix bug causing incorrect initial roll when inverted 2016-02-24 11:56:12 +11:00
Paul Riseborough 016695fc3e EKF: Reduce startup transients 
Update initial state variance values
 2016-02-24 11:48:42 +11:00
mcsauder 6613335937 Added constexpr back from const var type. 2016-02-23 16:15:52 -07:00
mcsauder 342010c113 Update c style array initialization to attempt to pass Travic CI build tests. 2016-02-23 15:52:02 -07:00
mcsauder 48e80e9e3e Correct C style array initialization. 2016-02-23 15:16:40 -07:00
mcsauder f9f00fa52b Remove unnecessary include. 2016-02-23 14:00:39 -07:00
mcsauder ccb5736353 Spaces to tab. 2016-02-23 19:58:30 -07:00
mcsauder a4cecb1704 Match variable initialization order difference with upstream. 2016-02-23 19:57:27 -07:00
mcsauder 79d07c831f Convert spaces to tabs to match upstream. 2016-02-23 19:53:55 -07:00
mcsauder 6c96f45f08 Remove whitespace differences with upstream for pull request. 2016-02-23 19:49:27 -07:00
mcsauder fad1c87631 Merge upstream and resolve merge conflicts. 2016-02-23 19:29:30 -07:00
Roman Bapst 8eb63a150d Merge pull request #53 from bugobliterator/pr-sharedlib 
Shared Library build
 2016-02-23 08:30:46 +01:00
Paul Riseborough a711632017 EKF: Add method to fuse horizontal magnetometer data 
This method is more suitable than a raw heading measurement because it works across a full range of pitch angles.
It has been made the default for ground operation.
 2016-02-20 19:45:32 +11:00
mcsauder 4ce4724105 Added variable initializations back into EstimatorInterface() constructor to resolve a runtime error that occurs with uninitialized variables. 2016-02-19 23:57:06 -07:00
Paul Riseborough 6df6ac0023 EKF: Fix sign error in heading innovation calculation and clean up 2016-02-20 11:54:53 +11:00
Paul Riseborough 90e1bd3e36 EKF: wrap compass yaw estimate 2016-02-19 16:53:55 +11:00
Paul Riseborough 7d6226eb45 EKF: Improve efficiency of yaw angle fusion 
Use direct calculation of Kaman gains with optimised algebra
 2016-02-19 16:25:01 +11:00
Paul Riseborough 7f121e81e4 EKF: Update yaw innovation calculation to match revised derivation 
The new derivation does not use magnetic field measurements in the observation model and instead fuses in a heading measurement directly.
 2016-02-19 16:18:07 +11:00
mcsauder 72243c4a84 Resolve tab/space differences with upstream master. 2016-02-18 03:28:40 -07:00
mcsauder 437f6ca5fb Moved initialization to object constructors to allow C99 compiler compatibility. 2016-02-18 03:21:04 -07:00
bugobliterator ba7f0fc9ff EKF: add licensing information for mathlib header and src 2016-02-17 20:02:08 -08:00
bugobliterator 80632cc12c EKF: add comments and licensing information to builder files 2016-02-17 19:55:47 -08:00
bugobliterator a40eb7cf37 EKF: add check for existence of matrix submodule 2016-02-17 17:51:09 -08:00
bugobliterator 263c48d089 EKF: remove dependecies and allow ekf to be built as standalone shared lib 2016-02-17 17:33:18 -08:00
Paul Riseborough 96c0e18acb Merge pull request #49 from PX4/pr-fixMagCovariance 
EKF: Fix magnetometer fusion and covariance handling
 2016-02-18 10:22:36 +11:00
bugobliterator 2096e24c06 EKF: fix time of sample receive setting 2016-02-17 13:29:12 -08:00
Paul Riseborough a679cdf1fb EKF: Change parameter default to not fuse declination when aiding 
Ground and flight testing has shown declination drift to not be a problem with current tuning and realistic vehicle motion.
 2016-02-17 17:22:02 +11:00
Paul Riseborough abf9476853 EKF: miscellaneous formatting and typo updates 2016-02-16 11:08:30 +11:00
Paul Riseborough ffe1d30864 EKF: Set covariances to zero for un-used states 2016-02-16 11:08:30 +11:00
Paul Riseborough 294aca8609 EKF: Improve variance limiting for stationary states 
Turn off the process noise if they grow too large and use the hard variance limit as a backup. This is numerically more stable than relying on a hard variance limit which does not deal with the covariances.
 2016-02-16 11:08:30 +11:00
Paul Riseborough 1d40507af8 EKF: Set Kaman gains to zero for un-used states 
This is a defensive programming technique. In theory this should not be necessary if the corresponding covariance entries are zero.
 2016-02-16 11:08:30 +11:00
Paul Riseborough 69b8982043 EKF: Initialise variances to zero for un-used states 
Setting these variances to zero makes it less likely that these states will be modified by fusion processing.
 2016-02-16 11:08:30 +11:00
Paul Riseborough d9bf4e9870 EKF: Enable control mode transitions to be detected 
Save the previous value of the filter control modes
 2016-02-16 11:08:30 +11:00
Paul Riseborough 402206a305 EKF: Fix critical bug in fusion of yaw and declination observations 
This bug was in the derivation and resulted from use of a tan instead of an atan operator. The derivations have been reworked and the updated auto-code has been imported as part of this patch.
 2016-02-16 11:08:30 +11:00
Paul Riseborough 7da40a45a4 EKF: Add control mode for wind state estimation 2016-02-16 11:08:30 +11:00
Paul Riseborough de02aebafd EKF: Reset covariance matrix when doing a yaw and magnetic field reset 
The correlation terms in the covariance matrix will be incorrect after a reset, so should be zeroed
 2016-02-14 22:01:53 +01:00
Paul Riseborough dba58aa4c6 EKF: Add control logic for fusion modes 2016-02-14 22:01:53 +01:00
Paul Riseborough 22c177c951 EKF: Re-align yaw and magnetic field states when changing into GPS aiding mode 2016-02-14 22:01:53 +01:00
Paul Riseborough c089079321 EKF: Split tilt and yaw align 2016-02-14 22:01:53 +01:00
Paul Riseborough 020cc5978e EKF: Move calculation of declination into a separate function 2016-02-14 22:01:53 +01:00
Paul Riseborough aa58b3e98c EKF: Split angular alignment into tilt and yaw and use yaw and magnetic field alignment function 2016-02-14 22:01:53 +01:00
Paul Riseborough 2bbe7f9a1c EKF: Update magnetometer fusion to use declination from the specified source 2016-02-14 22:01:53 +01:00
Paul Riseborough 83945581ed EKF: Add function to calculate the magnetic declination to use 2016-02-14 22:01:53 +01:00
Paul Riseborough 9017e077f8 EKF: Publish the magnetic declination we wish to save for next startup 2016-02-14 22:01:53 +01:00
Paul Riseborough 0308f2f796 EKF: Add parameters to control declination source and fusion type 2016-02-14 22:01:53 +01:00
Paul Riseborough 6140d4b21f EKF: Add function to reset yaw and magnetic field states 2016-02-14 22:01:53 +01:00
Roman ad978c642f estimator interface: 
added flag for in_air status of vehicle
 2016-02-14 20:43:22 +01:00
Roman ce0ddc0207 -added comments 
-removed unused print functions
-removed false _imu_time_last variable (correct is _time_last_imu)
 2016-02-14 09:49:26 +01:00
Paul Riseborough 9d7340e187 EKF: Fix bugs in position and velocity resets 
The position reset was not being compensated for velocity and measurement delay
The height was being reset with the position. It has been moved into a separate reset function
The maximum accepted GPS delay of 100msec was inadequate
The states  was being incorrectly reset to the GPS position and Baro height on initial alignment.
 2016-02-14 09:49:26 +01:00
Paul Riseborough da1de2cc4d EKF: Correct for sensor noise and baro offset during alignment 2016-02-14 09:49:26 +01:00
CarlOlsson 1e766c7510 moved fuse function to ekf_helper.cpp 2016-02-12 15:54:32 +01:00
Paul Riseborough fff2bd50f6 EKF: Fix bugs in position and velocity resets 
The position reset was not being compensated for velocity and measurement delay
The height was being reset with the position. It has been moved into a separate reset function
The maximum accepted GPS delay of 100msec was inadequate
The states  was being incorrectly reset to the GPS position and Baro height on initial alignment.
 2016-02-10 15:16:05 +11:00
Paul Riseborough 40e174b81c EKF: Correct for sensor noise and baro offset during alignment 2016-02-10 10:25:27 +11:00
Paul Riseborough 5ea0ef39cd EKF: Update declination fusion logic 
We do not need to run declination fusion if 3-axis mag fusion is not being used.
 2016-02-09 07:54:24 +11:00
Paul Riseborough 47ab5ebcdd EKF: Make PR comply with project convention for indenting 2016-02-08 15:12:38 +11:00
Paul Riseborough cdc42c1de0 EKF: Add simple control logic for magnetic declination constraint 2016-02-08 14:18:43 +11:00
Paul Riseborough dbb8e12948 EKF: Add fusion method to constrain declination when unobservable 
When fusing 3-axis magnetometer data without absolute external aiding leg GPS), the declination is ultimately unobservable and the declination of the  field states and the vehicle heading will slowly drift over time. To prevent this we need to fuse in a declination to constraint the NE earth field estimates.
 2016-02-08 14:18:43 +11:00
Paul Riseborough 9347afe250 EKF: Use unique variable name for mag heading fusion observation Jacobian 2016-02-08 14:18:43 +11:00
Paul Riseborough 6aeccf01ff EKF: Add missing fusion health status messages 2016-02-08 14:18:43 +11:00
Paul Riseborough 2c8c6efb45 EKF: Eliminate use of power function to square numbers 
Use of the power function to square a number can be very inefficient depending on processor and compiler. Replace with an inlined function  that multiplies the number by itself. This has not been put into the maths library because the use of the sq() function is peculiar to the autocode generated for the EKF and an inlined function was desired.
 2016-02-08 11:11:59 +11:00
Paul Riseborough 3f81eb7d1b EKF: fix formatting 2016-02-04 17:15:03 +11:00
Paul Riseborough 9bfc11b660 EKF: Use specific position observation noise when flying without GPS 
A larger position uncertainty is required when flying without GPS to reduce tilt attitude estimation errors caused by vehicle manoeuvring. This needs to be tuneable to allow optimisation for different use cases (e.g. outdoor vs indoor).
 2016-02-03 18:40:32 +11:00
Paul Riseborough 7de1d39ce4 EKF: Improve use of position and velocity observation noise parameters 
Fix bug where noise parameters were not being squared when calculating the observation variance.
Use GPS reported speed accuracy to set velocity observation noise and use the parameter to set the minimum.
 2016-02-03 18:15:38 +11:00
Paul Riseborough 2b1e8fe910 EKF: Update tuning parameters 
Set conservative defaults as a baseline for tuning
Add a missing parameter for magnetometer observation noise.
Correct error in definition of magnetic heading observations noise (previous parameter defined the variance directly, not the noise).
Scale vertical GPS noise to allow for average EPV to EPH ratio.
Update documentation and display names for consistency.
 2016-02-03 17:05:45 +11:00
Lorenz Meier b21a49b3f9 EKF: Use standard C++ initializers 2016-01-31 23:24:36 +01:00
Roman b749a7557b fixed typos 2016-01-31 21:36:58 +01:00
bugobliterator d79e12dfa1 EKF: fix code style 2016-01-31 00:12:16 -08:00
bugobliterator 8200ef4d17 EKF: allocate and unallocate buffer more robustly 2016-01-31 00:12:16 -08:00
bugobliterator c6249a2825 EKF: change estimator base class name to estimation interface 2016-01-31 00:12:07 -08:00
bugobliterator 3cafedd4c1 EKF: start storing samples only when ekf is initialised 2016-01-30 12:46:01 -08:00
bugobliterator b062bd2e38 EKF: move estimator specific variables to estimator core 2016-01-30 12:45:23 -08:00
bugobliterator 287f5eb0c2 EKF: move all estimator specific types to common header 2016-01-30 12:44:19 -08:00
bugobliterator b05e2d825c EKF: move estimator specific initialisations to estimator core 2016-01-30 12:42:24 -08:00
Paul Riseborough f1b82057c0 EKF: Combine GPS velocity innovation gate parameters 
Separate vertical and horizontal parameters for GPS velocity innovation gates are not required
 2016-01-30 12:21:50 -08:00
Paul Riseborough 466a104534 EKF: additional GPS check logic 
Improve naming of NED origin initialisation status
Add check for GPS solution type
Use GPS checks when regaining GPs in-flight
 2016-01-30 12:21:39 -08:00
bugobliterator a779d45cd0 ekf2: fix bug causing faulty imu data storage 2016-01-29 23:56:24 -08:00
bugobliterator 92dc41e885 EKF: fix typo in function declaration 2016-01-29 20:05:07 -08:00
bugobliterator 3eea6cdcc5 EKF: use namespace for structure definitions 
will be moved to a separate file in the future
 2016-01-29 20:04:21 -08:00
bugobliterator 001d621eb1 EKF: move imu data processing code to estimator core 2016-01-29 20:03:25 -08:00
bugobliterator 251996d387 ekf: move gps checks to Ekf library from estimator_base 2016-01-29 17:15:18 -08:00
Paul Riseborough 46b0e9654c Add filter control logic 2016-01-30 09:01:48 +11:00
Paul Riseborough 9236f11c80 EKF: Add position and velocity innovation consistency checks 2016-01-28 16:33:54 +11:00
Paul Riseborough 7713cc8c20 Merge pull request #17 from priseborough/pr-addMagErrHandling 
EKF: Add magnetometer fusion error handling
 2016-01-28 13:43:57 +11:00
Lorenz Meier f4b2c0eecf Merge pull request #11 from mcharleb/PRIu64-fix 
Fixed printf of uint64_t
 2016-01-28 01:01:10 +01:00
Paul Riseborough 802129f384 EKF: Add magnetometer fusion error handling 2016-01-28 09:53:55 +11:00
Paul Riseborough 39eef3a2d7 EKF: Remove use of camel case variable names 
Also fixes bug in GPS speed accuracy check that was using horizontal position accuracy variable by mistake.
 2016-01-28 08:37:45 +11:00
Paul Riseborough cc50d26601 EKF: Update comments 2016-01-28 08:13:38 +11:00
Paul Riseborough 4526cb9c4f EKF: remove redundant variable 2016-01-28 07:50:45 +11:00
Paul Riseborough 6326433c47 EKF: Eliminate call to hrt_absolute_time() 
This will make the library more portable
 2016-01-27 19:04:31 +11:00
Paul Riseborough 94a6644684 Add comprehensive user selectable GPS checks 2016-01-25 13:07:57 +11:00
Roman 8d0022ab1e enable estimator state and innovations data logging 2016-01-22 11:31:14 +01:00
Roman 86df68e404 added interface for parameters 2016-01-22 10:45:03 +01:00
nickolasrossi dfbbd36ff7 fix buffer overflow in constrain of wind velocity 2016-01-19 18:25:22 -08:00
Mark Charlebois 79bf35c4b3 Fixed printf of uint64_t 
Must use PRIu64 to prevent compiler errors.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
 2016-01-18 22:19:10 -08:00
Roman 0b5c90574c fix delta angle bias usage: 
the delta angle bias was applied to imu data which was not coming in the same
time intervall as the filter was operating. Therefore, the delta angle bias
applied to new imu data had to be scaled correctly in order to match
the imu time interval.
 2016-01-14 17:06:02 +01:00
Roman 2af5856361 initialise output height with baro 2016-01-08 06:59:10 +01:00
Roman a41f75ffb1 use full mag fusion 2016-01-08 06:59:10 +01:00
Roman f8354bb5e9 - do not fake vertical gps measurement as we have baro 
- formatting

Conflicts:
	EKF/ekf.cpp
 2016-01-08 06:59:10 +01:00
Roman 2a88fc6cfd initialise vertical position correctly 2016-01-08 06:42:22 +01:00
Roman 5aa5f7f8c3 use correct timestamp for measurements 2016-01-08 06:42:03 +01:00
Mark Charlebois b8837b4dc6 Added hexagon support 
std::to_string is not supported in the Hexagon complier

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
 2016-01-07 14:54:55 -08:00
Paul Riseborough edfb7aefcc EKF: Fix initial alignment errors 
Use the gravity vector to estimate the initial roll and pitch angle
Use the projection of the magnetic field measurement onto the earth axis horizontal plane to calculate the initial heading
 2016-01-03 00:03:03 +11:00
Lorenz Meier 52cb9d02c7 EKF: Initialize all local structs and variables 2016-01-01 12:31:36 +01:00
Paul Riseborough 0cb210d045 EKF: Changes required to enter POSCTL mode 2016-01-01 11:56:38 +11:00
Lorenz Meier eda69e727f Do not print states by default 2015-12-24 13:48:22 +01:00
Roman 457a57c6ff use simple heading fusion 2015-12-22 18:00:59 +01:00
Lorenz Meier 6b49e2495c Fix field initializers 2015-12-22 17:25:08 +01:00
Lorenz Meier 8894786752 Estimator base: Fix printf formatting 2015-12-22 17:24:58 +01:00
Lorenz Meier faf48242ed EKF: Fix ringbuffer warnings 2015-12-22 17:24:43 +01:00
Roman 5e5d6f432a added static gps mode and init mag state correctly 2015-12-22 11:22:17 +01:00
Roman f153a7cb44 added ifdef guard 2015-12-19 09:40:32 +01:00
Roman Bapst d233ca3990 added complementary filter for real time state estimation 2015-12-10 16:36:10 +01:00
Roman Bapst 67646a15b0 added full mag fusion 2015-12-10 09:07:00 +01:00
Roman 8de8b0eb76 prediction and vel pos heading fusion working 2015-12-09 17:47:52 +01:00
Roman 921df43d32 added tests for EKF ringbuffer 2015-12-07 22:41:11 +01:00
Roman d90c8fd7c2 fixed bug in covariance prediction 2015-12-07 22:41:11 +01:00
Roman cfc39bc2f9 implemented prediction of states and covariance matrix 2015-12-07 22:41:11 +01:00
Roman b52e38eb9d fix 2015-12-07 22:41:11 +01:00
Roman cd5db8f494 enhance ringbuffer 2015-12-06 17:07:44 +01:00
Roman b3491bcfc3 added file with ekf helper functions 2015-12-06 13:19:25 +01:00
Roman 859502753d added file with covariance calculations 2015-12-06 13:10:49 +01:00
Roman Bapst 144aa9c461 added base class for data storage 2015-12-05 11:08:06 +01:00