Commit Graph

663 Commits

Author SHA1 Message Date
Paul Riseborough e98edaa6cb AP_NavEKF: Return more accurate validity status for height above ground 2015-04-10 11:07:24 +09:00
Paul Riseborough 586e4a7d2b AP_NavEKF: Add Matlab derivations and simulations behind small EKF 2015-04-10 11:07:21 +09:00
Paul Riseborough c57e25142c AP_NavEKF: Update optical flow fusion maths to reduce height errors 2015-04-07 20:51:18 -07:00
myly10 55befdc345 AP_NavEKF: Typo correction for EAS_NOISE description 2015-04-06 15:52:29 -07:00
Paul Riseborough 9b3656e77c AP_NavEKF: Fix bug introduced in Y axis flow fusion 2015-04-05 21:17:20 -07:00
Andrew Tridgell baf292def1 AP_NavEKF: prevent float exception on startup 2015-04-05 09:16:14 -07:00
Paul Riseborough 7fc0f026d2 AP_NavEKF: Fix bug in optical flow innovation variance integrity check
The check allowed negative innovation variances to pass. If this did occur, the filter would diverge.
2015-04-04 17:03:06 -07:00
Andrew Tridgell c1a0375562 AP_NavEKF: prevent divide by zero in EKF logging 2015-04-04 07:09:02 -07:00
Paul Riseborough 10f050c53b AP_NavEKF: Prevent baro drift causing toilet bowling
The magnetic field states are reset once at 1.5 metres and again at 5 metres. This height check was using the height at the first arm event as the reference. In the situation where there is baro drift and extgended time between the first arm event and flight, this can cause the magnetic field state to be reset when on the ground. If this happens when flying off a metallic surface, the resultant heading errors can cause sever toilet bowling.
2015-04-03 15:18:42 -07:00
Paul Riseborough d4c60ca956 AP_NavEKF: Fix bug preventing reset of velocity after OF fusion timeout 2015-04-03 15:18:39 -07:00
Jonathan Challinger 95cd3480ec AP_NavEKF: review all uses of dtIMU and use dtIMUactual where necessary
pair-programmed-with: Paul Riseborough <p_riseborough@live.com.au>
2015-04-03 15:18:09 -07:00
Andrew Tridgell 79b44d3988 AP_NavEKF: initialise gndEffectMode 2015-04-03 15:15:11 -07:00
Andrew Tridgell 3165c43dfe AP_NavEKF: initialise gpsSpdAccuracy 2015-04-03 15:15:11 -07:00
Paul Riseborough 98c32012fa AP_NavEKF: remove accel bias rate limit when disarmed 2015-04-03 15:15:11 -07:00
Paul Riseborough fe76cb4c0b AP_NavEKF : Make initial height variance consistent with baro noise
This makes sense to do because we initialise the state to the instantaneous baro reading
2015-04-03 15:15:11 -07:00
Paul Riseborough a976e9dad2 AP_NavEKF : Fix bug in scaling of initial Z accel bias state variance 2015-04-03 15:15:11 -07:00
Paul Riseborough 92df3adb5e AP_NavEKF : Fix bug in Z accel bias update for IMU1 2015-04-03 15:15:10 -07:00
Paul Riseborough 5d0952ba23 AP_NavEKF: eliminate onGndBaroNoise 2015-04-03 15:15:10 -07:00
Paul Riseborough fafb898341 AP_NavEKF: tuning change to accel bias learning 2015-04-03 15:15:10 -07:00
Paul Riseborough 398accd151 AP_NavEKF: Improve pre-flight ready checking 2015-04-03 15:15:10 -07:00
Paul Riseborough 5c1a226bef AP_NavEKF : Improvements to pre-arm IMU bias estimation 2015-04-03 15:15:10 -07:00
Jonathan Challinger a5924acb3d AP_NavEKF: set dtIMU from ins expected sample rate 2015-04-03 15:15:10 -07:00
Paul Riseborough a1351e73ab AP_NavEKF : Compensate mag bias states for external copass offset changes 2015-04-03 15:15:09 -07:00
Paul Riseborough 14795719f6 AP_NavEKF: Add public function for estimated magnetometer offsets 2015-04-03 15:15:09 -07:00
Paul Riseborough 1c244af3d8 AP_NavEKF: Fix bug affecting in-flight GPS acquisition
This bug caused velocities to be reset to zero
2015-04-03 15:15:09 -07:00
Paul Riseborough b9b6938b1d AP_NavEKF: Add ability to start using GPS in-flight
Improve the quality of the GPS required to set an EKF  origin
Eliminate repeated update of origin height - origin height updates once when EKF origin is set.
Operation in GPS mode is linked to setting of origin
2015-04-03 15:15:09 -07:00
Paul Riseborough 98fa918b84 AP_NavEKF: Add new compass learning option
Enables compass learning to be on continuously for non-position hold operation
2015-04-03 15:15:09 -07:00
Paul Riseborough 961faa59d9 AP_NavEKF: Don't let reported GPS accuracy modify horiz vel data checks 2015-04-03 15:15:08 -07:00
Paul Riseborough a607eb8469 AP_NavEKF: Reduce the GPS glitch tether length from 100 to 50m 2015-04-03 15:15:08 -07:00
Paul Riseborough 496d31ab83 AP_NavEKF: Reduce max time Copters can reject GPS
The maximum time copters can reject GPS has been reduced from 10 to 7 seconds as flight logs have show that inertial dead reckoning with vibration and calibration errors is not good enough to support 10 seconds without aiding.
2015-04-03 15:15:08 -07:00
Paul Riseborough 8c2029d896 AP_NavEKF: Accel bias learning improvements
Speed up pre-flight learning
Smoothen in-flight learning
2015-04-03 15:15:08 -07:00
Paul Riseborough 19d1b3b813 AP_NavEKF: Update EKF origin height whilst disarmed
This patch causes the EKF to update the height of its origin each time it receives a valid GPs message whislt disarmed.
The resultant EKF origin height represents the height of the zero baro alt datum relative to the GPS height datum.
2015-04-03 15:15:08 -07:00
Paul Riseborough 70afcd7e70 AP_NavEKF: Add second stage alignment of yaw and earth field states
Flight tests have shown that the magnetic field distortion associated with flight from steel structures can extend 3m or higher. To counteract this, a second and final yaw and magnetic field alignment has been added which is activated when the height exceeds 5m for the first time.
2015-04-03 15:15:08 -07:00
Paul Riseborough ed9c05cf2a AP_NavEKF: Remove logic used to delay fusion for load levelling
Logic used to delay optical flow and airspeed fusion to prevent it occurring on the same time step as magnetometer fusion has been removed. This is no longer required to efficiency improvements made at the firmware level.
2015-04-03 15:15:07 -07:00
Paul Riseborough c0d23ffc30 AP_NavEKF: Filter accuracy and stability improvements
Improvements in PX4 firmware have reduced the computational load  and mkae the previous practicwe of splitting magnetometer and optical flow fusion across multiple time steps unnecessary and make it possible to perform a covariance prediction prior to fusing data on the same time step. This patch:

