Systems and methods for control state estimation of vehicles based on force disturbance signals

A computing system, comprising:

• a memory storing instructions;
• and
• a processor coupled to the memory, the processor is configured to execute instructions of a software program to:
• initialize driving operations of an autonomous vehicle;
• receive sensor signals from a sensor system of the autonomous vehicle;
• determine localization of the autonomous vehicle based on the sensor signals, including location of tires of the vehicle;
• determine a set of parameters including vehicle pose estimation, velocity, and acceleration based on the localization and the sensor signals;
• interpolating a previous plan of a path planning system to a current time and identifying a set of expected values, including vehicle pose estimation, velocity, acceleration, roll, pitch, and yaw rate, based on the interpolated previous plan, wherein the measurement update module generates an initial state by applying a filter to the comparison signals and the expected initial state;
• comparing the vehicle pose estimation, the velocity, and the acceleration to the expected values from the previous plan of the path planning system to generate comparison signals;
• and
• determine force disturbance signals including lateral force disturbances for front and rear axle lateral accelerations and a total longitudinal force disturbance based on the localization of the autonomous vehicle and the sensor signals, and adjusting a control model of the vehicle based on the determined force disturbance signals;
• and
• controlling the vehicle using the control model.

A non-transitory computer readable storage medium having embodied thereon a program, wherein the program is executable by a processor to perform a method comprising:

• initializing driving operations for a vehicle;
• obtaining sensor signals from a sensor system of the vehicle and determining localization of the vehicle based on the sensor signals, including location of tires of the vehicle;
• determining vehicle pose estimation, velocity, acceleration, road bank angle, and road grade angle based on the localization of the vehicle and the sensor signals, including location of tires of the vehicle
• interpolating a previous plan of a path planning system to a current time and identifying a set of expected values, including vehicle pose estimation, velocity, acceleration, roll, pitch, and yaw rate, based on the interpolated previous plan, wherein the measurement update module generates an initial state by applying a filter to the comparison signals and the expected initial state;
• comparing the vehicle pose estimation, the velocity, and the acceleration to the expected values from the previous plan of the path planning system to generate comparison signals;
• determining lateral acceleration limits and force disturbance signals including front and rear axle lateral accelerations (ayf, ayr)) and a total longitudinal force disturbance based on the localization, road bank angle, and road grade angle;
• adjusting a control model of the vehicle based on the determined force disturbance signals;
• and
• controlling the vehicle using the control model.