diff --git a/appendices/linear-quadratic-regulator/derivation.tex b/appendices/linear-quadratic-regulator/derivation.tex index 2f96f33..177b919 100644 --- a/appendices/linear-quadratic-regulator/derivation.tex +++ b/appendices/linear-quadratic-regulator/derivation.tex @@ -122,10 +122,16 @@ \section{Derivation} \mat{Q} = 0 \end{equation*} -Snippet \ref{lst:lqr} computes the infinite horizon, discrete time LQR. +Snippets \ref{lst:lqr_cpp} and \ref{lst:lqr_py} compute the infinite horizon, +discrete time LQR. +\begin{coderemote}{cpp}{snippets/LQR.cpp} + \caption{Infinite horizon, discrete time LQR solver in C++ (see subsection + \ref{subsec:dare} for DARE solver)} + \label{lst:lqr_cpp} +\end{coderemote} \begin{coderemote}{Python}{snippets/lqr.py} \caption{Infinite horizon, discrete time LQR solver in Python} - \label{lst:lqr} + \label{lst:lqr_py} \end{coderemote} Other formulations of LQR for finite horizon and discrete time can be seen on diff --git a/modern-control-theory/linear-algebra/common-control-theory-matrix-equations.tex b/modern-control-theory/linear-algebra/common-control-theory-matrix-equations.tex index 1100022..deb60bc 100644 --- a/modern-control-theory/linear-algebra/common-control-theory-matrix-equations.tex +++ b/modern-control-theory/linear-algebra/common-control-theory-matrix-equations.tex @@ -15,6 +15,7 @@ \subsection{Continuous algebraic Riccati equation (CARE)} \end{equation} \subsection{Discrete algebraic Riccati equation (DARE)} +\label{subsec:dare} \index{algebraic Riccati equation!discrete} The discrete algebraic Riccati equation (DARE) appears in the solution to the diff --git a/snippets/LQR.cpp b/snippets/LQR.cpp new file mode 100644 index 0000000..6007915 --- /dev/null +++ b/snippets/LQR.cpp @@ -0,0 +1,24 @@ +#include +#include + +#include "DARE.hpp" + +template +Eigen::Matrix LQR( + const Eigen::Matrix& A, + const Eigen::Matrix& B, + const Eigen::Matrix& Q, + const Eigen::Matrix& R, + const Eigen::Matrix& N) { + using StateMatrix = Eigen::Matrix; + + auto R_llt = R.llt(); + StateMatrix A_2 = A - B * R_llt.solve(N.transpose()); + StateMatrix Q_2 = Q - N * R_llt.solve(N.transpose()); + + StateMatrix S = DARE(A_2, B, Q_2, R); + + return (B.transpose() * S * B + R) + .llt() + .solve(B.transpose() * S * A + N.transpose()); +} diff --git a/snippets/lqr.py b/snippets/lqr.py index 3661b34..9918ad9 100644 --- a/snippets/lqr.py +++ b/snippets/lqr.py @@ -18,4 +18,4 @@ def lqr(A, B, Q, R, N): K -- numpy.array(inputs x states), controller gain matrix. """ P = sp.linalg.solve_discrete_are(a=A, b=B, q=Q, r=R, s=N) - return np.linalg.solve(R + B.T @ P @ B, B.T @ P @ A + N.T) + return np.linalg.solve(B.T @ P @ B + R, B.T @ P @ A + N.T)