1) Ensures that a covariance prediction is always performed prior to fusion of any observation
2) Removes the splitting of magnetometer fusion so that fusion of the X,Y and Z components occurs on the same time time step
3) Removes the splitting of optical flow fusion so that fusion of X and Y components occurs on the same time step
2015-04-03 15:15:07 -07:00
Jonathan Challinger de1f7f5e63 AP_NavEKF: use published delta velocities and delta angles if available 2015-04-03 15:15:07 -07:00
priseborough 3421a320b5 AP_NavEKF: Compensate for ground effect when takeoff or landing expected 2015-04-03 15:15:07 -07:00
Jonathan Challinger 20d92f5f9d AP_NavEKF: floor GPS velocity noise at parameter value for conservatism 2015-04-03 15:15:07 -07:00
priseborough 9a797a5d49 AP_NavEKF: Use GPS reported speed accuracy if available
UBlox receivers report an estimate of the speed accuracy that tests show correlates well to speed glitches. Using this to scale the GPS velocity observation noise will reduce the effect of bad GPS velocity data.
2015-04-03 15:15:06 -07:00
Andrew Tridgell 3289d38339 AP_NavEKF: make the init functions return bool
we need to know if it has initialised successfully
2015-03-28 10:51:38 -07:00
Andrew Tridgell de3f461a55 AP_NavEKF: use compass->last_update_usec() 2015-03-14 12:31:39 +11:00
Randy Mackay 230ca583d1 NavEKF: support sending EKF_STATUS_REPORT 2015-03-12 12:20:00 +09:00
Paul Riseborough 9f552eaa4b AP_NavEKF: Fix bug that resets position to origin when vehicle arms 2015-02-12 12:40:55 +11:00
Paul Riseborough b8d3da3846 AP_NavEKF: Report last known position when vehicle is disarmed 2015-02-12 12:40:55 +11:00
Andrew Tridgell 55041c7a7a AP_NavEKF: prevent division by zero in SmallEKF 2015-02-12 09:02:59 +11:00
Jonathan Challinger 50466848f7 AP_NavEKF: use hal.util soft_armed state 2015-02-11 20:25:11 +11:00
Grant Morphett 4860c84dff AP_NavEKF: Changes to fix the warnings in rover sitl build.
We are starting the process of resolving all the warnings in the
ardupilot builds of all vehicles and platforms.
2015-02-11 18:16:46 +11:00
Paul Riseborough aa94ff629d AP_NavEKF: Prevent bad GPS pre-arming casuing initial position errors
If the vehicle moves significantly or the GPS changes position significantly pre-armed, then the GPS glitch logic was being invoked when the first GPs measurements were fused. This patch resets the position to the GPS when the vehicle arms.
2015-02-03 15:57:10 +09:00
priseborough bc5581d634 AP_NavEKF: Prevent arming delays from failing GPS
Due to the way that gyro calibration is done, the EKF could be effectively not run for up to 30 seconds in extreme cases, making it possible that the GPS would be failed on arming and the copter put into a non-GPS mode.

the longer term solution is to update the gyro calibration so that it does not hold up other processing. the short tyermfix in thsi patch is to look for evidence of a 3D lock in the last received GPS message.
2015-02-03 15:57:09 +09:00
priseborough 2c012c2763 AP_NavEKF: Always check for new GPS data
This fixes a bug that meant that once the EKF had started up in a non-GPS mode, it would no longer read the GPS and therefore would never be able to use GPS again until reset.
2015-02-03 15:57:08 +09:00
Paul Riseborough 925d625ed1 AP_NavEKF: fix bug in small EKF velocity fusion 2015-02-03 09:49:17 +11:00
Paul Riseborough 5f24603ceb AP_NavEKF: Publish small EKF quaternion and gyro bias outputs 2015-02-03 09:49:16 +11:00
Andrew Tridgell 4c8b663200 AP_NavEKF: added initial version of SmallEKF
This will be used for gimbal bias estimations. 

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2015-02-03 09:49:16 +11:00
Andrew Tridgell 850af14949 AP_NavEKF: raise EKF_POS_GATE and EKF_GLITCH_RAD for planes
This weights GPS position more heavily for planes

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2015-01-31 21:49:20 +11:00
priseborough 5df733a883 AP_NavEKF: Allow flight transition to optical flow mode if GPS is lost 2015-01-22 14:41:04 +09:00
priseborough 04810c012d AP_NavEKF: Increase flow data valid timeout to handle arming delays
When Copter arms, the AHRS/EKF may not be run for a few hundred msec depending on conditions. This can cause the arming check to fail the optical flow sensor and place the EKF in a constant position mode.
2015-01-22 14:41:01 +09:00
priseborough 073b8e7c43 AP_NavEKF: Always explicitly set required const pos or vel mode when arming
This additional explicit setting of the constPosMode and constVelMode reduces the likelihood of logic errors being introduced in the future as it places the intended setting of these parameters at arming in the one place. the constVelmode and constPosMode only have one set of conditions each that can trigger these modes in flight, so if these modes are true after arming it will be clear that it was the in-flight condition that triggered.
2015-01-22 14:40:59 +09:00
priseborough 9c6dabe1cc AP_NavEKF: Add separate flow default parameters for platform types
Also reduces flow measurement noise default for copter only and increases gate to compensate.
2015-01-22 14:40:57 +09:00
priseborough d2da16e652 AP_NavEKF: Consistently set timeout flags whenever aiding is inhibited
This ensures the position and velocity measurement status will be set as timed out immediately after use of those measurements is inhibited. This will improve the timeliness of filter status reporting.
2015-01-22 14:40:55 +09:00
priseborough 824425625c AP_NavEKF: Update public method used to inhibit GPS use
This method is not currently used by any of our vehicle types, but will be required to enable a user selectable 'indoor mode'.
2015-01-22 14:40:52 +09:00
priseborough 1033f5fc1e AP_NavEKF: Apply flow nav vehicle limits regardless of sensor health
It does not make sense to relax the limits on vehicle speed and nav gains just because we have received some invalid flow data. This could make the situation worse if the invalid data was being caused by too much speed.
If we are relying on flow data the vehicle limits should always be applied.
2015-01-22 14:40:50 +09:00
priseborough 14b51f6d74 AP_NavEKF: Unconditionally fuse velocity in constant velocity mode
The zero velocity measurements in this mode are by definition always correct and should never be rejected
2015-01-22 14:40:48 +09:00
priseborough 6663d80176 AP_NavEKF: Simplify nested logic - functionally equivalent
Additional if else statement was unnecessary
2015-01-22 14:40:45 +09:00
priseborough 12c3368c4d AP_NavEKF: Bypass GPS glitch logic when not aiding
When we are not using GPS measurements, we should not be allowing the GPS glitch logic to reset position states as this can interfere with operation of non GPS modes.
2015-01-22 14:40:43 +09:00
priseborough 5c8e71a8d1 AP_NavEKF: Don't reset the position measurement timeout if not aiding
When PV aiding is disabled, then the timeout time reference should not be reset becasue we want the position measurement timeout status to remain true the whole time the measurement is not being used.
2015-01-22 14:40:41 +09:00
priseborough c505a458de AP_NavEKF: Always declare a position measurement timeout if aiding not used
If position and velocity aiding is turned off, then the position measurement should always be reported as timed out.
2015-01-22 14:40:39 +09:00
priseborough 3b166372cc AP_NavEKF: Always declare a velocity measurement timeout if velocity not used
If position and velocity aiding is turned off, then the velocity measurement should always be reported as timed out.
2015-01-22 14:40:36 +09:00
priseborough 95c3197170 AP_NavEKF: Ensure velocity will not be reset unless needed for aiding 2015-01-22 14:40:34 +09:00
priseborough 8aeec82846 AP_NavEKF: Continually turn off aiding whilst the vehicle is disarmed
This prevents the possibility of any logic errors turning aiding back on.

