Update the implementation of the camera measurement in the filter.

Somehow the values are in the order of 1e-45 for the distance and 10 for
the heading. That's obviously not correct yet...

/cc @raduf517
Add more comments and some cleanup