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

539 Commits

Author SHA1 Message Date
Paul Riseborough 920d83d68c EKF: Fix bugs preventing use of external vision yaw data 2016-05-14 21:56:41 +10:00
Paul Riseborough 13c3a95bc1 EKF: Add missing line returns 2016-05-14 21:17:30 +10:00
Paul Riseborough 59eb9eb3db EKF: fix shadow declaration arising from rebase 2016-05-14 21:17:30 +10:00
Paul Riseborough b985e58333 EKF: clean up control function 
With the addition of new observation types, the control function has become too large and needed be broken up into separate functions
 2016-05-14 21:17:29 +10:00
Paul Riseborough f4a0f69f6e EKF: print to console when starting EV fusion 2016-05-14 21:17:29 +10:00
Paul Riseborough 349c731375 EKF: remove PX4 dependant text output 2016-05-14 21:17:29 +10:00
Paul Riseborough ac9b7a3df6 EKF: Ensure use of EV aiding inhibits use of other height sources 2016-05-14 21:17:29 +10:00
devbharat cf1df5861a Undid hack in ecl. Handled yaw missalignment on the vision side. 2016-05-14 21:17:29 +10:00
devbharat d16f413b55 Hack to fix external vision pos offset compensation 2016-05-14 21:17:29 +10:00
devbharat d3bad9fdb0 Correct height fusion flag when using sensor other than baro 2016-05-14 21:17:29 +10:00
devbharat 6d20a426e0 Reset time_last_ext_vision 2016-05-14 21:17:29 +10:00
devbharat b681c9a5d0 Added external vision noise parameters etc and position offset 2016-05-14 21:17:29 +10:00
devbharat ff8f03b5dd Added compensation for VI sensor offset. Check sign. 2016-05-14 21:17:29 +10:00
Paul Riseborough e917d6c7f2 EKF: Add fusion of external yaw data 2016-05-14 21:17:29 +10:00
Paul Riseborough 37a09c61bc EKF: Don't use delayed data to start EV aiding 2016-05-14 21:17:29 +10:00
Paul Riseborough 25f1d1d766 EKF: Add fusion of external vision 3D pos data 2016-05-14 21:17:29 +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 3a0fcd03d7 EKF: Add interfaces and variables to use ext vision data 2016-05-14 21:17:29 +10:00
Paul Riseborough 172f4be594 EKF: Fix bug in calculation of terrain observation variance 2016-05-14 21:17:29 +10:00
Paul Riseborough c7e225124c EKF: Improve output observer position and velocity tracking 
Replace the delayed time feedback mechanism used by the translational states with a direct feedback method.
Time constants for velocity and position convergence can be separately adjusted with tunable parameters
The method is more computationally more expensive because it requires modification of the output buffer history but is acceptable because it only requires 6 FLOP per buffer index for a total of 30*6 = 180 FLOP
The method was not applied to the attitude states because the quaternion operations required at each buffer index would have been computationally prohibitive.
 2016-05-14 12:45:29 +09:30
Paul Riseborough 627d08ecc9 Revert "EKF: Fix bug in output observer" 
This reverts commit 03f3df7415.
 2016-05-14 12:26:46 +09:30
Paul Riseborough 03f3df7415 EKF: Fix bug in output observer 
Bug was causing output observer state history to be stored at the the EKF, not IMU output rate and resulted in degraded tracking performance
 2016-05-14 08:33:05 +09:30
Lorenz Meier d595596c42 Merge pull request #114 from PX4/fix_linux_build 
CMakeLists: use find_package Eigen3
 2016-05-12 12:55:40 +02:00