AP_NavEKF: Fix bug in logic
2015-01-22 14:40:31 +09:00
priseborough 81ee339e25 AP_NavEKF: Synchronise non-aiding mode state corrections
Synchronise with covariance prediction to improve numerical stability and accuracy of angle corrections. The 'noise' this produces in the position and velocity estimate is irrelevantbecause these are not used by the control loops during this mode of operation (they are nominally zero anyway).
2015-01-22 14:40:29 +09:00
priseborough ae6b85e63d AP_NavEKF: Explicitly set aiding to off when disarmed
This fixes a bug that could cause the AHRS to drift whilst the vehicle was disarmed depending sensor combinations at startup.
2015-01-22 14:40:27 +09:00
priseborough fb1962b111 AP_NavEKF: Let reported position whilst disarmed show inertial errors
Showing the positon states (which are nominally zero) in addition to the last known offset can provide useful log information.
2015-01-22 14:40:25 +09:00
Andrew Tridgell e8017a5079 AP_NavEKF: cope with the changed semantics of airspeed.use() 2015-01-20 11:28:14 +11:00
priseborough c40c3632bb AP_NavEKF: Critical Bug Fix
Prevents possible loss of attitude reference for flights without optical flow and GPS.

The optical flow measurement timeout can reset the velocity states which decouples the position states from IMU errors and therefore significantly reduces the amount of attitude error correction.
2015-01-13 16:05:31 +13:00
Andrew Tridgell ef5cdb0d6c AP_NavEKF: use more array bounds checked variables 2015-01-09 11:05:07 +11:00
priseborough 3d46680348 AP_NavEKF: Make NED origin independent of home position
This enables the filter to report the last known position after disarm.
The LLH location of the filters NED origin is published and should be logged every time the vehicle is armed to assist with post-flight trajectory reconstruction.
The LLH location of the filters NED origin can be set externally whilst the vehicle is disarmed.
2015-01-09 10:51:24 +11:00
priseborough f0ea858e4c AP_NavEKF: Make LLH output report last known position in const pos mode 2015-01-09 10:51:24 +11:00
priseborough a0957a83f8 AP_NavEKF: Fix bug in reported position and velocity
The last known position was being output on the velocities when in constant position mode.
2015-01-09 10:51:24 +11:00
priseborough 7d1cd604a8 AP_NavEKF: Report last known position when GPS is lost 2015-01-09 10:51:24 +11:00
priseborough 1789dc08a3 AP_NavEKF: Correctly report position timeout when GPS is lost 2015-01-09 10:51:24 +11:00
Randy Mackay e6e6a781c1 AP_NavEKF: init filter status bits to zero 2015-01-09 10:51:23 +11:00
priseborough ffd9f7a4ed AP_NavEKF: Predict filter solution status 2015-01-09 10:51:23 +11:00
Randy Mackay 657afcfe7a AP_NavEKF: add pred_horiz_pos flags to filter status 2015-01-09 10:51:23 +11:00
Randy Mackay f4d8bc586c Nav_EKF: getFilterStatus returns nav_filter_status struct 2015-01-09 10:51:23 +11:00
Randy Mackay 8a914af4a8 AP_NavEKF: add nav_filter_status definition 2015-01-09 10:51:23 +11:00
priseborough d57e0b6bde AP_NavEKF: Remove compiler warning messages 2015-01-09 10:51:23 +11:00
priseborough a4ba4d1000 AP_NavEKF: Fix bug in optical flow innovation variances
Also improves protection against badly conditioned variances
2015-01-09 10:51:23 +11:00
priseborough 8bc8d1444a AP_NavEKF: Extend definition of GPS availability to include user inhibit 2015-01-09 10:51:22 +11:00
priseborough d0d49065e7 AP_NavEKF: Prevent potential repeated use of optical flow data 2015-01-09 10:51:22 +11:00
priseborough b0c703e4f7 AP_NavEKF: Fix bug in optical flow fusion control logic 2015-01-09 10:51:22 +11:00
priseborough d656c94bbc AP_NavEKF: Fix out of bounds index bug 2015-01-09 10:51:22 +11:00
priseborough 300ba65f64 AP_NavEKF: Remove duplicate flow measurement state correction 2015-01-09 10:51:22 +11:00
priseborough 92bb75a635 AP_NavEKF: Prevent load leveling from dropping flow measurements 2015-01-09 10:51:22 +11:00
priseborough 2f5aa210ce AP_NavEKF: Enable recovery from extended flow measurement rejection 2015-01-09 10:51:21 +11:00
priseborough 9f4baaa865 AP_NavEKF: Update flow debug logging 2015-01-09 10:51:21 +11:00
priseborough 8d1dae3ac1 AP_NavEKF: Improve optical flow terrain height estimation
The two state auxiliary EKF has been replaced with a single state filter that only estimates terrain offset. The new filter fuses a optical flow line of sight rate scalar (length of the optical flow LOS rate vector) which provides a terrain offset estimate that is less affected by yaw errors.
Estimation of focal length scale factor error in flight wasn't accurate enough and will be replaced with a pre-flight intrinsic sensor calibration procedure as the scale factor error does not change over time provided the lens assembly is not adjusted.

