diff --git a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
index bb062846af..2ab27d1fbf 100644
--- a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
+++ b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
@@ -6,61 +6,11 @@
 
 namespace frc {
 
-LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
-    const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
-    const wpi::array<double, 1>& Qelems, const wpi::array<double, 1>& Relems,
-    units::second_t dt)
-    : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
-                               MakeCostMatrix(Relems), dt) {}
-
-LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(const Matrixd<1, 1>& A,
-                                                         const Matrixd<1, 1>& B,
-                                                         const Matrixd<1, 1>& Q,
-                                                         const Matrixd<1, 1>& R,
-                                                         units::second_t dt)
-    : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, dt) {}
-
-LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
-    const Matrixd<1, 1>& A, const Matrixd<1, 1>& B, const Matrixd<1, 1>& Q,
-    const Matrixd<1, 1>& R, const Matrixd<1, 1>& N, units::second_t dt)
-    : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, N, dt) {}
-
-LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
-    const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
-    const wpi::array<double, 2>& Qelems, const wpi::array<double, 1>& Relems,
-    units::second_t dt)
-    : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
-                               MakeCostMatrix(Relems), dt) {}
-
-LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(const Matrixd<2, 2>& A,
-                                                         const Matrixd<2, 1>& B,
-                                                         const Matrixd<2, 2>& Q,
-                                                         const Matrixd<1, 1>& R,
-                                                         units::second_t dt)
-    : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, dt) {}
-
-LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
-    const Matrixd<2, 2>& A, const Matrixd<2, 1>& B, const Matrixd<2, 2>& Q,
-    const Matrixd<1, 1>& R, const Matrixd<2, 1>& N, units::second_t dt)
-    : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, N, dt) {}
-
-LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
-    const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
-    const wpi::array<double, 2>& Qelems, const wpi::array<double, 2>& Relems,
-    units::second_t dt)
-    : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
-                               MakeCostMatrix(Relems), dt) {}
-
-LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(const Matrixd<2, 2>& A,
-                                                         const Matrixd<2, 2>& B,
-                                                         const Matrixd<2, 2>& Q,
-                                                         const Matrixd<2, 2>& R,
-                                                         units::second_t dt)
-    : detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, dt) {}
-
-LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
-    const Matrixd<2, 2>& A, const Matrixd<2, 2>& B, const Matrixd<2, 2>& Q,
-    const Matrixd<2, 2>& R, const Matrixd<2, 2>& N, units::second_t dt)
-    : detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, N, dt) {}
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<1, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<2, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<2, 2>;
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
index 4bc0083331..5f4f099a35 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
@@ -4,26 +4,14 @@
 
 #pragma once
 
-#include <frc/fmt/Eigen.h>
-
-#include <string>
-
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/Cholesky"
-#include "Eigen/Eigenvalues"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
 #include "frc/EigenCore.h"
-#include "frc/StateSpaceUtil.h"
-#include "frc/system/Discretization.h"
 #include "frc/system/LinearSystem.h"
 #include "units/time.h"
