Fixes a problem observed in a flight log where rapid temperature change caused the accel bias to change faster than the EKF could keep up.
This allows the bias to be learned faster but with acceptable level of noise in the estimate
IMU data was being corrected before being used by the co-variance prediction, whereas the delta angles and velocities in the derivation were supposed to be uncorrected.
This patch creates separate variable for the corrected data
Magnetometer bias states will subject to larger errors early in flight before flight motion makes the offsets observable and the state variances reduce.
Adds a check on state variances.
Replaces the parameter check with a check of the actual filter fusion method being used.
Allow different process noise to be set for body (sensor bias) and earth field states.
This allows a stable magnetometer bias estimate to be available at end of flight whilst still allowing for external magnetic anomalies during landing.
Adjust default values to give stable mag bias learning and fast learning of external anomalies.
Automatically use the highest gain consistent with a 5% overshoot to minimise RMS tracking errors.
Provide an alternative correction method for the position and velocity states that allows the user to specify the time-constant. This can be used to fine tune the output observer for for platform specific sensor errors and control loop sensitivity estimation noise.
The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe.
A check for yaw errors due to a ground based magnetic anomaly has been introduced.
The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags.
Reduce the number of places where decisions to perform a yaw and field reset are made.
Don't perform a reset unless there is is data in the buffer
Don't use 3-axis fusion if the field states still need to be reset.
When starting 3-axis fusion request a reset if not previously performed.
Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
The commencement of airspeed fusion could cause pitch errors due to small variances and large innovations. This issue is addressed by the following changes:
1) The airspeed measurement is used to set wind states to a value that reduces initial innovations.
2) The wind state variances are set to values that better reflect the wind speed uncertainty
The function used to reset magnetic field states and yaw angle should not be used when there is no magnetometer. If it is incorrectly called without a magnetometer it should not change the attitude or field states.
When changing the vehicle yaw angle, the correlation between the attitude errors and errors in other states is invalid so the corresponding co-variance terms need to be zeroed.
This needs to be done in more than one place.
M_NSE is a measurement noise
P_NSE is a observation noise
I_GATE is an innovation gate
This also ensures the new parameter values required to use the EKF2 will be enforced.
The new function can deal with a variable number of function parameters.
Additionally, I renamed the functions to norm(), because this is the
standard name used in several other projects.
Implements the following techniques to enable planes to operate without magnetometers.
1) When on ground with mag use inhibited, a synthetic heading equal to current heading is fused to prevent uncontrolled covariance growth.
2) When transitioning to in-flight, the delta between inertial and GPS velocity vector is used to align the yaw.
3) The yaw gyro bias state variance is reset following an in-flight heading reset to enable the yaw gyro bias to be learned faster.
Use an Euler yaw heading that switches between a 321 and 312 rotation
sequence to avoid areas of singularity. Using Euler yaw decouples the
observation from the roll and pitch states and prevents magnetic
disturbances from affecting roll and pitch via the magnetometer fusion
process.
The use of yaw angle fusion during startup and ground operation causes problems with tail-sitter vehicle types.
Instead of observing an Euler yaw angle, we now observe the yaw angle obtained by projecting the measured magnetic field onto the the horizontal plain.
This avoids the singularities associated with the observation of Euler yaw angle.
The innovation calculation should have been updated when the heading fusion maths was updated.
We now use a direct heading or yaw angle measurement in the derivation, not the difference between observed and published declination.
This removes a legacy design concept that is no longer required in this filter implementation. Planes will not be armed without EKF aiding and the proposed copter throw mode also requires EKF aiding to be operating.
The other problem with interrupting fusion during the launch is it doesn't reduce the corrections, it just delays them as wen the launch completes, the EKF inertial position estimate is still moving still moved and the corrections are therefore just delayed by the short launch interval.
Thank you to OXINARF for picking up the inconsistency with the previous logic
Change to user adjustable fusion of constant position (as per legacy EKF) instead of constant velocity.
Enable user to specify use of 3-axis magnetometer fusion when operating without aiding.
Don't allow gyro scale factor learning without external aiding data as it can be unreliable
Sensor bias corrections were being applied to the incoming IMU data using the wrong delta time.
This was what was driving the different tuning between plane and copter for gyro bias process noise so the same gyro bias process noise default tuning value can now be used for all platform types.
Sensor bias corrections were being applied a a second time to the output observer inertial data.
Eliminate the use of horizontal position states during non-aiding operation to make it easier to tune.
Explicitly set the horizontal position associated Kalman gains to zero and the coresponding covariance entries to zero after avery fusion operation.
Make the horizontal velocity observation noise used during non-aiding operation adjustable.
Use a fixed value of velocity noise during initial alignment so that the flight peformance can be tuned without affecting the initial alignment.
The non-GPS mode was not being activated for small height gains - eg indoor flight.
The incorrect innovation consistency check was being applied to the synthetic velocity observations.
The problem with using min() and max() is that they conflict with some
C++ headers. Name the macros in uppercase instead. We may go case by
case later converting them to be typesafe.
Changes generated with:
git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)max(/\1MAX(/g'
git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)min(/\1MIN(/g'
The airspeed observation buffer was only being checked when new data arrived instead of every frame which introduced some timing jitter. The buffer is now checked every filer update step.
The duplication and inconsistent naming of booleans used to indicate availability f data has been fixed.
These changes were pair coded an tested by Siddharth Purohit and Paul Riseborough
Fix indexing errors
Move buffer code into a separate file
Split observer and IMU/output buffers and remove duplicate sample time
Optimise observation buffer search
Reduce maximum allowed fusion age to 100 msec
This revised threshold value is still double the maximum that has been observed in flight logs so far with healthy sensors
The previous value was too slow to switch for sudden IMU gyro faults
We can afford an ocasional false trigger becasue the front end will only select another instance if it is healthy and has lower errors
The ad-hoc scaling of error growth has been replaced with a consistent method that uses the main nav filters published vertical velocity uncertainty and the terrain gradient assumption.
GPS height has been added as a measurement option along with range finder and baro
Selection of the height measurement source has been moved into a separate function
Each height source is assigned its own measurement noise
If GPS or baro alt is not able to be used, it reverts to baro
When baro is not being used, an offset is continually calculated which enables a switch to baro without a height step.
The copter method was being used for plane and the plane method was not being run due to the change in flight status not being detected.
The plane reset method did not trigger if the EKF had already dragged the velocity states along with the GPS or could align to an incorrect heading.
The method has been reworked so that it resets to the GPS course, but only if there are inconsistent angles and large innovations.
To stop a failed magnetometer causing a loss of yaw reference later in flight, if all available sensors have been tried in flight and timed out, then no further magnetoemter data will be used
Down-sample the IMU and output observer state data to 100Hz for storage in the buffer.
This reduces storage requirements for Copter by 75% or 6KB
It does not affect memory required by plane which already uses short buffers due to its 50Hz execution rate.
This means that the EKF filter operations operate at a maximum rate of 100Hz.
The output observer continues to operate at 400Hz and coning and sculling corrections are applied during the down-sampling so there is no loss of accuracy.
Large magnetometer innovations on the ground could be caused by factors that will disappear when flying, eg:
a) Bad initial gyro bias
b) External magnetic field disturbances (adjacent metal structures, placement of hatches with magnets, etc)
To avoid unnecessary switches, we inhibit switching until off-ground and when sufficient time has lapsed from power on to learn gyro bias offsets.
If the magnetometer fails innovation consistency checks for too long (currently 10 sec), then the next available sensor approved for yaw measurement will be used.
The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis.
This was not achieved with the previous implementation.
These changes move the innovation consistency checks for all three axes to the top before any axes are fused.
Unnecessary performance timers have been removed.
This was problematic to implement with magnetometer switching. It is likely that slow magnetometer learning can still be performed externally (eg plane) but this will need to be monitored to see if it causes issues.
The setting of the EKF origin and the entry into GPS aiding mode have been separated to make the logic clear.
The order of operations has been changed to ensure that when a reset to GPS is performed, a valid GPS measurement is available in the buffer
Declaration of GPS availability is not made unless the GPS data has been entered into the buffer
Only applied to interfaces required for data logging.
If an invalid instance is requested, the data for the primary instance is returned. This allows the primary data to be returned by calling with a -1 instance value.
priseborough
Jonathan Challinger
Andrew Tridgell
Tom Pittenger
Ricardo de Almeida Gonzaga
dgrat
Lucas De Marchi
Peter Barker
Don Gagne
Siddharth Purohit and Paul Riseborough
Siddharth Bharat Purohit
Caio Marcelo de Oliveira Filho
Paul Riseborough