AP_NavEKF: Remove unwanted printf
2015-01-09 10:51:21 +11:00
priseborough d599fa588e AP_NavEKF: Don't allow EKF to initialise without GPS lock if we are a plane
This is needed because planes arm automatically.

AP_NavEKF: Fix bug in GPS patch
2015-01-09 10:51:21 +11:00
priseborough 2f0b1b3d9c AP_NavEKF: Fix bug preventing use of optical flow without GPS 2015-01-03 21:49:22 +11:00
Andrew Tridgell 16affe51be AP_NavEKF: avoid an extra quaternion copy 2015-01-03 15:44:07 +11:00
priseborough 69e86d3ea4 AP_NavEKF: Update EKF optical flow data logging 2015-01-03 14:09:12 +11:00
priseborough b651eac48d AP_NavEKF: Apply timeout to terrain offset validity reporting
The terrain offset solution status is usable for a short period of time without state updates so a timeout has been added which prevents the rapid changes in solution status due to short duration sensor read errors.
2015-01-03 14:09:09 +11:00
priseborough e6474d676e AP_NavEKF: Report correct horiz vel sol'n status during optical flow nav 2015-01-03 14:09:05 +11:00
priseborough 75201c8968 AP_NavEKF: Prevent divide by zero casued by Tnb_flow.c.z = 0 2015-01-03 14:07:13 +11:00
priseborough 8d3940ce1b AP_NavEKF: Improved use of enumerated type for aiding mode 2015-01-02 12:10:41 +09:00
priseborough 76d1378962 AP_NavEKF: Fix compiler warning messages 2015-01-01 23:39:48 +11:00
priseborough 3e3dd9d695 AP_NavEKF: Fix parenthesis error in EKF status reporting and clean up logic
This fixes a bug which could have caused the realative position status to be incorrectly reported under some conditions and also caused a compiler warning message. the logic used to report the filter solution status has been broken down into smaller, easier to understand statements.
2015-01-01 23:39:48 +11:00
priseborough c06f6e105e AP_NavEKF: Consistent initialisation of tuning parameters and variables
Non user adjustable parameters are now declared as 'const' in the header.
The _ prefix has been removed from non user adjustable tuning parameters.
We use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary.
For consistency some signed integer type declarations have been changed to unsigned where appropriate.
2015-01-01 23:39:31 +11:00
priseborough f1dfa282dc AP_NavEKF: Consolidate constant velocity mode decision logic
The decision to switch to constant velocity mode during optical flow operation and te decision to switch back were previously being made in two different places in code. Both decisions are now made in the one place which makes the code easier to follow and maintain.
2015-01-01 19:53:21 +11:00
priseborough 9caf2ac895 AP_NavEKF: Make reversion to no GPS mode unambiguous 2015-01-01 19:53:21 +11:00
priseborough 58e9dd5dcd AP_NavEKF: Enumerate Position and Velocity aiding status 2015-01-01 19:53:21 +11:00
priseborough b016bae445 AP_NavEKF: Clean up reset of constant velocity mode
Move velocity store out of optical flow to velocity and position fusion control as it is a velocity fusion function.
Always clear the previous mode status
2014-12-31 13:16:16 +09:00
priseborough 3d207c316a AP_NavEKF: Change names for position and velocity hold modes
Name change done to avoid confusion with flight modes. modes are now referred to as constant position and constant velocity mode.
2014-12-31 13:16:15 +09:00
priseborough b650ee51a9 AP_NavEKF: Consolidate arming checks 2014-12-31 13:16:15 +09:00
priseborough 48cae0df15 AP_NavEKF: Fall back to attitude and hgt estimation for copter if GPS lost 2014-12-31 13:16:14 +09:00
priseborough b21f9daa90 AP_NavEKF: Update solution status reporting
This patch makes the reporting of an absolute position solution less abbiguaous and ensures that relative position is always true if absolute position is true
2014-12-31 13:16:13 +09:00
priseborough b6d300db19 AP_NavEKF: clean up determination of GPS availability 2014-12-31 13:14:22 +09:00
priseborough 40b02fc19a AP_NavEKF: Ensure filter doesn't try to use GPS after initialisation 2014-12-31 13:14:20 +09:00
priseborough bab1a69f1c AP_NavEKF: make position and velocity resets consistent with GPS usage 2014-12-31 13:14:18 +09:00
priseborough 0dc2356175 AP_NavEKF: Ensure initialisation methods read required sensor data 2014-12-31 13:14:16 +09:00
priseborough 5d1de79f08 AP_NavEKF: Modify initial values to make startup behaviour more consistent 2014-12-31 13:14:14 +09:00
priseborough a63af34d8f AP_NavEKF: Allow 10 seconds for the filter to settle after initialisation
Filter is declared unhealthy and will not arm for first 10 seconds after initialisation
2014-12-31 13:14:12 +09:00
priseborough 0a5de21dc7 AP_NavEKF: Fix bug in reporting of solution status
Because synthetic  position measurements at 0,0 are fused during position hold mode, a position timeout cannot be used as the only means of detecting position solution status.
2014-12-31 13:14:09 +09:00
priseborough 92c3026072 AP_NavEKF: Link process noise to arm status, not pos hold mode 2014-12-31 13:14:07 +09:00
priseborough a535b73a6d AP_NavEKF: Initialise timeout status to true
This status will be cleared when data arrives and is fused successfully
2014-12-31 13:14:04 +09:00
priseborough 363c1e393d AP_NavEKF: Latch use of position hold mode for duration of flight
Required to prevent acquisition of GPS mid flight causing unwanted change in position and velocity
A distinction has been mad between the arm and disarm transition and the decision to use position hold mode (formerly static mode)
2014-12-31 13:14:02 +09:00
priseborough a0a6c0362f AP_NavEKF: Relax timeout check applied to optical flow data
200 msec was too short and could lead to false positives. 5000 msec is the largest time we can go free inertial.
2014-12-31 13:14:00 +09:00
priseborough 5bd4ee9715 AP_NavEKF: Use compensation for baro delay in position hold mode 2014-12-31 13:13:58 +09:00
priseborough 4dc1ee2d66 AP_NavEKF: Rename static mode to avoid confusion with other non-GPS modes
This renames static mode as posHoldMode to make it clearer what this and the other non-GPS modes do
2014-12-31 13:13:54 +09:00
priseborough b160f4c03b AP_NavEKF: Compensate optical flow fusion for GPS glitch recovery offset 2014-12-31 13:13:52 +09:00
priseborough e4c969084d AP_NavEKF: Improve behaviour recovering from a GPS timeout
When regaining GPS after a timeout, an offset is applied when fusing  GPS velocity so that GPS velocity and position data as fused by the EKF is kinematically consistent.
This velocity offset is also accounted for when fusing air data so that wind estimates are not corrupted when the GPS position offset is being pulle back to zero.
The intended behaviour is that the EKF position will be pulled back to the GPS position at a rate of 5m/s for planes and 1 m/s for copters. This avoids large deviations in trajectory when GPS is regained.
2014-12-31 13:13:50 +09:00
priseborough 6eb533121c AP_NavEKF: Add static mode to solution status message 2014-12-31 13:13:48 +09:00
priseborough 5c3a56a087 AP_NavEKF: Fix error in comments 2014-12-31 13:13:46 +09:00
priseborough 3891dada78 AP_NavEKF: remove unnecessary function 2014-12-31 13:13:44 +09:00
priseborough a16253796f AP_NavEKF: Fix bug preventing the non-GPS mode being selected externally
When on the ground it is likely the flow sensor will be returning  data that does not meet the minimum quality requirements selected.
The previous check was for the presence of valid data. This has been loosened to look for the presence of data.
When the vehicle becomes airborne, the quality of flow data normally improves as the image comes into focus.
2014-12-31 13:13:42 +09:00
priseborough a42100e4c5 AP_NavEKF: Improved handling of no GPS
This patch enables indoor operation of the EKF by putting it into static mode if optical flow operation is not enabled and no GPS is available
2014-12-31 13:13:40 +09:00
priseborough 73a06cd0c1 AP_NavEKF: Fix bug causing magnetic field state learning to be forgotten 2014-12-31 13:13:37 +09:00
priseborough bf89c56e54 AP_NavEKF: Fix comments error in description of flow sensor sign conventions 2014-12-31 13:13:35 +09:00
priseborough 4eb19c2324 AP_NavEKF: Rationalise health status reporting
1) Un-used public methods to report height and position drifting have been removed
2) A time-out has been added to the airspeed innovation consistency check so that if we are relying on airspeed to constrain velocity drift, a filter divergence or other fault that causes the airspeed to be continually rejected will trigger a change in health status.
3) A timeout of velocity, position or height measurements does not cause a filter fault to be reported. Timeouts can be due to sensor errors and do not necessarily indicate that the filter has failed.
4) Time-outs of various measurements are used to present a consolidated bitmask which inidicates which parts of the solution can be used, eg attitude, height, velocity, relative position, absolute position, etc.
2014-12-31 13:13:31 +09:00
priseborough 78b30e4ce5 AP_NavEKF: Increase rate at which GPS glitch offsets are reduced for planes 2014-12-31 13:13:29 +09:00
priseborough f2c506339a AP_NavEKF: Reduce time required to recover from GPS timeouts
The time required for GPS to be lost or rejected before vehicles with airspeed sensors either reset to GPS or invoke the zero side-slip assumption to constrain drift has been reduced from 15 to 10 seconds
A duplicate zeroing of the GPS position offset has been removed
If the vehicle is a non hovering vehicle (eg a plane) then the speed at which the GPS offset is pulled back to zero after a reset is increased from 1 to 3  m/s

