Cool. Well, now all that remains, unless I misunderstood something, is to refine that with the Kalman filter (see here).