update
Calculates the optimal control input (accelerations) to correct for the given state error.
Return
The calculated optimal control accelerations as a Matrix. The matrix will have dimensions 3 x 1: ax, ay, alpha.
Parameters
error
The current state error of the system, represented as a Matrix. Expected dimensions: num_states x 1. Order of states MUST match the LQR's design: dx, dy, dtheta, dvx, dvy, domega.
Calculates the optimal control input to correct for the given state error.
Return
The calculated optimal control acceleration.
Parameters
error
The current state error of the system, represented as a dual twist (position error AND velocity error).