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

136 Commits

Author SHA1 Message Date
Paul Riseborough 5f9752cac8 EKF: Improve function names 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
Paul Riseborough 928a7dd059 Merge pull request #157 from PX4/pr-ekf2ResetInterface 
Change state reset information interface
 2016-06-08 14:18:31 +10:00
Paul Riseborough 99b34f0df4 EKF: Only reset necessary terms when mag fusion covariance reset required 
Only the quaternion and mag fusion state covariances are used in the mag fusion calculations.
 2016-06-08 11:58:33 +10:00
Paul Riseborough 081e17729c EKF: delay commencement of 3D mag fusion until clear of ground 
Wait until enough height has been gained to be clear of ground based magnetic anomalies. Failure to do so can result in incorrect earth field initialisation.
 2016-06-06 21:59:46 +10:00
Roman Bapst 670c6ca019 change state reset information interface for more convenient handling 
on firmware side
 2016-06-06 13:22:20 +02: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 e371a303a9 EKF: publish the quaternion reset event 2016-06-01 17:13:00 +10:00
Paul Riseborough 2b0d5c28f0 EKF: capture full quaternion change for reset events 2016-06-01 17:13:00 +10:00
Paul Riseborough b2e432e211 EKF: publish position and velocity reset data 2016-06-01 17:13:00 +10:00
Paul Riseborough 733862f649 EKF: move the reset status struct to the Ekf class 
This protects it from being modified externally
 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
Carl Olsson 0fafc49a49 fixed typos (#147) 2016-05-22 17:40:43 +02:00
waltjohnson 7c158aa59b Addressed C99 compiler issues of initializing variables in header and unused included statements. 2016-05-19 09:58:18 -06:00
Paul Riseborough 22fba0fc6e EKF: don't initialise filter without EV data if we are relying on it 2016-05-18 19:25:19 +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 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 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 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 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 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 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 727a43764f EKF: update initial angle alignment check 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
Paul Riseborough 4fcbfb5d42 EKF: correct error in comment 2016-04-23 07:55:25 +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 421703c267 EKF: rework initialisation of height state and offsets 2016-04-22 08:38:41 +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
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 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 31bf342fc1 EKF: publish GPS check status 2016-04-19 17:38:22 +10:00
Roman Bapst 60abf07bee added function to return accelerometer bias 2016-04-18 17:52:16 +02: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
CarlOlsson 6bb2f7638c adopted ekf.h 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 dca186c6e8 EKF: Add required declarations for optical flow 2016-03-11 11:03:43 +11: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 d5e47d21db EKF: miscellaneous comment and format changes 2016-03-01 15:35:45 +11: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
mcsauder 6613335937 Added constexpr back from const var type. 2016-02-23 16:15:52 -07:00
mcsauder a4cecb1704 Match variable initialization order difference with upstream. 2016-02-23 19:57:27 -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 437f6ca5fb Moved initialization to object constructors to allow C99 compiler compatibility. 2016-02-18 03:21:04 -07: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 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 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 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 47ab5ebcdd EKF: Make PR comply with project convention for indenting 2016-02-08 15:12:38 +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 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
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 c6249a2825 EKF: change estimator base class name to estimation interface 2016-01-31 00:12:07 -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 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 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 802129f384 EKF: Add magnetometer fusion error handling 2016-01-28 09:53:55 +11:00
Roman 8d0022ab1e enable estimator state and innovations data logging 2016-01-22 11:31:14 +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 5e5d6f432a added static gps mode and init mag state correctly 2015-12-22 11:22:17 +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 cfc39bc2f9 implemented prediction of states and covariance matrix 2015-12-07 22:41:11 +01:00
Roman Bapst 144aa9c461 added base class for data storage 2015-12-05 11:08:06 +01:00