depth map working
Optimization: only estimate a position, if we really had an update or observation