This also improves recovery from bad inertial data for planes
2014-12-18 21:50:40 +11:00
priseborough f73816dbb5 AP_NavEKF: Improve robustness to long periods without GPS 2014-12-18 21:07:45 +11:00
priseborough 3afde0061f AP_NavEKF: Use named states for velocity reset
Readibility improvement
2014-12-18 17:29:50 +11:00
priseborough f6ce25df2a AP_NavEKF: Do not reset vertical velocity state from GPS
Doing this can cause large height and height rate errors if large GPS velocity errors cause the GPS tn be rejected for long enough to cause a timeout and reset of states.
2014-12-18 17:29:47 +11:00
Randy Mackay e581e81da5 AP_NavEKF: reorder declation to fix compiler warning
No functional change
2014-12-11 15:37:56 +09:00
Andrew Tridgell cb9c518ab8 Revert "AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing'"
This reverts commit 13df6fb1c9.

This patch was a result of an incorrect merge of the optflow branch
into master. It reintroduced the bug fixed by this commit:

  AP_NavEKF : Fix bug in reset of GPS glitch offset
  8aa267f75f
2014-12-08 07:53:47 +11:00
priseborough a2bd3b4a42 AP_NavEKF: Add public method returning height above ground level 2014-12-06 18:16:51 +11:00
priseborough d701cfcb75 AP_NavEKF: Output terrain relative PosD when relying on Optical Flow 2014-12-06 18:16:51 +11:00
priseborough 0156d846f1 AP_NavEKF: Increase height at which nav gain reduction starts
Compensates for optical flow induced noise increase with height.
2014-12-06 18:16:51 +11:00
priseborough a905432ffe AP_NavEKF: reduce height at which nav gain reduction starts 2014-12-06 18:16:51 +11:00
priseborough 5785272933 AP_NavEKF: Improve robustness to corrupted flow sensor data 2014-12-06 18:16:51 +11:00
priseborough b1d44d4dde AP_NavEKF: Add protection for negative height above ground 2014-12-06 18:16:50 +11:00
priseborough d994da0886 AP_NavEKF: Expand EKF speed limit public method to handle control limits 2014-12-06 18:16:50 +11:00
priseborough bc2255d6b1 AP_NavEKF: Improve comments in setInhibitGPS public method 2014-12-06 18:16:50 +11:00
priseborough 416eaf4633 AP_NavEKF: Apply single definition of using optical flow 2014-12-06 18:16:50 +11:00
priseborough b56b68ce10 AP_NavEKF: Add public method reporting horizontal speed limit
This is required if using optical flow, as depending on height, the speed must be limited to prevent the sensor saturating
2014-12-06 18:16:50 +11:00
priseborough c8fb376cc4 AP_NavEKF: Fix bug in bias rate of change limiting
This doesn't change the behaviour, but means the result is achieved using the correct parameters, and comments are consistent
2014-12-06 18:16:50 +11:00
priseborough 599e53f3f2 AP_NavEKF: Prevent flow scale factor updating on ground 2014-12-06 18:16:49 +11:00
priseborough 083e22966c AP_NavEKF: Add public method to report available output data 2014-12-06 18:16:49 +11:00
priseborough e53d28854e AP_NavEKF: Add public method to inhibit GPS use and clean-up GPS use logic
This provides the calling vehicle software the abiity to request the EKF to not use GPS.
An integer is returned that indicates the type of operation available:
0 = request rejected (request will only be accepted if the EKF is in static mode, eg pre-armed)
1 = request accepted, attitude, vertical velocity and position estimates available
2 = request accepted, attitude, height rate, height, horizontal velocity and relative position estimates available
2014-12-06 18:16:49 +11:00
priseborough 8fb1d9cf8d AP_NavEKF: Fix bug preventing opt flow scale factor estimation
Also removes un-used variable declarations
2014-12-06 18:16:49 +11:00
priseborough a78920761c AP_NavEKF: Update comments in flow data interface 2014-12-06 18:16:49 +11:00
priseborough 52c7e56a4a AP_NavEKF: Add parameter for max valid optical flow rate magnitude 2014-12-06 18:16:49 +11:00
priseborough 6f8971d80a AP_NavEKF: Speed up flow sensor gyro bias correction 2014-12-06 18:16:49 +11:00
priseborough d7ad45ebda AP_NavEKF: Don't fuse flow measurements if too big 2014-12-06 18:16:49 +11:00
priseborough 84944fdd4e AP_NAvEKF: Revert back to 10Hz fusion 2014-12-06 18:16:49 +11:00
priseborough 1f78a73cdb AP_NavEKF: Make flow measurement delay vehicle specific
This takes into account the inter-sampling delay between the flow driver and the APM software which depends on the rate at which the dirver is being checked. This is 50Hz for plane and rover, and 200Hz for Copter.
2014-12-06 18:16:49 +11:00
priseborough 5f941655a8 AP_NavEKF: changes to support to 20Hz flow fusion rate 2014-12-06 18:16:48 +11:00
priseborough 0bf991eef9 AP_NavEKF: Fix bug in optical flow fusion smoothing 2014-12-06 18:16:48 +11:00
priseborough 839b814d12 AP_NavEKF : Inhibit flow and range fusion in static mode 2014-12-06 18:16:47 +11:00
priseborough 9b9934ac06 AP_NavEKF : fix velocity timeout bug during optical flow operation 2014-12-06 18:16:47 +11:00
priseborough 517026980b AP_NavEKF : Add fault and timeout logging 2014-12-06 18:16:47 +11:00
priseborough f640ec30ff AP_NavEKF : Reset held velocity on arming
This held velocity is used to constrain the drift in the optical flow solution before the vehicle is high enough for the optical flow sensor to work.
2014-12-06 18:16:47 +11:00
priseborough 06c19a3a4d AP_NavEKF : Update optical flow debug logging 2014-12-06 18:16:47 +11:00
priseborough 1791dec622 AP_NavEKF : Fix velocity hold mode 2014-12-06 18:16:47 +11:00
priseborough 84421e0a35 AP_NavEKF : Adjust initial flow scale factor and limits 2014-12-06 18:16:47 +11:00
priseborough 573633daf7 AP_NavEKF : Fix incorrect use of flow sensor time-stamp
flow sensor time stamp is now issued by the flow sensor and is on a different time-base
2014-12-06 18:16:47 +11:00
priseborough 9ea97c1a38 AP_NavEKF : Modify optical flow data interface
Sign conventions have changed
2014-12-06 18:16:46 +11:00
priseborough 8faeb190de AP_NavEKF : Rework logic to cope with bad flow data 2014-12-06 18:16:46 +11:00
priseborough 41f0231cfb AP_NavEKF : improve logic dealing with lack of flow or range data 2014-12-06 18:16:46 +11:00
priseborough 9ba46ad795 AP_NavEKF : Improve handling of flow sensor failure 2014-12-06 18:16:46 +11:00
priseborough 2ee1c549be AP_NavEKF : improve criteria used to inhibit scale factor estimation 2014-12-06 18:16:46 +11:00
priseborough e09ff84218 AP_NavEKF : Don't estimate focal length scale factor without reliable velocity 2014-12-06 18:16:46 +11:00
priseborough 4c92a5f23f AP_NavEKF : Use GPS velocity if PX4Flow sensor fails 2014-12-06 18:16:46 +11:00
priseborough 283811edcb AP_NavEKF : stop velocity change when flow measurements drop out 2014-12-06 18:16:46 +11:00
priseborough 9132fcfe43 AP_NavEKF : Change mag fusion to use corrected compass values
This enables compass units to be switched in-flight
2014-12-06 18:16:46 +11:00
priseborough c3be486c29 AP_NavEKF : Enable inhibiting of GPS measurements 2014-12-06 18:16:45 +11:00
priseborough 79698f7742 AP_NavEKF : improvements to range finder fusion 2014-12-06 18:16:45 +11:00
priseborough 1222559da0 AP_NavEKF : Fix bug affecting sonar fusion 2014-12-06 18:16:45 +11:00
priseborough 117bd2a998 AP_NavEKF : Add time based noise to terrain offset state 2014-12-06 18:16:45 +11:00
priseborough b1d3d5d9a3 AP_NavEKF : Don't update focal length scale factor at low speeds 2014-12-06 18:16:45 +11:00
priseborough 8dd1081f54 AP_NavEKF : Add range measurement to EKF debug message 2014-12-06 18:16:45 +11:00
priseborough 744c72d40b AP_NavEKF : Updates to range finder fusion 2014-12-06 18:16:45 +11:00
priseborough a20729f60f AP_NavEKF : Update names in optical flow debug logging 2014-12-06 18:16:45 +11:00
priseborough 235b3bebda AP_NavEKF : Add range finder health and enable range finder fusion
The 2-state auxiliary filter is also renamed.
2014-12-06 18:16:45 +11:00
priseborough 988de2a898 AP_NavEKF : Add smoothing to optical flow fusion 2014-12-06 18:16:44 +11:00
priseborough 70c779dbc2 AP_NavEKF : Optical flow numerical optimisations 2014-12-06 18:16:44 +11:00
priseborough 2a1420171a AP_NavEKF : reduce minimum accepted flow quality 2014-12-06 18:16:44 +11:00
priseborough 3fbedc9f98 AP_NAvEKF : update comments 2014-12-06 18:16:44 +11:00
priseborough 81b09c9361 AP_NavEKF : temporary mods to test use of flow sensor internal gyro data 2014-12-06 18:16:44 +11:00
priseborough cd418c946c AP_NavEKF : preliminary implementation for optical flow and range finder fusion
Range finder measurements are not input to EKF at this time, however the method for fusing them is
implemented.
2014-12-06 18:16:44 +11:00
priseborough 13df6fb1c9 AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing'
This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.

Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and the various calls to the hal.scheduler->millis() object have been consolidated.