-#include "unsupported/Eigen/MatrixFunctions"
-#include "wpimath/MathShared.h"
 
 namespace frc {
-namespace detail {
 
 /**
  * Contains the controller coefficients and logic for a linear-quadratic
@@ -37,7 +25,7 @@ namespace detail {
  * @tparam Inputs Number of inputs.
  */
 template <int States, int Inputs>
-class LinearQuadraticRegulatorImpl {
+class LinearQuadraticRegulator {
  public:
   using StateVector = Vectord<States>;
   using InputVector = Vectord<Inputs>;
@@ -54,11 +42,9 @@ class LinearQuadraticRegulatorImpl {
    * @param dt     Discretization timestep.
    */
   template <int Outputs>
-  LinearQuadraticRegulatorImpl(
-      const LinearSystem<States, Inputs, Outputs>& plant,
-      const StateArray& Qelems, const InputArray& Relems, units::second_t dt)
-      : LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, Relems, dt) {
-  }
+  LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
+                           const StateArray& Qelems, const InputArray& Relems,
+                           units::second_t dt);
 
   /**
    * Constructs a controller with the given coefficients and plant.
@@ -69,12 +55,10 @@ class LinearQuadraticRegulatorImpl {
    * @param Relems The maximum desired control effort for each input.
    * @param dt     Discretization timestep.
    */
-  LinearQuadraticRegulatorImpl(const Matrixd<States, States>& A,
-                               const Matrixd<States, Inputs>& B,
-                               const StateArray& Qelems,
-                               const InputArray& Relems, units::second_t dt)
-      : LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems),
-                                     MakeCostMatrix(Relems), dt) {}
+  LinearQuadraticRegulator(const Matrixd<States, States>& A,
+                           const Matrixd<States, Inputs>& B,
+                           const StateArray& Qelems, const InputArray& Relems,
+                           units::second_t dt);
 
   /**
    * Constructs a controller with the given coefficients and plant.
@@ -85,35 +69,11 @@ class LinearQuadraticRegulatorImpl {
    * @param R  The input cost matrix.
    * @param dt Discretization timestep.
    */
-  LinearQuadraticRegulatorImpl(const Matrixd<States, States>& A,
-                               const Matrixd<States, Inputs>& B,
-                               const Matrixd<States, States>& Q,
-                               const Matrixd<Inputs, Inputs>& R,
-                               units::second_t dt) {
-    Matrixd<States, States> discA;
-    Matrixd<States, Inputs> discB;
-    DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
-
-    if (!IsStabilizable<States, Inputs>(discA, discB)) {
-      std::string msg = fmt::format(
-          "The system passed to the LQR is uncontrollable!\n\nA =\n{}\nB "
-          "=\n{}\n",
-          discA, discB);
-
-      wpi::math::MathSharedStore::ReportError(msg);
-      throw std::invalid_argument(msg);
-    }
-
-    Matrixd<States, States> S =
-        drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
-
-    // K = (BᵀSB + R)⁻¹BᵀSA
-    m_K = (discB.transpose() * S * discB + R)
-              .llt()
-              .solve(discB.transpose() * S * discA);
-
-    Reset();
-  }
+  LinearQuadraticRegulator(const Matrixd<States, States>& A,
+                           const Matrixd<States, Inputs>& B,
+                           const Matrixd<States, States>& Q,
+                           const Matrixd<Inputs, Inputs>& R,
+                           units::second_t dt);
 
   /**
    * Constructs a controller with the given coefficients and plant.
@@ -125,30 +85,15 @@ class LinearQuadraticRegulatorImpl {
    * @param N  The state-input cross-term cost matrix.
    * @param dt Discretization timestep.
    */
-  LinearQuadraticRegulatorImpl(const Matrixd<States, States>& A,
-                               const Matrixd<States, Inputs>& B,
-                               const Matrixd<States, States>& Q,
-                               const Matrixd<Inputs, Inputs>& R,
-                               const Matrixd<States, Inputs>& N,
-                               units::second_t dt) {
-    Matrixd<States, States> discA;
-    Matrixd<States, Inputs> discB;
-    DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+  LinearQuadraticRegulator(const Matrixd<States, States>& A,
+                           const Matrixd<States, Inputs>& B,
+                           const Matrixd<States, States>& Q,
+                           const Matrixd<Inputs, Inputs>& R,
+                           const Matrixd<States, Inputs>& N,
+                           units::second_t dt);
 
-    Matrixd<States, States> S =
-        drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
-
-    // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
-    m_K = (discB.transpose() * S * discB + R)
-              .llt()
-              .solve(discB.transpose() * S * discA + N.transpose());
-
-    Reset();
-  }
-
-  LinearQuadraticRegulatorImpl(LinearQuadraticRegulatorImpl&&) = default;
-  LinearQuadraticRegulatorImpl& operator=(LinearQuadraticRegulatorImpl&&) =
-      default;
+  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
+  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
 
   /**
    * Returns the controller matrix K.
@@ -208,10 +153,7 @@ class LinearQuadraticRegulatorImpl {
    *
    * @param x The current state x.
    */
-  InputVector Calculate(const StateVector& x) {
-    m_u = m_K * (m_r - x);
-    return m_u;
-  }
+  InputVector Calculate(const StateVector& x);
 
   /**
    * Returns the next output of the controller.
@@ -219,10 +161,7 @@ class LinearQuadraticRegulatorImpl {
    * @param x     The current state x.
    * @param nextR The next reference vector r.
    */
-  InputVector Calculate(const StateVector& x, const StateVector& nextR) {
-    m_r = nextR;
-    return Calculate(x);
-  }
+  InputVector Calculate(const StateVector& x, const StateVector& nextR);
 
   /**
    * Adjusts LQR controller gain to compensate for a pure time delay in the
@@ -242,13 +181,7 @@ class LinearQuadraticRegulatorImpl {
    */
   template <int Outputs>
   void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
-                         units::second_t dt, units::second_t inputDelay) {
-    Matrixd<States, States> discA;
-    Matrixd<States, Inputs> discB;
-    DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
-
-    m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
-  }
+                         units::second_t dt, units::second_t inputDelay);
 
  private:
   // Current reference
