* Fix comment
* Ekf wrapper for testing
Add utility function for accessing information in the ekf object
* Add step function for Gps sensor
* Add RangeFinder and Flow to simulated sensors
* Add first fusion logic tests
* Add units to function name
* Use EXPECT_TRUE
* Adding missing qualifiers
* Improve EXPECT_ calls
* Improve naming
This commit introduces a quality measure in the range data. It is
used to properly decide whether to initialize the HAGL estimate on
sensor data or MIN_HGT parameter, as well as in the decision of
whether a 'fake' measurement should be published on the ground to
allow for optical flow take-offs.
The handling of invalid flow data when on ground is performed in controlOpticalFlowFusion() where it is able to handle flow sensors that don't publish gyro data.
Heading data is assumed to be from a dual antenna array at a specified yaw angle offset in body frame, but with the heading data already corrected for antenna offset. The offset is required to apply the correct compensation for combined rotations and to determine when the yaw observation has become badly conditioned.
Motion compensated optical flow rates are supposed to be zeroed if reported flow quality is below the minimum threshold value when on ground.
The comments and logic have been amended to be consistent and make the design intent clearer.
* optical flow: fixed calculation of velocity of the flow sensor relative to
the IMU
- gyroXYZ holds a delta angle and first needs to converted to a gyro rate
Signed-off-by: Roman <bapstroman@gmail.com>
* optical flow: calculate height above the ground with respect to the flow
camera
- the flow camera can be offset from the IMU which needs to be considered
Signed-off-by: Roman <bapstroman@gmail.com>
* estimator interface: fixed comment regarding optical flow sample timestamp
- the timestamp on an optical flow sample corresponds to the trailing
edge of the flow integration period
Signed-off-by: Roman <bapstroman@gmail.com>
* EKF collect_imu take const imu sample and populate buffer
* EKF calculateOutputStates cleanup
* EKF add calculate_quaternion output predictor method
* EKF: update documentation
* EKF: remove unnecessary getter function
* EKF calculateOutputStates only apply dt correction to bias
* EKF pytest assert attitude validity, not update() return
* EKF: correct documentation
* EKF: Do not make attitude validity dependent on yaw alignment status
Yaw alignment could fail in flight due to temporary loss of data and yet the quaternions would still usable for stabilisation even though the absolute earth yaw angle wrt true north was uncertain.
The gyro data accumulation needs to be across the same integration period as the flow sensor. The previous code didn't sample the accumulation until the midpoint of the flow data had fallen behind the fusion time horizon.
This PR changes the optical flow time stamp definition so that flow data is retrieved when the leading edge of the flow accumulation period falls behind the fusion time horizon. This enables the accumulated gyro data to be sampled at the correct time. Fusion is then delayed until the mid sample time has fallen behind the fusion time horizon.
Remove duplicate checking for dead reckoning and consolidate into a single function.
Use separate timers to check for start of dead reckoning and check when dead reckoning has been performed for too long for the nav solution to be valid.
Allow the timeout for validity reporting to be adjusted externally.
Separate external reporting of dead reckoning from internal checks.
This enables the EKF to use an additional NE velocity measurement. This can be used to improve position hold stability when landing using a beacon system for positioning by fusing the beacon velocity estimates.
Add calculation of a vertical position derivative to the output predictor. This will have degraded tracking relative to the EKF states, but the velocity will be closer to the first derivative of the position and reduce the effect inertial prediction errors on control loops that are operating in a pure velocity feedback mode.
Move calculation of IMU offset angular rate correction out of velocity accessor and into output predictor.
Provide separate accessor for vertical position derivative.
kamilritz