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

263 Commits

Author SHA1 Message Date
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