Add C++ LQR solver

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

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

snippets/LQR.cpp

@@ -0,0 +1,24 @@
+#include <Eigen/Cholesky>
+#include <Eigen/Core>
+
+#include "DARE.hpp"
+
+template <int States, int Inputs>
+Eigen::Matrix<double, Inputs, States> LQR(
+    const Eigen::Matrix<double, States, States>& A,
+    const Eigen::Matrix<double, States, Inputs>& B,
+    const Eigen::Matrix<double, States, States>& Q,
+    const Eigen::Matrix<double, Inputs, Inputs>& R,
+    const Eigen::Matrix<double, States, Inputs>& N) {
+  using StateMatrix = Eigen::Matrix<double, States, States>;
+
+  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<States, Inputs>(A_2, B, Q_2, R);
+
+  return (B.transpose() * S * B + R)
+      .llt()
+      .solve(B.transpose() * S * A + N.transpose());
+}

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)