@@ -261,170 +194,13 @@ class LinearQuadraticRegulatorImpl {
   Matrixd<Inputs, States> m_K;
 };
 
-}  // namespace detail
-
-template <int States, int Inputs>
-class LinearQuadraticRegulator
-    : public detail::LinearQuadraticRegulatorImpl<States, Inputs> {
- public:
-  /**
-   * Constructs a controller with the given coefficients and plant.
-   *
-   * @tparam Outputs The number of outputs.
-   * @param plant  The plant being controlled.
-   * @param Qelems The maximum desired error tolerance for each state.
-   * @param Relems The maximum desired control effort for each input.
-   * @param dt     Discretization timestep.
-   */
-  template <int Outputs>
-  LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
-                           const wpi::array<double, States>& Qelems,
-                           const wpi::array<double, Inputs>& Relems,
-                           units::second_t dt)
-      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
-
-  /**
-   * Constructs a controller with the given coefficients and plant.
-   *
-   * @param A      Continuous system matrix of the plant being controlled.
-   * @param B      Continuous input matrix of the plant being controlled.
-   * @param Qelems The maximum desired error tolerance for each state.
-   * @param Relems The maximum desired control effort for each input.
-   * @param dt     Discretization timestep.
-   */
-  LinearQuadraticRegulator(const Matrixd<States, States>& A,
-                           const Matrixd<States, Inputs>& B,
-                           const wpi::array<double, States>& Qelems,
-                           const wpi::array<double, Inputs>& Relems,
-                           units::second_t dt)
-      : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
-                                 MakeCostMatrix(Relems), dt) {}
-
-  /**
-   * Constructs a controller with the given coefficients and plant.
-   *
-   * @param A  Continuous system matrix of the plant being controlled.
-   * @param B  Continuous input matrix of the plant being controlled.
-   * @param Q  The state cost matrix.
-   * @param R  The input cost matrix.
-   * @param dt Discretization timestep.
-   */
-  LinearQuadraticRegulator(const Matrixd<States, States>& A,
-                           const Matrixd<States, Inputs>& B,
-                           const Matrixd<States, States>& Q,
-                           const Matrixd<Inputs, Inputs>& R, units::second_t dt)
-      : detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q, R, dt} {}
-
-  /**
-   * Constructs a controller with the given coefficients and plant.
-   *
-   * @param A  Continuous system matrix of the plant being controlled.
-   * @param B  Continuous input matrix of the plant being controlled.
-   * @param Q  The state cost matrix.
-   * @param R  The input cost matrix.
-   * @param N  The state-input cross-term cost matrix.
-   * @param dt Discretization timestep.
-   */
-  LinearQuadraticRegulator(const Matrixd<States, States>& A,
-                           const Matrixd<States, Inputs>& B,
-                           const Matrixd<States, States>& Q,
-                           const Matrixd<Inputs, Inputs>& R,
-                           const Matrixd<States, Inputs>& N, units::second_t dt)
-      : detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q,
-                                                             R, N, dt} {}
-
-  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
-  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
-};
-
-// Template specializations are used here to make common state-input pairs
-// compile faster.
-template <>
-class WPILIB_DLLEXPORT LinearQuadraticRegulator<1, 1>
-    : public detail::LinearQuadraticRegulatorImpl<1, 1> {
- public:
-  template <int Outputs>
-  LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant,
-                           const wpi::array<double, 1>& Qelems,
-                           const wpi::array<double, 1>& Relems,
-                           units::second_t dt)
-      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
-
-  LinearQuadraticRegulator(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
-                           const wpi::array<double, 1>& Qelems,
-                           const wpi::array<double, 1>& Relems,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
-                           const Matrixd<1, 1>& Q, const Matrixd<1, 1>& R,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
-                           const Matrixd<1, 1>& Q, const Matrixd<1, 1>& R,
-                           const Matrixd<1, 1>& N, units::second_t dt);
-
-  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
-  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
-};
-
-// Template specializations are used here to make common state-input pairs
-// compile faster.
-template <>
-class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 1>
-    : public detail::LinearQuadraticRegulatorImpl<2, 1> {
- public:
-  template <int Outputs>
-  LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant,
-                           const wpi::array<double, 2>& Qelems,
-                           const wpi::array<double, 1>& Relems,
-                           units::second_t dt)
-      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
-
-  LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
-                           const wpi::array<double, 2>& Qelems,
-                           const wpi::array<double, 1>& Relems,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
-                           const Matrixd<2, 2>& Q, const Matrixd<1, 1>& R,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
-                           const Matrixd<2, 2>& Q, const Matrixd<1, 1>& R,
-                           const Matrixd<2, 1>& N, units::second_t dt);
-
-  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
-  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
-};
-
-// Template specializations are used here to make common state-input pairs
-// compile faster.
-template <>
-class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 2>
-    : public detail::LinearQuadraticRegulatorImpl<2, 2> {
- public:
-  template <int Outputs>
-  LinearQuadraticRegulator(const LinearSystem<2, 2, Outputs>& plant,
-                           const wpi::array<double, 2>& Qelems,
-                           const wpi::array<double, 2>& Relems,
-                           units::second_t dt)
-      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
-
-  LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
-                           const wpi::array<double, 2>& Qelems,
-                           const wpi::array<double, 2>& Relems,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
-                           const Matrixd<2, 2>& Q, const Matrixd<2, 2>& R,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
-                           const Matrixd<2, 2>& Q, const Matrixd<2, 2>& R,
-                           const Matrixd<2, 2>& N, units::second_t dt);
-
-  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
-  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
-};
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<1, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<2, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<2, 2>;
 
 }  // namespace frc
