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

151 Commits

Author SHA1 Message Date
Paul Riseborough f064915889 EKF: Enable planes to recover from bad mag data at start of flight 
Adjusts yaw by the amount of the error between GPS and EKF course if innovations are large.
 2017-07-08 11:13:40 +02:00
Paul Riseborough eb1e73ec81 Merge pull request #283 from PX4/ekf_matrix_cleanup 
EKF matrix typedef cleanup
 2017-07-03 21:02:24 +10:00
Daniel Agar c44488fdb8
EKF matrix typedef cleanup 2017-06-19 11:10:01 -04:00
Roman 1b92c9b5b3 ekf_helper: fixed resetting height using range finder 
- take range sensor offset wrt to IMU into account
- use projection to earth Z axis

Signed-off-by: Roman <bapstroman@gmail.com>
 2017-06-15 09:10:43 +02:00
Roman a1e67396f4 ekf: added methods for setting control height flags 
Signed-off-by: Roman <bapstroman@gmail.com>
 2017-06-15 09:10:43 +02:00
Paul Riseborough 394dd95cba EKF: Don't do magnetic field estimation without earth frame measurements 
Use horizontal acceleration to check if yaw is observable independent of the magnetometer.
Use rotation about the vertical to check if mag raises are observable.
If neither yaw of mag biases are observable, save the magnetic field variances and switch to magnetic yaw fusion.
Use the last learned declination when using magnetic yaw fusion so that the yaw reference remains consistent.
When yaw or biases become observable, reinstate the saved variances and switch back to 3D mag fusion.
 2017-05-17 08:59:01 +02:00
Paul Riseborough 20db74eca8 Revert "EKF: Add parameter control of individual IMU axis delta velocity bias estimation" 
This reverts commit 9c31632e2b.
 2017-05-15 16:38:17 +10:00
Paul Riseborough 9c31632e2b EKF: Add parameter control of individual IMU axis delta velocity bias estimation 
Add a bitmask parameter to control bias learning for individual axes. This is achieved by setting the disabled states to zero together with their corresponding covariances.
Minor cleanup of the covariance prediction comments.
Removal of unnecessary variable copy operations.
Replace index operations to initialise covariance to zero with the more efficient memset.
 2017-05-11 07:51:37 +02:00
Daniel Agar 3919d60f66
ekf2 initialization fixes 2017-04-24 18:56:50 -04:00
Paul Riseborough f0bbbc8dc6 EKF: Add reporting of bad accel status 2017-04-07 08:47:13 +10:00
Paul Riseborough 040639837e EKF: Use common timeout test for reporting deadreckoning 
Use a parameter to set the time-out threshold
Use a class variable for the time-out flag
Clean up comments
 2017-03-15 07:57:38 +01:00
Paul Riseborough 6ac7e2f807 EKF: report conservative position errors when failing innovation checks 2017-03-15 07:57:38 +01:00
Paul Riseborough 21e7734149 EKF: fix compile error 2017-03-14 13:00:54 +01:00
Paul Riseborough f7cae7f3ab EKF: improve compatibility with ARM cross compiler 
This change removes the following compiler error when building using the ARM cross compiler.

/Users/paul/src/Firmware/src/lib/ecl/EKF/ekf_helper.cpp:45:12: error: 'std::abs' has not been declared
 using std::abs;
 2017-03-12 22:55:23 +01:00
Paul Riseborough cf31945038 EKF: Fix use of incorrect timestamp 
This was incorrectly using the IMU (1/250 sec) timestamp instead of the EKF (1/100 sec) value.
The corresponding accelerometer limit has been made a parameter and adjusted to match previous behaviour.
 2017-03-10 10:14:06 +01:00
Paul Riseborough e4a4427991 Revert "EKF: fix compile error" 
This reverts commit 6db552717d.
 2017-03-10 07:38:02 +11:00
