This ensures that when we start using GPS, that the EKF will be using the correct declination for that location
If declination is not known it defaults to zero
This prevents bad inertial or GPS data combined with the post takeoff heading alignment check used by plane from resulting in earth field states that have an incorrect declination
This patch reworks the in-air transition criteria to reduce the likelihood of false positives and to ensure that there will be enough ground speed to make the heading check work reliably.
This check will declare the EKF as unhealthy if the horizontal position innovations exceed a threshold before motors are armed.
This will help to prevent a takeoff with bad inertial data caused by bad accel or gyro offsets.
Prolonged yaw rotations with gyro scale factor errors can cause yaw errors and gyro bias estimation errors to build up to a point where EKF health checks fail.
This patch introduces the following protections:
1) The assumed yaw gyro error is scaled using a filtered yaw rate and an assumed 3% scale factor error (MPU6000 data sheet)
2) When the filtered yaw rate magnitude is greater than 1 rad/sec, the Z gyro bias process noise is zeroed and the state variance set to zero to inhibit modification of the bias state
3) When the filtered yaw rate magnitude is greater than 1 rad/sec, the magnetometer quaternion corrections are scaled by a factor of four to maintain tighter alignment with the compass
Increases magnetometer weighting on yaw corrections when there there is no other aiding to constrain yaw drift.
Prevents switch to GPS if magnetometer data is failing innovation checks which indicates a bad yaw angle
The interface definition has been modified so that it returns true for a position obtained usin geither the normal inertial navigation calculation, or a raw GPS measurement.
This enables this function to be used to set a home position before flight.
If a calculated position is not available, the function will return a value based on raw GPS or last calculation if available, but the status will be set to false to indicate that it cannot be used for control.
The inconsistent baro data during ground effect takeoff combined with the larger variances in the Z accel bias state early in flight can cause unwanted changes in bias estimate and therefore changes in height estimation error.
This patch turns of the process noise and state updates for the Z accel bias state when takeoff in ground effect is expected.
Thsi fixes a potential bug where the vehicle could land at a lower location without disarming and re-enter ground effect takeoff mode wiht a baro height floor above the current altitude, causing unpredictable height gain
Ground effect baro errors can cause a spike in height innovation on disarming if ground effect compensation was used during the landing. This causes a transient AHRS fault message if this innovation is outside the pre-arm check limits.
Resetting the vertical position state to the measured height after disarming prevents this.
This estimate is used to offset baro data if we need to switch across from range finder data due to sensor failure. The previous filter coefficients gave a 0.5 seconds time constant on the offset which was too susceptible to baro noise.
Range finder and optical flow data can drop-out and be reliable very close to ground. these patches enable the takeoff to be more relaibly detected and constrain optical flow navigation drift in the first part of takeoff.
Falls back to baro if range finder is unavailable
Adds parameter enabling user to select which height source (baro or range finder) will be used during optical flow nav.
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.
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
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.
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.
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.
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.
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
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.
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.
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.
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.
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.
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.
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.
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.
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.
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.
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).
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.
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.
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
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.
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.
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.
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.
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
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
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.
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)
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.
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.
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.
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
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.
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
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
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.
Andrew Tridgell
Paul Riseborough
Tom Pittenger
Jonathan Challinger
Randy Mackay
Arthur Benemann
myly10
Grant Morphett