Techniques for compensating for errors in position of a vehicle are discussed herein. In some cases, a discrepancy may exist between a measured state of the vehicle and a desired state as determined by a system of the vehicle. Techniques and methods for a planning architecture of an autonomous vehicle that is able to provide maintain a smooth trajectory as the vehicle follows a planned path or route. In some cases, a planning architecture of the autonomous vehicle may compensate for differences between an estimated state and a planned path without the use of a separate system. In this example process, the planning architecture may include a mission planning system, a decision system, and a tracking system that together output a trajectory for a drive system.