Documentation for the LQR Steering Control Model
Overview:
The LQR (Linear Quadratic Regulator) steering control model implemented in lqr_steer_control.py provides a method for autonomous vehicles to track a desired trajectory by adjusting steering angle based on feedback from the current state and the desired trajectory. This model utilizes a combination of PID speed control and LQR steering control to achieve smooth and accurate trajectory tracking.

Components:
State Representation:
The state of the vehicle is represented by its position (x, y), orientation yaw, and velocity v.

Control Inputs:
The control inputs to the system are the acceleration a and steering angle delta.
Model Parameters:

Various parameters are used in the model, including the wheelbase L of the vehicle, the maximum steering angle max_steer, time step dt, and speed proportional gain Kp.
LQR Parameters:

The LQR controller is parameterized by the matrices Q and R, which define the cost function for the state and control inputs, respectively.
Implementation Details:
The model calculates the optimal steering angle delta using LQR control based on the current state and the desired trajectory.
It linearizes the dynamics of the system around the current state to obtain the state-space representation.
The discrete-time Algebraic Riccati equation (DARE) is solved to obtain the optimal control gain.
The control input delta is determined by a combination of feedforward and feedback components.
Additionally, a PID controller is used to adjust the vehicle's speed to match the desired speed profile along the trajectory.
Usage:
The model can be integrated into autonomous vehicle control systems for path tracking applications.
Input parameters include the desired trajectory (cx, cy, cyaw, ck), speed profile, and simulation parameters.
The main() function demonstrates the usage of the model by simulating the vehicle's trajectory tracking behavior.
Derivation Steps for the Linearized A, B Matrix
The linearized A, B matrix is derived to represent the state-space model of the vehicle dynamics. Here are the steps involved:

State Space Representation:

Define the state vector x comprising position, orientation, and velocity.
Define the control input vector u comprising acceleration and steering angle.
State Transition Model:

Formulate the state transition equation x_next = A * x + B * u to represent the dynamics of the system.
Linearization:

Linearize the state transition equation around the current state using Taylor series expansion.
Obtain the Jacobian matrices A and B, representing the partial derivatives of the state transition equation with respect to the state and control inputs, respectively.
Discretization:

Convert the continuous-time state-space model to discrete-time by discretizing the dynamics using appropriate integration methods.
LQR Control Design:

Apply the LQR control design methodology to obtain the optimal control gain matrix K by solving the discrete-time Algebraic Riccati equation (DARE).
Implementation:

Utilize the obtained matrices A, B, and K in the control algorithm to compute the optimal control input for the given state and desired trajectory.

By following these steps, the linearized A, B matrix can be derived and integrated into the control algorithm to achieve effective trajectory tracking performance.

@Gopigunaganti Thank you for great explanation!!.
I think updating the document is good idea: https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/lqr_steering_control/lqr_steering_control.html
So, PR is welcome.

ok ! ,thanks for the reply .

@Gopigunaganti Thanks for your full explanation of the control model !
But it seems that the actual states for LQR used in the code are not (x,y,yaw,v) but (traj_err, traj_err_dot, yaw_err, yaw_err_dot).
Could you please explain in more detail how this A/B matrix was derived?

    x[0, 0] = e
    x[1, 0] = (e - pe) / dt
    x[2, 0] = th_e
    x[3, 0] = (th_e - pth_e) / dt

    A = np.zeros((4, 4))
    A[0, 0] = 1.0
    A[0, 1] = dt
    A[1, 2] = v
    A[2, 2] = 1.0
    A[2, 3] = dt

    B = np.zeros((4, 1))
    B[3, 0] = v / L

@lijieamd I have also encountered the same issue. Have you now understood the state-space equations for it?

@AtsushiSakai Hello, I saw that you used the kinematics modeling of the vehicle in the LQR, and it seems that establishing the following equation for control can make the lateral error smaller (and I still don't understand how your current kinematic model is built)：
x = np.zeros((3, 1))
x[0,0] = state.x-cx[ind]
x[1,0] = state.y-cy[ind]
x[2,0] = th_e

# A = [1,  0,  -v*dt*sin(heading_r);
#      0,  1,  v * dt * cos(heading_r);
#      0,  0,  1]
A = np.zeros((3, 3))
A[0,0] = 1
A[0,2] = -v*dt*math.sin(cyaw[ind])
A[1,1] = 1
A[1,2] = v*dt*math.cos(cyaw[ind])
A[2,2] = 1

# B = [dt * cos(heading_r),    0;
#      dt * sin(heading_r),    0;
#      dt * tan(delta_r)/L,  v*dt/(L * cos(delta_r)^2)];
B = np.zeros((3, 2))
B[0,0] = dt*math.cos(cyaw[ind])
B[1,0] = dt*math.sin(cyaw[ind])
B[2,0] = dt*math.tan(dl_r)/L
B[2,1] = v*dt/(L*math.cos(dl_r)**2)

Let me explain it in the docs by PR #1015