Paul Riseborough 6db552717d EKF: fix compile error 2017-03-07 21:13:45 +11:00
Daniel Agar eb4f5ecbb9 cmake add all EKF sources (#243) 
- trivial cmake cleanup
 - travis-ci build with both gcc and clang
 - update matrix to latest (matches PX4)
 - closes #240
 2017-02-27 00:46:48 -05:00
Paul Riseborough 0d77470efc EKF: Use conservative reporting of velocity errors when aiding is lost 2017-02-25 21:17:01 +01:00
Paul Riseborough 8070691aa4 EKF: Add accessor function for velocity accuracy 2017-02-25 21:17:01 +01:00
Paul Riseborough d6abf3f2e4 EKF: add accessor function for local position accuracy 2017-02-25 21:17:01 +01:00
Paul Riseborough b7d0b3c4d0 EKF: Improve naming of WGS-84 accuracy accessor function 2017-02-25 21:17:01 +01:00
Paul Riseborough d22599b440 EKF: Add reporting of inertial dead-reckoning status 2017-02-25 21:17:01 +01:00
Daniel Agar 2750961be6 c++11 initialization cleanup (#237) 2017-02-05 13:05:10 -05:00
Daniel Agar 47b8ef258b clang-tidy performance-type-promotion-in-math-fn (#236) 2017-02-02 00:17:08 -05:00
Daniel Agar d668a2d798 clang-tidy performance-unnecessary-value-param 2017-01-14 16:26:25 -08:00
Paul Riseborough 934890c2df EKF: Add validity flag to ekf origin reporting 
Flag is false until the origin has been initialised
 2016-12-28 00:56:09 +01:00
CarlOlsson c8e3c24150 adopted ekf_helper.cpp 2016-11-05 12:19:01 +01:00
Paul Riseborough b10b0184c5 EKF: Add IMU vibration calculation and reporting 
Add calculation and reporting of IMU delta angle and velocity coning and high frequency vibration
 2016-10-19 21:46:55 +11:00
Roman 17c195d1cd ekf2 height reset: do not apply height change to height state twice (bug) 
Signed-off-by: Roman <bapstroman@gmail.com>
 2016-10-18 07:58:19 +02:00
Roman f80b3481a4 reset the newest output sample for all possible resets 
Signed-off-by: Roman <bapstroman@gmail.com>
 2016-10-12 08:54:38 +02:00
Roman 238d947239 proper handling of position and velocity reset 
- position reset method was returning before the actual delta values
were written and applied to the output buffer
- apply reset delta also to the output sample which was already taken out
from the output buffer, otherwise the complementary filter solution is
offset from the ekf solution

Signed-off-by: Roman <bapstroman@gmail.com>
 2016-10-12 08:54:38 +02:00
Paul Riseborough 352750e5d1 EKF: publish ekf solution status summary data 2016-10-06 09:02:29 +02:00
Paul Riseborough 007e73379f EKF: Improve reporting of position error 2016-10-06 09:02:29 +02:00
Paul Riseborough 744b79c1b2 EKF: Publish innovation test ratios 2016-10-06 09:02:29 +02:00
Paul Riseborough 65762e5998 EKF: publish output predictor tracking errors 2016-10-03 14:13:07 +11:00
Julian Oes 616aeb9ad7 ekf_helper: fix forgotten return value 2016-09-27 16:36:29 +02:00
Beat Küng 73b8ac954d ekf_helper.cpp: remove unused includes 
This fixes a compile error with GCC 5.2.0 and cross-compilation:
/usr/arm-none-eabi/include/c++/5.2.0/arm-none-eabi/armv7e-m/fpu/bits/error_constants.h:122:27: error: 'ENOTSUP' was not declared in this scope
       not_supported =     ENOTSUP,

The problem was that the #include <iomanip> was taken from the host system,
which conflicts with includes from NuttX (NuttX has only basic C++ STL
support)
 2016-07-15 12:50:11 +02:00
Paul Riseborough f5e9ea1626 EKF: Prevent potential divide by zero 2016-06-30 17:06:32 +10:00
Paul Riseborough 7bc9217f00 EKF: Clean up control of observation fusion 
All the decision for a sensor are made within a specific function for that sensor and when there is data to process at the fusion time horizon.
Information and warning messages are improved.
 2016-06-30 17:06:32 +10:00
Roman Bapst bec1a6831e added method to get gyro bias 
Signed-off-by: Roman Bapst <roman@px4.io>
 2016-06-21 14:13:47 +02:00
Roman 703fb4e31b do not return early in velocity reset method otherwise 
some code will not be executed
 2016-06-08 07:51:18 +02:00
Paul Riseborough f26aca55a1 EKF: Fix bug in quaternion covariance initialisation function 
The covariances between quaternion and non-quaternion states were not being updated
 2016-06-08 13:23:30 +10:00
Paul Riseborough 6a55d908c5 EKF: replace reset event times with event counters 
Using a 64bit integer was unnecessary given it was only being used to detect a new reset event.
 2016-06-01 17:13:00 +10:00
Paul Riseborough 9ec09f5f4e EKF: update output observer and capture reset event for magnetic heading resets 2016-06-01 17:13:00 +10:00
Paul Riseborough aca0336392 EKF: update vertical position and velocity reset capture 
Use reset event struct members instead of separate variables
 2016-06-01 17:13:00 +10:00
Paul Riseborough 0605c88ecc EKF: capture horizontal position reset events 2016-06-01 17:13:00 +10:00
Paul Riseborough e3a1b4a3b3 EKF: capture velocity reset events 2016-06-01 17:13:00 +10:00
Paul Riseborough 79705da7e6 EKF: make output predictor states consistent with position reset 2016-06-01 17:13:00 +10:00
Paul Riseborough 54d90261d5 EKF: make output predictor states consistent with velocity reset 2016-06-01 17:13:00 +10:00
Paul Riseborough ad02818b3d EKF: Improve height reset for external vision 
reset to measurement closest in time to fusion time horizon
 2016-05-27 14:00:50 +10:00
Paul Riseborough 1b6c5bbafd EKF: Enable height source to be selected independent of EV aiding 2016-05-27 13:14:52 +10:00
Paul Riseborough ae5071f668 EKF: Improve heading reset 
Improves reset of quaternion covariance matrix after a heading reset by preserving variance in roll and pitch and resetting yaw variance to the measurement variance.
 2016-05-21 19:30:15 +10:00
Paul Riseborough 94a63ec9d5 EKF: don't interfere with quaternion covariances during tilt alignment 2016-05-18 19:25:19 +10:00
Paul Riseborough e4b2e9c93d EKF: Improve yaw alignment 
Uses best conditioned of 321 or 312 Euler sequence to calculate initial yaw angle.
Allows alignment of yaw angle using external vision data
 2016-05-18 12:16:16 +10:00
Paul Riseborough 57b2a256f7 EKF: Improve initialisation of quaternion covariances 
Convert uncertainty in initial rotate vector into quaternion covariances using symbolic toolbox derived expressions.
Enable setting of initial angle uncertainty via a parameter
 2016-05-17 12:47:20 +10:00
Paul Riseborough 1540e937b1 EKF: Improve tilt alignment monitoring 
Convert quaternion covariances into an angular alignment variance vector and discard the z component so that yaw uncertainty does not affect the result.
 2016-05-17 11:17:24 +10:00
Paul Riseborough 81469d6621 EKF: Add position, height and velocity reset for EV aiding 2016-05-14 21:17:29 +10:00
Paul Riseborough 25682dce91 EKF: Prevent badly conditioned covariance calculation when starting or resetting to optical flow 2016-05-08 10:30:40 +10:00
Paul Riseborough 7f5669fb2d EKF: consolidate covariance corrections 
Combines the forced symmetry, variance limiting and zeroing of covariances for unwanted states in the one function.
This ensures a consistent correction is applied after every covariance prediction or correction.
 2016-05-07 20:29:50 +10:00
Paul Riseborough da9d894441 EKF: Improvements to covariance reset 2016-05-07 12:57:41 +10:00
Paul Riseborough fe9f88a8b4 EKF: test new derivation 
Use direct attitude parameterisation
Discard scale factors
Add accel bias
 2016-05-05 21:23:03 +10:00
Roman 0c77ebf6bd EKF: Reduce likelihood of bad covariance values after height resets 
The cross terms (covariances) are now reset before the variance variance is set.

The vertical velocity state variance following a reset without GPS has been reduced.
 2016-05-01 21:39:56 +10:00
Paul Riseborough 469a7d1711 EKF: rework height reset 
calculate and save the amount that the vertical states have changed and the time of the change
apply the change delta to the output observer states and buffer
 2016-04-22 08:43:56 +10:00
Paul Riseborough 2fa8a11a29 EKF: replace unnecessary memcopy 2016-04-19 19:10:41 +10:00
Paul Riseborough 31bf342fc1 EKF: publish GPS check status 2016-04-19 17:38:22 +10:00
Paul Riseborough e10093854a EKF: correct outputs for IMU offset 2016-04-12 11:14:31 +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
CarlOlsson f8878d41dc adopted ekf_helper.cpp 2016-03-30 17:01:03 +02:00
Paul Riseborough 6b2e2dba90 EKF: Add GPS height option and improve height recovery 2016-03-16 10:40: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 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 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 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 78d6f6941c Merge pull request #61 from PX4/pr-ImprovedAccuracyReporting 
EKF: Improve Position Accuracy Reporting
 2016-02-26 22:48:44 +11: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 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
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 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 83945581ed EKF: Add function to calculate the magnetic declination to use 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 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
bugobliterator d79e12dfa1 EKF: fix code style 2016-01-31 00:12:16 -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
Roman 8d0022ab1e enable estimator state and innovations data logging 2016-01-22 11:31:14 +01:00
nickolasrossi dfbbd36ff7 fix buffer overflow in constrain of wind velocity 2016-01-19 18:25:22 -08:00
Roman 2a88fc6cfd initialise vertical position correctly 2016-01-08 06:42:22 +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
Roman f153a7cb44 added ifdef guard 2015-12-19 09:40:32 +01:00
Roman 8de8b0eb76 prediction and vel pos heading fusion working 2015-12-09 17:47:52 +01:00