* 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>
