Linear Quadratic Regulator (LQR). Riccati equation solver in C++.
The Linear Quadratic Regulator (LQR) is a classic control technique used to design optimal control laws for linear systems.
This article explains the mathematical foundation for LQR and demonstrates how to solve a control problem using an inverted pendulum on a cart as an example in C++. Additionally, it shows how to numerically solve the Riccati equation to obtain the optimal state-feedback control law and gain matrix K for the control inputs.
The source code for this article you will find on my GitHub.
The matrix K is an essential matrix in the control system, especially connected with pole placement (eigenvalues) and affects the dynamic behavior of the system, including stability, transient response, and Bode plot characteristics.
LQR aims to minimize a quadratic cost function similar to MPC (see my previous article).
Here’s the formulation of LQR:
Given a discrete-time LTI system represented as:
where
- xk is the state vector at time step k.
- uk is the control input vector at time step k.
- A is the state transition matrix.
- B is the input matrix.
The goal of LQR is to minimize the following quadratic cost function:
subject to the system dynamics, where Q and R are positive semi-definite matrices. The difference here is that the summation extends to infinity, indicating that the cost is being minimized over an infinite time horizon.
The optimal control input uk in LQR is given by a linear state feedback control law:
where K is the optimal control gain matrix.
The optimal control gain can be obtained by solving the discrete-time algebraic Riccati equation:
where P is the solution to the Riccati equation. Once P is obtained, the optimal control gain K is given by:
The example I solve can be formulated as follows,
A complete description of the model I used, you can find here.
Consider my implementation in C++.
Here is the output from the simulation you can run on your own.
Thank you for reading.