Carl Olsson 5217b73df1 added bad_airspeed reporting (#123) 2016-05-12 10:06:04 +02:00
Carl Olsson b126921894 changed airspeed innovation gate to 5 (#119) 2016-05-12 10:05:26 +02:00
Julian Oes b8ccc58887 cmake: ignore more eigen warnings 2016-05-12 08:38:37 +02:00
Julian Oes b74114a573 cmake: ignore deprecated warnings of Eigen 2016-05-12 08:27:59 +02:00
Julian Oes 07ecffc963 cmake: another try to get the module path right 2016-05-10 17:59:01 +02:00
Julian Oes 001433c660 cmake: debug output for path 2016-05-10 17:59:01 +02:00
Julian Oes d242bb78b9 cmake: try to provide path to FindEigen3 2016-05-10 17:59:01 +02:00
Julian Oes dcd191d734 cmake: include FindEIgen3.cmake for older cmake 2016-05-10 17:59:01 +02:00
Julian Oes 3d28f7ecca CMakeLists: use find_package Eigen3 
Instead of hardcoding the Eigen3 path which only works on Mac, use
find_package(Eigen3).
 2016-05-10 17:59:01 +02:00
Paul Riseborough 1a24ec5f50 EKF: remove unused flag from control status message 2016-05-10 10:45:06 +10:00
Paul Riseborough 481c624975 EKF: Remove use of vehicle arm status 
Use single externally set in-air status for all decisions
 2016-05-10 10:23:08 +10:00
Paul Riseborough 3cdf56e57c Merge pull request #122 from CarlOlsson/airspeed_fix 
fixed_airspeed_healthy_bug
 2016-05-09 10:24:22 +10:00
CarlOlsson ee7d7aeb8a fixed_airspeed_healthy_bug 2016-05-08 18:30:37 +02:00
Paul Riseborough 175723f690 EKF: Allow for larger accel bias offsets on startup 2016-05-08 18:03:58 +10:00
Paul Riseborough 161ac2e051 EKF: ensure position co-variances are reset 
When transitioning into optical flow nav  from a non-aiding condition during ground operation, ensure position covariances are reset as they could be invalid.
 2016-05-08 18:03:09 +10:00
Paul Riseborough feb4db6347 EKF: protect against large IMU errors during initial tilt alignment 
Turn off innovation consistency checks for height and synthetic position measurements until the tilt as aligned
 2016-05-08 17:54:45 +10:00
Paul Riseborough 00973d6215 EKF: fix pos/vel fusion innovation variance bug 2016-05-08 17:51:47 +10:00
Paul Riseborough 2144a40a3c EKF: redefine accelerometer bias state inhibit switch bit location 2016-05-08 16:03:21 +10:00
Paul Riseborough 724280fd1f EKF: move calculation of optical flow observation variance into a function 
Allows it to be used when calculating initial state variance
 2016-05-08 15:40:06 +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 c66ed7b662 EKF: Add interface method for filter fault status 2016-05-07 21:18:00 +10:00
Paul Riseborough 020b87933e EKF: replace fault status struct with a union to facilitate logging 2016-05-07 21:11:16 +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 0c6a367e1b EKF: fix Travis build error 2016-05-07 17:45:49 +10:00
Paul Riseborough 22f0f52bf1 EKF: fix travis build error 2016-05-07 17:06:45 +10:00
Paul Riseborough 9e53ff2f80 EKF: Improve protection against bad optical flow fusion 2016-05-07 14:23:56 +10:00
Paul Riseborough 1126048a93 EKF: Improve protection against bad airspeed fusion 2016-05-07 14:23:38 +10:00
Paul Riseborough c8d95586e7 EKF: Improve protection against bad magnetometer fusion 2016-05-07 14:23:20 +10:00
Paul Riseborough 310bd97080 EKF: Improve protection against bad pos vel fusion 2016-05-07 14:22:59 +10:00
Paul Riseborough da9d894441 EKF: Improvements to covariance reset 2016-05-07 12:57:41 +10:00
Paul Riseborough d9aeace6ac EKF: tuning update 2016-05-05 21:23:03 +10:00
Paul Riseborough f1b7e7714e EKF: Make average update rate of EKF closer to target 
With the EKF, the average update rate is more important than the instantaneous value as it affects tuning. This patch ensures that the EKF prediction cycle will be performed early if the previous one was late in an attempt to maintain the target update rate.
 2016-05-05 21:23:03 +10:00
Paul Riseborough 1daa6c7866 EKF: update tuning defaults 2016-05-05 21:23:03 +10:00
Paul Riseborough fc6c0c7ebf EKF: Update covariance prediction 
Refresh auto generated code
Make 3-axis accel fusion the default and no accel fusion the option
 2016-05-05 21:23:03 +10:00
Paul Riseborough ecaffe63af EKF: update tuning defaults 2016-05-05 21:23:03 +10:00
Paul Riseborough 7a74d58591 EKF: reduce initial accel bias uncertainty 2016-05-05 21:23:03 +10:00
Paul Riseborough a7d08d08fc EKF: Relax tilt alignment check 2016-05-05 21:23:03 +10:00
Paul Riseborough 5523a4f225 EKF: Fix IMU bias compensation scale error in output filter 
Delta angle and velocities are calculated assuming data is at the filter update rate, not the IMU update rate
 2016-05-05 21:23:03 +10:00
Paul Riseborough fd109b00ab EKF: replace in-line code with existing function 2016-05-05 21:23:03 +10:00
Paul Riseborough 727a43764f EKF: update initial angle alignment check 2016-05-05 21:23:03 +10:00
Paul Riseborough bc34b14779 EKF: Initialise height state variance to a value compatible with the measurement 
A large height state variance and small measurement variance can destabilise the filter in the first few seconds after alignment
 2016-05-05 21:23:03 +10:00
Paul Riseborough 29a361d3a3 EKF: Use intuitive units for gyro and accel bias process noise 
This also makes the tuning less sensitive to changes in the EKF update rate.
 2016-05-05 21:23:03 +10:00
Paul Riseborough 799865c934 EKF: update default tuning parameters 2016-05-05 21:23:03 +10:00
Paul Riseborough f01f4ae4b1 EKF: tidy up mag declination fusion 2016-05-05 21:23:03 +10:00
Paul Riseborough 0d31aad33a EKF: Fix bad limit on magnetometer noise parameter 2016-05-05 21:23:03 +10:00
Paul Riseborough 7069fb7a04 EKF: refresh auto code for mag fusion 2016-05-05 21:23:03 +10:00
Paul Riseborough ffea65e1a4 EKF: fix bug in state prediction 2016-05-05 21:23:03 +10:00
Paul Riseborough 63f093a10a EKF: Update upper limits for state variances 
This limiting is a  last resort measure and the limiting for normal operation should be achieved by modification in the covariance prediction
 2016-05-05 21:23:03 +10:00
Paul Riseborough 26a51c667d EKF: Improve robustness to initial gyro bias errors 
Allows for ~6 deg/sec of startup bias
 2016-05-05 21:23:03 +10:00
Paul Riseborough 7530e30012 EKF: Improve IMU data collection 
Use IMU driver published delta angle integration time to determine when enough data has been accumulated.
Perform basic first order sculling corrections on delta velocity data
Comment steps to make make method clearer
 2016-05-05 21:23:03 +10:00
Paul Riseborough f23f0b1035 EKF: fix bugs in stationary process model covariance prediction 
Noise variance for stationary states was  being overwritten by the covariance prediction operations for the kinematic states
Unwanted states could end up with non-zero covariance values
The forced symmetry operation was being applied too late to be effective
 2016-05-05 21:23:03 +10:00
Paul Riseborough a373ada858 EKF: explicitly define floating point type in hardcoded constants 2016-05-05 21:23:03 +10:00
Paul Riseborough f4e84a2234 EKF: fix axis label error in comments 2016-05-05 21:23:03 +10:00
Paul Riseborough 82da832816 EKF: tidy up Kalman gain calculations 
Inhibiting of states is controlled via zeroing rows and columns in the covariance prediction so conditional logic in gain calculations is unnecessary.
 2016-05-05 21:23:03 +10:00
Paul Riseborough 311d046a26 EKF: Enable optional activation of XY delta velocity bias estimation 2016-05-05 21:23:03 +10:00
Paul Riseborough 572c2280bd EKF: tidy up covariance prediction 2016-05-05 21:23:03 +10:00
Paul Riseborough 54e713969d EKF: Fix rebase error 2016-05-05 21:23:03 +10:00
Paul Riseborough 67d23948b5 EKF: preliminary re-tune 2016-05-05 21:23:03 +10:00
Paul Riseborough 03c35266ef EKF: remove unused parameter 2016-05-05 21:23:03 +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
Daniel Agar 22d18d638c enable Wshadow 2016-05-04 19:45:40 -04:00
Beat Küng f2dda3c183 ekf: initialize _terrain_var, fixes access to uninitialized value 
in PX4 Firmware, Ekf::get_terrain_vert_pos depends on it. This is the
valgrind output:
==15439== Thread 14 ekf2:
==15439== Conditional jump or move depends on uninitialised value(s)
==15439==    at 0x5610087: sqrtf (in /usr/lib64/libm-2.22.so)
==15439==    by 0x4D1AF0: Ekf::get_terrain_vert_pos(float*) (terrain_estimator.cpp:135)
==15439==    by 0x46641F: Ekf2::task_main() (ekf2_main.cpp:655)
==15439==    by 0x42BF3D: entry_adapter(void*) (px4_posix_tasks.cpp:103)
==15439==    by 0x4E3C609: start_thread (in /usr/lib64/libpthread-2.22.so)
==15439==    by 0x5BF7A4C: clone (in /usr/lib64/libc-2.22.so)
 2016-05-02 17:35:42 +02: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 6c57bea1c3 EKF: fix bug in IMU error application timing patch 2016-04-30 15:11:12 +10:00
Paul Riseborough 15df20831a EKF: improve output complementary filter tracking 
Optimise for height tracking and adjust gains automatically with changes in time delay.
 2016-04-30 11:03:45 +10:00
Paul Riseborough 4fa1e9c651 EKF: Fix timing errors in state prediction 
IMU corrections were being applied at the wrong time horizon to the EKF states
IMU downsampling rotates the delta velocities into the orientation produced by the last delta angle, but this wasn't consistent with the way the state prediction was using the delta velocity data.
 2016-04-30 11:02:19 +10:00
Julian Oes f8a48f9cc5 ekf: now it should even compile for NuttX 2016-04-28 16:30:49 +02:00
Julian Oes 12f4d6f703 ekf: another stab at fixing isfinite for all 
The previous solution did not work for Snapdragon, so I needed to copy
what is used in px4_defines.h.
 2016-04-28 16:09:12 +02:00
Paul Riseborough 22ad87599c Merge pull request #112 from PX4/fix_ubuntu1604_2 
ekf: compile fix for Ubuntu 16.04
 2016-04-27 21:21:04 +10:00
Julian Oes 46e207106b ekf: use `std::isfinite` instead of `isnan` 2016-04-27 12:58:22 +02:00
Paul Riseborough 4fcbfb5d42 EKF: correct error in comment 2016-04-23 07:55:25 +10:00
Paul Riseborough 3e81b86280 EKF: improve height reset logic 
Don't use failed sensors unless necessary and handle case where not height sensor is available for reset
 2016-04-22 10:50:53 +10:00
Paul Riseborough 874558d194 EKF: improve detection of bad vert accel data 
Improve ability to detect if bad vertical accel data has caused loss of height accuracy by using historical data.
 2016-04-22 09:24:04 +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 e3365525c2 EKF: rework height reset logic 2016-04-22 08:39:24 +10:00
Paul Riseborough 421703c267 EKF: rework initialisation of height state and offsets 2016-04-22 08:38:41 +10:00
Paul Riseborough a7417657c3 EKF: ensure fusion timeout counters are reset when required 2016-04-22 08:33:11 +10:00
Paul Riseborough 10bf05e9a6 EKF: publish the vertical position offset 2016-04-22 08:30:46 +10:00
Paul Riseborough 0de15b1b20 EKF: Add variables to monitor vertical position and height offset 2016-04-22 08:28:56 +10:00
Paul Riseborough 62c6d40f1f EKF: Add methods to ring buffer to access specific indices 2016-04-22 08:21:36 +10:00
Paul Riseborough b295f9050c EKF: ensure GPS check status is correctly initialised 2016-04-20 21:53:15 +10:00
Lorenz Meier 9487eac505 Merge pull request #98 from priseborough/pr-ekf2StatusReporting 
EKF: improve reporting of filter status
 2016-04-20 11:44:02 +02:00
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