Automated vehicle drift correction

Claim 1

A navigation system for a wheeled vehicle having steering means and means for controlling the steering means in response to a stored program, said vehicle in operation having a heading angle and a spatial position within a predetermined coordinate frame of reference, and a bearing and a range to a target at a known predetermined position within said frame of reference, said navigation system comprising: (a) means for determining a linear distance traveled by the vehicle and an amount of rotation of the vehicle about a predetermined center of rotation during each of a plurality of successive time intervals; (b) means coupled to said determining means for producing, in response to said linear distance and said amount of rotation determined during each said time interval, updated estimates of said heading angle and said spatial position of the vehicle within said predetermined coordinate frame of reference; (c) means coupled to said updated estimate producing means for determining at least one of said bearing and said range to a target at a known predetermined position within said reference frame, and for deriving an error signal as a difference between an actual measured respective one of said bearing and said range and a respective one of said bearing and said range that should have been measured if the vehicle was actually at the estimated position with the estimated heading; and (d) filter means responsive to said error signal for producing corrected estimated values of said heading angle and of said spatial position of said vehicle and also for producing a corrected estimated value of at least one further parameter of said vehicle for substantially continuously calibrating a respective one of said means (a), (b) and (c).

Claim 9

A method of controlling an automated wheeled vehicle, comprising the steps of: (a) sensing a linear and angular displacement of the vehicle in each of a plurality of successive time intervals; (b) updating estimates of a current position and heading of the vehicle after each said successive time interval in response to the sensed linear and angular displacements; (c) taking at least one of a bearing and a range from the vehicle to a target at a known position; (d) calculating from the estimated position and heading of the vehile at least one of an expected range to said target and an expected bearing to said target at the time at which it is taken; (e) deriving an error signal as a difference between respective ones of the expected and measured ranges and bearings; (f) filtering the error signal to produce corrected estimates of the position and heading and of at least one further parameter used in at least one of the steps (a) and (b) which is subject to an error; and (g) using said at least one further parameter to substantially continuously correct a respective one of steps (a), (b), (c) and (d).

Claim 10

A method of controlling an automated wheeled vehicle, comprising the steps of: (a) sensing a linear and angular displacement of the vehicle in each of a plurality of successive time intervals; (b) updating estimates of a current position and heading of the vehicle after each said successive time interval in response to the sensed linear and angular displacements; (c) taking at least one of a bearing and a range from the vehicle to a target at a known position; (d) calculating from the estimated position and heading of the vehile at least one of an expected range to said target and an expected bearing to said target at the time at which it is taken; (e) deriving an error signal as a difference between respective ones of the expected and measured ranges and bearings; (f) filtering the error signal to produce corrected estimates of the position and heading and of at least one further parameter used in at least one of the steps (a) and (b) which is subject to an error; and (g) using said at least one further parameter to substantially continuously correct a respective one of steps (a), (b), (c) and (d).