* 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>
High rate optical flow data could make flow fusion to run every major update cycle, resulting in the calculation of bias errors in the body rates used to compensate flow data failing time validity checks and not running. This resulted in a slow drift of the nav solution if bias errors were present in the in the gyro data used for flow sensor motion compensation.
Everywhere where KHP is used, it is first completely reset, thus making
it unnecessary to keep it as a class member.
This saves 2.3KB RAM.
Stack sizes don't need changing, since there is already a function
Ekf::predictCovariance(), which needs around 3KB of stack and is called
close to where the fuse* functions are called.
All the decision for a sensor are made within a specific function for that sensor and when there is data to process at the fusion time horizon.
Information and warning messages are improved.
Combines the forced symmetry, variance limiting and zeroing of covariances for unwanted states in the one function.
This ensures a consistent correction is applied after every covariance prediction or correction.
Roman Bapst
Paul Riseborough
Mohammed Kabir
Daniel Agar
Beat Küng