Real-time estimation of a vehicle’s path

When driving at low speed, a kinematic vehicle movement model–where the path is modeled with a circle on the center of the real axle–is used.? A curvature difference between the curvature of the desired circular path and the curvature of the estimated circular path is sometimes a term in the steering control.? Articles such as Least squares fitting of circles, Chernov et. al. present methods to estimate a circle from observations that span > 10~20? arc.? A problem with estimating the circle of a slow moving vehicle is the small arc that will be encompassed in a fixed size history of the vehicle pose.

Consider the most recent pose estimate of a vehicle (P0, ??0) at time T0, with sufficient distance (say Dmin) away from a previous pose (P1, ??1) at time T1, as shown above. Under the assumption that the movement is on a circular path, the heading change from time T1 to T0 is ?1=??0–??1.? Since the elapsed time is T0–T1, so the rotation rate is ?1/(T0-T1).? The distance is proportional to this heading change: D1=2 ??1 tan(?1/2), so that the radius can be estimated: ??1 = D1/{2 tan(?1/2)}.? From this radius, the relationship between the center of the circle and current position is (x0, y0) =(x1+cos(90?–??0), y1–sin(90?–??0)) = (x1+sin(??0), y1–cos(??0)) so that the circle center (x1, y1) = (x0–sin(??0), y0+cos(??0)).

This calculation can be stored into history, and then averaged or filtered.