Clarify documentation for ToF laser ranging

[nikhil-sethi]

This basic PR relates to #1239 and adds comments to make the ToF laser measurement model clear. The image for the online documentation has also been changed to reflect the concerned formula and diagram from the thesis.

[hmllr]

Hi! Thanks for your pull request. However, it seems as if the documentation is now explaining the case of theta_px going towards zero, which is not the case with our sensor (it unfortunately is in some commented out code/comment which should be removed). Do you want to adapt your PR to explain the used measurement model (6.53 in the masterthesis but assuming no pitch/roll over theta_px / 2 (or adding this back in, most of the code for it is still there but mostly commented out))? Or should I go ahead and clean up the misleading comments?

[nikhil-sethi]

Hey @hmllr!
Thanks for checking the PR out. The commented-out code and the funky history of this file (https://github.com/bitcraze/crazyflie-firmware/commits/557dc37f7c8f215e10360f8a0b53372c94d7be3b/src/modules/src/estimator_kalman.c) tripped me up while writing the doc, but your explanation did make it somewhat clear.
It seems like the cos(angle) was removed at some point because of a small aperture approximation and a NaN bug (f3ba4c0) but were added mistakenly without any effect here (557dc37). Would it be right to assume that the small angle approximation is still being used (as the angle variable is unused)? I can't test physically unfortunately but would love to help in correcting this. Feel free to make any mods to the PR as well!

[hmllr]

I think it is not the small angle approximation that is used (ok, it is an assumption that the angle is small, but not in the classical way where you then leave away the cosine) - it is used that the VL sensor always returns the closest point it sees, and the FoV is 27degrees for the VL53L1X (or 25degrees for the older VL53L0X - I have no idea where the 15degree in the commented out code are coming from), so the drone hardly tilts enough to not return the closest distance to the ground.
This means there are two possibilities in how to clean this:
a) assume we always tilt less than 13.5degrees and removing all this angle stuff
b) the more correct way would be to add it back in. I haven't tested it though and am not 100% sure.
Does this make things a bit clearer?

I am also linking issue #474 here, very old and related.

[hmllr]

Actually, let me correct myself. I think we are currently rotating everything to body frame, even though we should not up to 13.5degree of pitch/roll. So I looked into why this was only partly there - turned out it was removed in 2016 for NaN issues in the acos (issue #182) and only partly re-introduced in 2017 (I guess by mistake, issue #239).