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.
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 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'
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
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.
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.
Apply filtering to baro innovation check and and don't apply innovation checks once aiding has commenced because GPS and baro disturbances on the ground and during launch could generate a false positive
Prevents frame over-runs due to simultaneous fusion of measurements on each instance.
The offset is only applied if less than 5msec available between frames
Use the more robust, but less accurate compass heading fusion up to 5m altitude
Wait for the magnetometer data fusion time offset to be correct before using data to reset states
Don't reset magnetic field states if the vehicle is rotating rapidly as timing offsets will produce large errors
When doing the yaw angle reset, apply the reset increment to all quaternions stored in the output buffer to avoid transients produced by yaw rotations and the 0.25 second fusion time horizon offset.
Only do the one yaw and mag reset at 5m, not two at 1.5 and 5.0m
Always re-do the yaw and mag reset when leaving the ground.
Because we have changed the yaw angle and have taken a point sample on the magnetic field, covariances associated with the magnetic field states will be invalid and subsequent innovations could cause an unwanted disturbance in roll and pitch.
The reset of the Euler angles to a new yaw orientation was being done using roll and pitch from the output observer states, not the EKF state vector which meant that when roll and pitch were changing, the reset to a new yaw angle would also cause a roll and pitch disturbance.
The legacy EKF switches GPs aiding on on arming, whereas the new EKF switches it on based on GPS data quality.
This means the decision to arm and therefore the predicted solution flags must now reflect the actual status of the navigation solution as it will no longer change when motor arming occurs.
If high vibration levels cause offsets between the two, it switches to the accelerometer with lower vibration levels. The default behaviour is to use the average of both accelerometers.
Don't fuse other measurements on the same frame that magnetometer measurements arrive if running at a high frame rate as there will be insufficient time to complete other operations.