Unused variables have been removed
2014-12-06 18:16:43 +11:00
priseborough dcb252cc05 AP_NavEKF : Fix bug in reset of position and velocity states
If the inertial solution velocity or position needs to be reset to the GPS, the stored state history for the corresponding states should also be reset.
Otherwise the next GPS measurement will be compared to an invalid previous state and will be rejected.
The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s
2014-12-06 18:16:43 +11:00
Jonathan Challinger 7692761f34 AP_NavEKF: Added getAccelNED function 2014-12-05 19:40:01 +09:00
priseborough 874d0780aa AP_NavEKF: Reduce vulnerability to ground fixed magnetic interference
Re-initialisation of the magnetic field states and yaw angle is now only performed a maximum of two times after start-up.

Once when coming out of static modefor the first time (first arm event)
Again (for copter only) when the altitude gain above the arming altitude exceeds 1.5m

this prevents magnetic interference present at arming (eg arming on a metal roof)from corrupting the magnetic field states enough to cause bad heading errors and toilet bowling on copter
2014-12-05 14:33:04 +11:00
Andrew Tridgell 46f601d703 AP_NavEKF: added getStaticMode() function 2014-11-22 18:27:51 +11:00
Andrew Tridgell 8acfbb2ee0 AP_NavEKF: add another health check in the EKF
if SV, SP and SH are all off then the most likely cause is divergence
of the EKF. This was done based on a flight log with bad gyro cal
2014-11-22 18:27:51 +11:00
Jonathan Challinger cc955b738b AP_NavEKF: Clean up flight detector logic 2014-11-14 10:34:49 +11:00
priseborough 5359da9c68 AP_NavEKF : Improved Magnetometer Error Handling
(Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector.
Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight.
For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-11-14 10:34:48 +11:00
Jonathan Challinger 6c4d4713aa AP_NavEKF: Add parameter that specifies EKF-to-DCM/INAV fallback strictness 2014-11-06 10:03:52 +11:00
Jonathan Challinger 967986d5c6 AP_NavEKF: Split getAccelBias into getAccelZBias and getIMU1Weighting 2014-10-30 18:17:38 +11:00
Jonathan Challinger bb6d8fd44a AP_NavEKF: Set prevDelAng after using it 2014-10-30 18:17:37 +11:00
Jonathan Challinger 0727ac5c79 AP_NavEKF: clean up unused variable lastDivergeTime_ms 2014-10-30 18:17:37 +11:00
priseborough 5e51c09978 AP_NavEKF : Remove ineffective numerical divergence checks 2014-10-30 18:17:37 +11:00
priseborough 6f759fdb8e AP_AHRS : Tidy up logic used to enable synthetic sideslip fusion 2014-10-30 18:17:36 +11:00
priseborough 6a85753a5b AP_NavEKF : add INIT_GYRO_BIAS_UNCERTAINTY definition 2014-10-29 17:39:47 +09:00
priseborough fb14da7a4b AP_NavEKF : Add public method to reset gyro bias states 2014-10-29 15:32:21 +09:00
Randy Mackay 7d48704f82 AP_NavEKF: fix parameter descriptions
Errors pointed out by Richard (DK)
2014-10-27 22:20:03 +09:00
priseborough 2bc74934b8 AP_NavEKF: Track baro alt when pre-armed
This will help prevent spurious alt disparity warning messages for copter
2014-10-21 20:58:59 +09:00
Jonathan Challinger 6076f3fa22 AP_NavEKF: Quaternion::to_euler now uses references instead of pointers 2014-10-20 06:24:25 +11:00
priseborough 8aa267f75f AP_NavEKF : Fix bug in reset of GPS glitch offset
The GPS glitch offset was being zeroed during position resets. This caused the filter to reject subsequent GPS measurements if the GPS error persisted long enough to invoke a timeout and a position reset.
2014-10-03 09:17:03 +10:00
Andrew Tridgell d6c56b8f7a AP_NavEKF: make it clear that all sat counts are covered 2014-10-01 12:55:29 +10:00
Andrew Tridgell a1f5be5b9e AP_NavEKF: simplify variable handling in EKF
use named state variables instead of states[] array where possible.
2014-10-01 12:55:29 +10:00
priseborough 8223a0d193 AP_NavEKF : Explicitly initialise gpsNoiseScaler to default value 2014-10-01 12:55:29 +10:00
priseborough b61a6c64d7 AP_NavEKF : Reduce weighting on GPS when not enough satellites
GPS measurement variance is doubled if only 5 satellites, and quadrupled if 4 or less.
The GPS glitch rejection thresholds remain the same
This will reduce the impact of GPS glitches on attitude.
2014-10-01 12:55:29 +10:00
priseborough f99f5759f5 AP_NavEKF : Fix bug in GPS innovation variance growth calculation
The uncertainty in acceleration is currently only scaled using horizontal accelerations, however during vertical plane aerobatics and high g pullups, misalignment in angles can cause significant horizontal acceleration error.
The scaling now uses the 3D acceleration vector length to better work during vertical plane high g maneouvres.
This error was found during flight testing with 8g pullups
2014-10-01 12:55:29 +10:00
priseborough f0ee11e951 AP_NavEKF : Fix bug in reset of position, height and velocity states
If the inertial solution velocity or position needs to be reset to the GPS or baro, the stored state history for the corresponding states should also be reset.
Otherwise the next GPS or baro measurement will be compared to an invalid previous state and will be rejected. This is particularly a problem if IMU saturation or timeout has occurred because the previous states could be out by a large amount
The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s.
2014-10-01 12:55:28 +10:00
priseborough f71aeea61d AP_NavEKF : Reduce sensitivity on filter divergence check
Flying aerobatics with Trad Heli has shown that the divergence check can be false triggered when large magnetometer errors and GPS dropouts are present.
This can also happen with multi rotors if large yaw rates are present.
This was an unintended consequence of the ekfsmoothing patch which improved filter stability during high rate manoeuvres, but made the divergence test more sensitive.
2014-09-17 10:22:55 +09:00
priseborough 36bf304f29 AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing'
This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-09-17 10:22:49 +09:00
priseborough 5b72cb7610 AP_NavEKF : Clean up time stamps
Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-17 10:22:45 +09:00
priseborough 0f62dbf6ed AP_NavEKF : Clear compass fail on transition between armed and disarmed
This allows a compass that has been declared failed, possibly because of
external disturbances (eg movement of hatches, proximity of tools, etc)
to be given a second chance when the vehicle is armed.
2014-08-25 19:29:07 +10:00
Andrew Tridgell 5d40ec8014 AP_NavEKF: make use_compass() public 2014-08-24 21:00:24 +10:00
Andrew Tridgell 3ee3a71644 AP_NavEKF: prevent a possible numerical error on startup
fixes issue #1294
2014-08-08 15:55:20 +10:00
priseborough d5442fe64e AP_NavEKF : Modified compass fail logic
If the vehicle can fly without a compass (a fly forward vehicle)
then if the compass times out (large errors for more than 10 seconds,
then it will be declared permanently failed and will not be
used until the filter is reset
2014-07-31 21:12:11 +10:00
Andrew Tridgell 383070b9c0 AP_NavEKF: ensure get_position() fills in flags 2014-07-25 11:40:26 +10:00
Emile Castelnuovo 946a461873 VRBRAIN: added VRBRAIN to #if 2014-06-19 11:27:44 +02:00
priseborough edc79ca2a4 AP_NavEKF: Increase divergence test margin based on analysis of more user flight logs
Analysis of copter logs has shown cases with a healthy EKF where spikes in EKF4.DS of up to 25% of the threshold have occurred.
A value of closer to 10% for normal operation is preferred.
2014-05-24 22:20:24 +10:00
priseborough 5fe0d2c1b2 AP_NavEKF: Add protection for accel bias estimation errors
Don't do bias estimation if tilted by more than 60 degrees to prevent scale
factor errors affecting result unnecessarily.
Prevent Kalman gain from having the wrong sign due to numerical errors
associated with small process noise values.
Allow smaller Z accel bias process noise values to be set
2014-05-18 08:09:00 +10:00
priseborough 3222e8f7cb AP_NavEKF: Default parameter adjustments
Bring Plane glitch protection thresholds into alignment with copter and
rover
Slight increase in accelerometer bias process noise to prevent bias
estimate divergence into limits (Rover and Plane only as Copter does not
seem respond as well to this change)
effective increase in threshold on divergence test to allow increased
margin for bad GPS velocities
2014-05-18 08:08:49 +10:00
priseborough e40e50e2e1 AP_NavEKF: Prevent start-up transients re-tripping divergence test 2014-05-16 22:05:22 +10:00
priseborough a19015ed61 AP_NavEKF: Scale divergence check with covariance
This provides consistent behaviour for a range of gyro bias process
noise settings and automatically adjusts sensitivity as filter learns bias
2014-05-16 21:24:25 +10:00
priseborough 0337d44b2e AP_NavEKF: Fix bug causing immediate clearing of diverged status on reset 2014-05-16 21:24:04 +10:00
priseborough d150904dc6 AP_NavEKF: Increase gyro bias process noise
This is required to stop the bias estimate from becoming too static
towards the end of longer flights.
2014-05-16 21:24:04 +10:00
Andrew Tridgell 5edf4ba596 AP_NavEKF: fixed up handling of bitfields in faultStatus
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 21:42:39 +10:00
priseborough ce8e1daa16 AP_NavEKF: Improved divergence detection and reset
Divergence is now detected by looking for very large changes in the gyro
bias. This will cause the filter to be reset and declared unhealthy for
10 seconds.

Don't reset filterDiverged status immediately during reset

Set filterDiverged true if covariance blows up

Add fault status reporting
2014-05-15 21:14:21 +10:00
Andrew Tridgell 85ebe81ed3 AP_NavEKF: fixed logic for divergence test
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-11 18:00:38 +10:00
priseborough 40dc0e56fe AP_NavEKF: Add protection for badly conditioned innovation variances 2014-05-11 18:00:38 +10:00
priseborough 925c5751bd AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding
This will enable in-flight magnetometer calibration to be inhibited unconditionally,
This is required for long balloon carriage flights where ground speed can be high
enough to put it into in-air state, but with very poor observability of magnetic field
states causing bad state estimates and heading offsets to develop over time.

The covariance matrix no longer has rows and columns artificially zeroed when in static
mode. Instead booleans indicating whether wind or magentic field state estimation is
active are used to:

a) Set the process noise on these states to zero to stop their variances from increasing
unchecked when not being updated, and
b) Turn off updates for these states when measurement fusion is being performed.

This reduces the likelihood of a badly conditioned covariance matrix forming
during static mode operation.

A filter divergence check has also been added that will declare the filter unhealthy if
position, velocity and magnetic field observations are all failing their innovation
consistency checks. This unhealthy status will persist for 10 seconds after the
condition clears.

AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-05-11 18:00:38 +10:00
priseborough 3026ccee13 AP_NavEKF: Use baro data during launch transients whilst in static mode
Baro data can be used as it will still be valid during the launch and
is not a synthesised measurement.
2014-05-11 18:00:38 +10:00
priseborough 114bd56e2a AP_NavEKF: Fix display names in parameter list 2014-04-30 06:35:41 +10:00
priseborough 5fefce5899 AP_NavEKF: Synthetic sideslip fusion numerical error protections 2014-04-29 17:26:45 +10:00