Abstract: A system for estimating a trajectory of a vehicle includes vehicle state sensors detecting a state of movement of the vehicle and environment sensors monitoring an environment of the vehicle. A free space module determines a free space corridor based on the detected state of movement of the vehicle and the monitored environment of the vehicle. A goal point determination module determines at least one goal point for the trajectory to be estimated based on the free space corridor. A trajectory planning module estimates a reference trajectory based on the at least one goal point and calculates a deviation between an actual trajectory and the reference trajectory of the vehicle. A trajectory control module minimizes the deviation between the actual trajectory and the reference trajectory of the vehicle and outputs optimal control parameters for the movement of the vehicle based on the minimized deviation.