Modify body Jacobian F. Junho has an identity in the db/db term, which is not
consistent with the handling of X and V.

Update covariance based on known estimate (GPS aided)

Dynamically adjusted state size. We can add and delete features as needed.
Junho's code requires a fixed state size.

Treat measurements as independent. Compute S_i individually for each measurement
and compose S as block diagonal for speed improvement.

Use quaternion reflection instead of Householder Transformation for determining
the predicted reflection measurement.

Below here is not yet implemented:

Add initial reflection view.

Junho sets all features to a fixed initial depth e.g 10m, 20m. I calculate the
initial depth from the reflection information. Better initialization should give
better results, because we can decrease P0 and rely in vision more.