+
+#include "LinearQuadraticRegulator.inc"
diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
new file mode 100644
index 0000000000..9ba743e4b2
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
@@ -0,0 +1,113 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/fmt/Eigen.h>
+
+#include <string>
+
+#include "Eigen/Cholesky"
+#include "Eigen/Eigenvalues"
+#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/Discretization.h"
+#include "unsupported/Eigen/MatrixFunctions"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+template <int States, int Inputs>
+template <int Outputs>
+LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
+    const LinearSystem<States, Inputs, Outputs>& plant,
+    const StateArray& Qelems, const InputArray& Relems, units::second_t dt)
+    : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
+
+template <int States, int Inputs>
+LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
+    const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
+    const StateArray& Qelems, const InputArray& Relems, units::second_t dt)
+    : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
+                               MakeCostMatrix(Relems), dt) {}
+
+template <int States, int Inputs>
+LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
+    const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
+    const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
+    units::second_t dt) {
+  Matrixd<States, States> discA;
+  Matrixd<States, Inputs> discB;
+  DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+  if (!IsStabilizable<States, Inputs>(discA, discB)) {
+    std::string msg = fmt::format(
+        "The system passed to the LQR is uncontrollable!\n\nA =\n{}\nB "
+        "=\n{}\n",
+        discA, discB);
+
+    wpi::math::MathSharedStore::ReportError(msg);
+    throw std::invalid_argument(msg);
+  }
+
+  Matrixd<States, States> S =
+      drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
+
+  // K = (BᵀSB + R)⁻¹BᵀSA
+  m_K = (discB.transpose() * S * discB + R)
+            .llt()
+            .solve(discB.transpose() * S * discA);
+
+  Reset();
+}
+
+template <int States, int Inputs>
+LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
+    const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
+    const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
+    const Matrixd<States, Inputs>& N, units::second_t dt) {
+  Matrixd<States, States> discA;
+  Matrixd<States, Inputs> discB;
+  DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+  Matrixd<States, States> S =
+      drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
+
+  // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
+  m_K = (discB.transpose() * S * discB + R)
+            .llt()
+            .solve(discB.transpose() * S * discA + N.transpose());
+
+  Reset();
+}
+
+template <int States, int Inputs>
+typename LinearQuadraticRegulator<States, Inputs>::InputVector
+LinearQuadraticRegulator<States, Inputs>::Calculate(const StateVector& x) {
+  m_u = m_K * (m_r - x);
+  return m_u;
+}
+
+template <int States, int Inputs>
+typename LinearQuadraticRegulator<States, Inputs>::InputVector
+LinearQuadraticRegulator<States, Inputs>::Calculate(const StateVector& x,
+                                                    const StateVector& nextR) {
+  m_r = nextR;
+  return Calculate(x);
+}
+
+template <int States, int Inputs>
+template <int Outputs>
+void LinearQuadraticRegulator<States, Inputs>::LatencyCompensate(
+    const LinearSystem<States, Inputs, Outputs>& plant, units::second_t dt,
+    units::second_t inputDelay) {
+  Matrixd<States, States> discA;
+  Matrixd<States, Inputs> discB;
+  DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
+
+  m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
+}
+
+}  // namespace frc