diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py
index ac25370ff8d..d2434a36880 100755
--- a/upstream_utils/sleipnir.py
+++ b/upstream_utils/sleipnir.py
@@ -48,8 +48,8 @@ def copy_upstream_src(wpilib_root):
 def main():
     name = "sleipnir"
     url = "https://github.com/SleipnirGroup/Sleipnir"
-    # main on 2024-12-07
-    tag = "01206ab17d741f4c45a7faeb56b8a5442df1681c"
+    # main on 2025-02-08
+    tag = "c1d78b8b42c60ac37107ca291179a89470b2a5ec"
 
     sleipnir = Lib(name, url, tag, copy_upstream_src)
     sleipnir.main()
diff --git a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch
index deaaf7b5cb7..31e47736271 100644
--- a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch
+++ b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Wed, 29 May 2024 16:29:55 -0700
-Subject: [PATCH 1/3] Use fmtlib
+Subject: [PATCH 1/6] Use fmtlib
 
 ---
  include/.styleguide             |  1 +
diff --git a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch
index 6b816784a7a..db1b360070c 100644
--- a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch
+++ b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch
@@ -1,26 +1,29 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Sun, 16 Jun 2024 12:08:49 -0700
-Subject: [PATCH 2/3] Use wpi::SmallVector
+Subject: [PATCH 2/6] Use wpi::SmallVector
 
 ---
- include/.styleguide                            |  1 +
- include/sleipnir/autodiff/Expression.hpp       | 13 +++++++------
- include/sleipnir/autodiff/ExpressionGraph.hpp  | 15 ++++++++-------
- include/sleipnir/autodiff/Hessian.hpp          |  4 ++--
- include/sleipnir/autodiff/Jacobian.hpp         | 10 +++++-----
- include/sleipnir/autodiff/Variable.hpp         | 10 +++++-----
- include/sleipnir/autodiff/VariableMatrix.hpp   |  4 ++--
- include/sleipnir/optimization/Multistart.hpp   |  7 ++++---
- .../optimization/OptimizationProblem.hpp       |  8 ++++----
- include/sleipnir/util/Pool.hpp                 |  7 ++++---
- include/sleipnir/util/Spy.hpp                  |  4 ++--
- src/.styleguide                                |  1 +
- src/optimization/solver/InteriorPoint.cpp      |  4 ++--
- src/optimization/solver/SQP.cpp                |  4 ++--
- .../solver/util/FeasibilityRestoration.hpp     | 18 +++++++++---------
- src/optimization/solver/util/Filter.hpp        |  4 ++--
- 16 files changed, 60 insertions(+), 54 deletions(-)
+ include/.styleguide                             |  1 +
+ .../autodiff/AdjointExpressionGraph.hpp         |  8 ++++----
+ include/sleipnir/autodiff/Expression.hpp        | 13 +++++++------
+ include/sleipnir/autodiff/ExpressionGraph.hpp   | 14 ++++++++------
+ include/sleipnir/autodiff/Gradient.hpp          |  4 ++--
+ include/sleipnir/autodiff/Hessian.hpp           |  4 ++--
+ include/sleipnir/autodiff/Jacobian.hpp          | 12 ++++++------
+ include/sleipnir/autodiff/Variable.hpp          | 12 ++++++------
+ include/sleipnir/autodiff/VariableMatrix.hpp    | 17 +++++++++--------
+ include/sleipnir/optimization/Multistart.hpp    |  7 ++++---
+ .../optimization/OptimizationProblem.hpp        | 10 +++++-----
+ include/sleipnir/util/Pool.hpp                  |  7 ++++---
+ include/sleipnir/util/Spy.hpp                   |  1 +
+ src/.styleguide                                 |  1 +
+ src/optimization/solver/InteriorPoint.cpp       |  8 ++++----
+ src/optimization/solver/Newton.cpp              |  4 ++--
+ src/optimization/solver/SQP.cpp                 |  8 ++++----
+ src/optimization/solver/util/Filter.hpp         |  4 ++--
+ src/util/PrintDiagnostics.hpp                   |  9 +++++----
+ 19 files changed, 77 insertions(+), 67 deletions(-)
 
 diff --git a/include/.styleguide b/include/.styleguide
 index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f1854fbbc523 100644
@@ -32,11 +35,50 @@ index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f185
    ^fmt/
 +  ^wpi/
  }
+diff --git a/include/sleipnir/autodiff/AdjointExpressionGraph.hpp b/include/sleipnir/autodiff/AdjointExpressionGraph.hpp
+index ed8ddf7e24ed8e962bfdd01e575b91caeb1a4c30..4e67df3dfb19b28e0da522cdb9b76f69138d3143 100644
+--- a/include/sleipnir/autodiff/AdjointExpressionGraph.hpp
++++ b/include/sleipnir/autodiff/AdjointExpressionGraph.hpp
+@@ -6,11 +6,11 @@
+ #include <utility>
+ 
+ #include <Eigen/SparseCore>
++#include <wpi/SmallVector.h>
+ 
+ #include "sleipnir/autodiff/ExpressionGraph.hpp"
+ #include "sleipnir/autodiff/Variable.hpp"
+ #include "sleipnir/autodiff/VariableMatrix.hpp"
+-#include "sleipnir/util/small_vector.hpp"
+ 
+ namespace sleipnir::detail {
+ 
+@@ -98,7 +98,7 @@ class AdjointExpressionGraph {
+    * @param triplets The sparse matrix triplets.
+    * @param row The row of wrt.
+    */
+-  void AppendAdjointTriplets(small_vector<Eigen::Triplet<double>>& triplets,
++  void AppendAdjointTriplets(wpi::SmallVector<Eigen::Triplet<double>>& triplets,
+                              int row) const {
+     detail::UpdateAdjoints(m_topList);
+ 
+@@ -112,10 +112,10 @@ class AdjointExpressionGraph {
+ 
+  private:
+   // Topological sort of graph from parent to child
+-  small_vector<Expression*> m_topList;
++  wpi::SmallVector<Expression*> m_topList;
+ 
+   // List that maps nodes to their respective column
+-  small_vector<int> m_colList;
++  wpi::SmallVector<int> m_colList;
+ };
+ 
+ }  // namespace sleipnir::detail
 diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp
-index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..ef9a15bf69d8cae6b2196513b72ec4b359cc8752 100644
+index 20642133e197a67b622a6660dae0dd51e0420812..8a2d1ca81d142f91449f9bb5068f644b63cc41c6 100644
 --- a/include/sleipnir/autodiff/Expression.hpp
 +++ b/include/sleipnir/autodiff/Expression.hpp
-@@ -11,11 +11,12 @@
+@@ -11,10 +11,11 @@
  #include <numbers>
  #include <utility>
  
@@ -45,14 +87,13 @@ index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..ef9a15bf69d8cae6b2196513b72ec4b3
  #include "sleipnir/autodiff/ExpressionType.hpp"
  #include "sleipnir/util/IntrusiveSharedPtr.hpp"
  #include "sleipnir/util/Pool.hpp"
- #include "sleipnir/util/SymbolExports.hpp"
 -#include "sleipnir/util/small_vector.hpp"
  
  namespace sleipnir::detail {
  
-@@ -29,8 +30,8 @@ inline constexpr bool kUsePoolAllocator = true;
+@@ -28,8 +29,8 @@ inline constexpr bool kUsePoolAllocator = true;
  
- struct SLEIPNIR_DLLEXPORT Expression;
+ struct Expression;
  
 -inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr);
 -inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr);
@@ -61,7 +102,7 @@ index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..ef9a15bf69d8cae6b2196513b72ec4b3
  
  /**
   * Typedef for intrusive shared pointer to Expression.
-@@ -418,7 +419,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x);
+@@ -631,7 +632,7 @@ inline ExpressionPtr sqrt(const ExpressionPtr& x);
   *
   * @param expr The shared pointer's managed object.
   */
@@ -70,7 +111,7 @@ index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..ef9a15bf69d8cae6b2196513b72ec4b3
    ++expr->refCount;
  }
  
-@@ -427,12 +428,12 @@ inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) {
+@@ -640,12 +641,12 @@ inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) {
   *
   * @param expr The shared pointer's managed object.
   */
@@ -86,129 +127,151 @@ index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..ef9a15bf69d8cae6b2196513b72ec4b3
  
    while (!stack.empty()) {
 diff --git a/include/sleipnir/autodiff/ExpressionGraph.hpp b/include/sleipnir/autodiff/ExpressionGraph.hpp
-index c614195d82ad022dfd0c3f6f8240b042c0056c2f..714bcbb82907e754138347334c7fca8a7ccf055d 100644
+index 4672466da48c2831b602ee9ce020a1c43a870dfd..81aacd5e1652b476dc0f1960437069f44042fed0 100644
 --- a/include/sleipnir/autodiff/ExpressionGraph.hpp
 +++ b/include/sleipnir/autodiff/ExpressionGraph.hpp
-@@ -4,10 +4,11 @@
+@@ -4,8 +4,9 @@
  
- #include <span>
+ #include <ranges>
  
 +#include <wpi/SmallVector.h>
 +
  #include "sleipnir/autodiff/Expression.hpp"
- #include "sleipnir/util/FunctionRef.hpp"
- #include "sleipnir/util/SymbolExports.hpp"
 -#include "sleipnir/util/small_vector.hpp"
  
  namespace sleipnir::detail {
  
-@@ -36,7 +37,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
-     // https://en.wikipedia.org/wiki/Breadth-first_search
+@@ -16,8 +17,9 @@ namespace sleipnir::detail {
+  *
+  * @param root The root node of the expression.
+  */
+-inline small_vector<Expression*> TopologicalSort(const ExpressionPtr& root) {
+-  small_vector<Expression*> list;
++inline wpi::SmallVector<Expression*> TopologicalSort(
++    const ExpressionPtr& root) {
++  wpi::SmallVector<Expression*> list;
+ 
+   // If the root type is a constant, Update() is a no-op, so there's no work
+   // to do
+@@ -26,7 +28,7 @@ inline small_vector<Expression*> TopologicalSort(const ExpressionPtr& root) {
+   }
  
-     // BFS list sorted from parent to child.
--    small_vector<Expression*> stack;
-+    wpi::SmallVector<Expression*> stack;
+   // Stack of nodes to explore
+-  small_vector<Expression*> stack;
++  wpi::SmallVector<Expression*> stack;
  
-     stack.emplace_back(root.Get());
+   // Enumerate incoming edges for each node via depth-first search
+   stack.emplace_back(root.Get());
+@@ -73,7 +75,7 @@ inline small_vector<Expression*> TopologicalSort(const ExpressionPtr& root) {
+  *
+  * @param list Topological sort of graph from parent to child.
+  */
+-inline void UpdateValues(const small_vector<Expression*>& list) {
++inline void UpdateValues(const wpi::SmallVector<Expression*>& list) {
+   // Traverse graph from child to parent and update values
+   for (auto& node : list | std::views::reverse) {
+     auto& lhs = node->args[0];
+@@ -94,7 +96,7 @@ inline void UpdateValues(const small_vector<Expression*>& list) {
+  *
+  * @param list Topological sort of graph from parent to child.
+  */
+-inline void UpdateAdjoints(const small_vector<Expression*>& list) {
++inline void UpdateAdjoints(const wpi::SmallVector<Expression*>& list) {
+   // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
+   // for background on reverse accumulation automatic differentiation.
+ 
+diff --git a/include/sleipnir/autodiff/Gradient.hpp b/include/sleipnir/autodiff/Gradient.hpp
+index 28134cb2d10570aab4fd48b11b3eb64976e7581b..3f9bfeac8c7142503e0595c8ab2a72dde8f19c79 100644
+--- a/include/sleipnir/autodiff/Gradient.hpp
++++ b/include/sleipnir/autodiff/Gradient.hpp
+@@ -5,13 +5,13 @@
+ #include <utility>
  
-@@ -119,7 +120,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
-    *
-    * @param wrt Variables with respect to which to compute the gradient.
-    */
--  small_vector<ExpressionPtr> GenerateGradientTree(
-+  wpi::SmallVector<ExpressionPtr> GenerateGradientTree(
-       std::span<const ExpressionPtr> wrt) const {
-     // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
-     // for background on reverse accumulation automatic differentiation.
-@@ -128,7 +129,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
-       wrt[row]->row = row;
-     }
- 
--    small_vector<ExpressionPtr> grad;
-+    wpi::SmallVector<ExpressionPtr> grad;
-     grad.reserve(wrt.size());
-     for (size_t row = 0; row < wrt.size(); ++row) {
-       grad.emplace_back(MakeExpressionPtr());
-@@ -231,13 +232,13 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
+ #include <Eigen/SparseCore>
++#include <wpi/SmallVector.h>
  
-  private:
-   // List that maps nodes to their respective row.
--  small_vector<int> m_rowList;
-+  wpi::SmallVector<int> m_rowList;
+ #include "sleipnir/autodiff/Jacobian.hpp"
+ #include "sleipnir/autodiff/Variable.hpp"
+ #include "sleipnir/autodiff/VariableMatrix.hpp"
+ #include "sleipnir/util/SolveProfiler.hpp"
+ #include "sleipnir/util/SymbolExports.hpp"
+-#include "sleipnir/util/small_vector.hpp"
  
-   // List for updating adjoints
--  small_vector<Expression*> m_adjointList;
-+  wpi::SmallVector<Expression*> m_adjointList;
+ namespace sleipnir {
  
-   // List for updating values
--  small_vector<Expression*> m_valueList;
-+  wpi::SmallVector<Expression*> m_valueList;
- };
+@@ -63,7 +63,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
+   /**
+    * Returns the profiler.
+    */
+-  const small_vector<SolveProfiler>& GetProfilers() const {
++  const wpi::SmallVector<SolveProfiler>& GetProfilers() const {
+     return m_jacobian.GetProfilers();
+   }
  
- }  // namespace sleipnir::detail
 diff --git a/include/sleipnir/autodiff/Hessian.hpp b/include/sleipnir/autodiff/Hessian.hpp
-index 4563aa234bd7b0ec22e12d2fc0b6f4569bee7f39..2e60d89e95280bdac422b0d7dab955ba111b0059 100644
+index 21f219a9b296c6d9056c309d611f589a77c3c990..30bb88bd117ac3dbb44ec5478b40285d1130065a 100644
 --- a/include/sleipnir/autodiff/Hessian.hpp
 +++ b/include/sleipnir/autodiff/Hessian.hpp
-@@ -6,6 +6,7 @@
+@@ -3,6 +3,7 @@
+ #pragma once
  
- #include <Eigen/Core>
  #include <Eigen/SparseCore>
 +#include <wpi/SmallVector.h>
  
- #include "sleipnir/autodiff/ExpressionGraph.hpp"
+ #include "sleipnir/autodiff/AdjointExpressionGraph.hpp"
  #include "sleipnir/autodiff/Jacobian.hpp"
-@@ -13,7 +14,6 @@
- #include "sleipnir/autodiff/Variable.hpp"
+@@ -10,7 +11,6 @@
  #include "sleipnir/autodiff/VariableMatrix.hpp"
+ #include "sleipnir/util/SolveProfiler.hpp"
  #include "sleipnir/util/SymbolExports.hpp"
 -#include "sleipnir/util/small_vector.hpp"
  
  namespace sleipnir {
  
-@@ -36,7 +36,7 @@ class SLEIPNIR_DLLEXPORT Hessian {
-   Hessian(Variable variable, const VariableMatrix& wrt) noexcept
-       : m_jacobian{
-             [&] {
--              small_vector<detail::ExpressionPtr> wrtVec;
-+              wpi::SmallVector<detail::ExpressionPtr> wrtVec;
-               wrtVec.reserve(wrt.size());
-               for (auto& elem : wrt) {
-                 wrtVec.emplace_back(elem.expr);
+@@ -51,7 +51,7 @@ class SLEIPNIR_DLLEXPORT Hessian {
+   /**
+    * Returns the profilers.
+    */
+-  const small_vector<SolveProfiler>& GetProfilers() const {
++  const wpi::SmallVector<SolveProfiler>& GetProfilers() const {
+     return m_jacobian.GetProfilers();
+   }
+ 
 diff --git a/include/sleipnir/autodiff/Jacobian.hpp b/include/sleipnir/autodiff/Jacobian.hpp
-index ac00c11ef8c947cbe95c58081bdbfb42bf901051..0c660156c80f94539383b8f0d01d7853e41e0297 100644
+index 052e4cb431970b104813679933899b886ba46802..70af2691a3dd26b49c9d606374fb1edfafa4459d 100644
 --- a/include/sleipnir/autodiff/Jacobian.hpp
 +++ b/include/sleipnir/autodiff/Jacobian.hpp
-@@ -5,13 +5,13 @@
+@@ -5,6 +5,7 @@
  #include <utility>
  
  #include <Eigen/SparseCore>
 +#include <wpi/SmallVector.h>
  
- #include "sleipnir/autodiff/ExpressionGraph.hpp"
- #include "sleipnir/autodiff/Profiler.hpp"
+ #include "sleipnir/autodiff/AdjointExpressionGraph.hpp"
  #include "sleipnir/autodiff/Variable.hpp"
- #include "sleipnir/autodiff/VariableMatrix.hpp"
+@@ -12,7 +13,6 @@
+ #include "sleipnir/util/ScopedProfiler.hpp"
+ #include "sleipnir/util/SolveProfiler.hpp"
  #include "sleipnir/util/SymbolExports.hpp"
 -#include "sleipnir/util/small_vector.hpp"
  
  namespace sleipnir {
  
-@@ -81,7 +81,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
-   VariableMatrix Get() const {
-     VariableMatrix result{m_variables.Rows(), m_wrt.Rows()};
+@@ -144,7 +144,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
+   /**
+    * Returns the profilers.
+    */
+-  const small_vector<SolveProfiler>& GetProfilers() const {
++  const wpi::SmallVector<SolveProfiler>& GetProfilers() const {
+     return m_profilers;
+   }
  
--    small_vector<detail::ExpressionPtr> wrtVec;
-+    wpi::SmallVector<detail::ExpressionPtr> wrtVec;
-     wrtVec.reserve(m_wrt.size());
-     for (auto& elem : m_wrt) {
-       wrtVec.emplace_back(elem.expr);
-@@ -138,16 +138,16 @@ class SLEIPNIR_DLLEXPORT Jacobian {
+@@ -152,18 +152,18 @@ class SLEIPNIR_DLLEXPORT Jacobian {
    VariableMatrix m_variables;
    VariableMatrix m_wrt;
  
--  small_vector<detail::ExpressionGraph> m_graphs;
-+  wpi::SmallVector<detail::ExpressionGraph> m_graphs;
+-  small_vector<detail::AdjointExpressionGraph> m_graphs;
++  wpi::SmallVector<detail::AdjointExpressionGraph> m_graphs;
  
    Eigen::SparseMatrix<double> m_J{m_variables.Rows(), m_wrt.Rows()};
  
@@ -221,13 +284,16 @@ index ac00c11ef8c947cbe95c58081bdbfb42bf901051..0c660156c80f94539383b8f0d01d7853
 -  small_vector<int> m_nonlinearRows;
 +  wpi::SmallVector<int> m_nonlinearRows;
  
-   Profiler m_profiler;
+-  small_vector<SolveProfiler> m_profilers;
++  wpi::SmallVector<SolveProfiler> m_profilers;
  };
+ 
+ }  // namespace sleipnir
 diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp
-index c04d629f482efe59497570ca1fd9b09a4af2ae1e..d192fb96e7984b7c0ca30262668c41e5e84ca34e 100644
+index bd0742164f73b88f3dcb106f9042bf39b82c82a5..c9dd5a824a8c22bc0bd645d77fea0ee149f54c90 100644
 --- a/include/sleipnir/autodiff/Variable.hpp
 +++ b/include/sleipnir/autodiff/Variable.hpp
-@@ -10,6 +10,7 @@
+@@ -12,13 +12,13 @@
  #include <vector>
  
  #include <Eigen/Core>
@@ -235,15 +301,23 @@ index c04d629f482efe59497570ca1fd9b09a4af2ae1e..d192fb96e7984b7c0ca30262668c41e5
  
  #include "sleipnir/autodiff/Expression.hpp"
  #include "sleipnir/autodiff/ExpressionGraph.hpp"
-@@ -17,7 +18,6 @@
+ #include "sleipnir/util/Assert.hpp"
  #include "sleipnir/util/Concepts.hpp"
- #include "sleipnir/util/Print.hpp"
  #include "sleipnir/util/SymbolExports.hpp"
 -#include "sleipnir/util/small_vector.hpp"
  
- namespace sleipnir {
+ #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+ #include "sleipnir/util/Print.hpp"
+@@ -241,7 +241,7 @@ class SLEIPNIR_DLLEXPORT Variable {
  
-@@ -445,8 +445,8 @@ template <typename LHS, typename RHS>
+   /// Updates the value of this variable based on the values of its dependent
+   /// variables
+-  std::optional<small_vector<detail::Expression*>> m_graph;
++  std::optional<wpi::SmallVector<detail::Expression*>> m_graph;
+ 
+   friend SLEIPNIR_DLLEXPORT Variable abs(const Variable& x);
+   friend SLEIPNIR_DLLEXPORT Variable acos(const Variable& x);
+@@ -477,8 +477,8 @@ template <typename LHS, typename RHS>
            (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
            (!std::same_as<std::decay_t<LHS>, double> ||
             !std::same_as<std::decay_t<RHS>, double>)
@@ -254,7 +328,7 @@ index c04d629f482efe59497570ca1fd9b09a4af2ae1e..d192fb96e7984b7c0ca30262668c41e5
  
    if constexpr (ScalarLike<std::decay_t<LHS>> &&
                  ScalarLike<std::decay_t<RHS>>) {
-@@ -534,7 +534,7 @@ small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
+@@ -566,7 +566,7 @@ small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
   */
  struct SLEIPNIR_DLLEXPORT EqualityConstraints {
    /// A vector of scalar equality constraints.
@@ -263,7 +337,7 @@ index c04d629f482efe59497570ca1fd9b09a4af2ae1e..d192fb96e7984b7c0ca30262668c41e5
  
    /**
     * Concatenates multiple equality constraints.
-@@ -596,7 +596,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
+@@ -628,7 +628,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
   */
  struct SLEIPNIR_DLLEXPORT InequalityConstraints {
    /// A vector of scalar inequality constraints.
@@ -273,7 +347,7 @@ index c04d629f482efe59497570ca1fd9b09a4af2ae1e..d192fb96e7984b7c0ca30262668c41e5
    /**
     * Concatenates multiple inequality constraints.
 diff --git a/include/sleipnir/autodiff/VariableMatrix.hpp b/include/sleipnir/autodiff/VariableMatrix.hpp
-index 47452b8988b3a1a96a78d28644200b1c4cdc89c9..57b09913d15e9590873ad7bf62e2baff9fbc5df9 100644
+index 73bc588a24151905ee58b309158afc5f88e706c1..091bd720d815f74d1d2b0858c1b63acda69d4095 100644
 --- a/include/sleipnir/autodiff/VariableMatrix.hpp
 +++ b/include/sleipnir/autodiff/VariableMatrix.hpp
 @@ -11,6 +11,7 @@
@@ -292,7 +366,59 @@ index 47452b8988b3a1a96a78d28644200b1c4cdc89c9..57b09913d15e9590873ad7bf62e2baff
  
  namespace sleipnir {
  
-@@ -946,7 +946,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+@@ -842,7 +842,8 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     constexpr iterator() noexcept = default;
+ 
+-    explicit constexpr iterator(small_vector<Variable>::iterator it) noexcept
++    explicit constexpr iterator(
++        wpi::SmallVector<Variable>::iterator it) noexcept
+         : m_it{it} {}
+ 
+     constexpr iterator& operator++() noexcept {
+@@ -861,7 +862,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+     constexpr reference operator*() const noexcept { return *m_it; }
+ 
+    private:
+-    small_vector<Variable>::iterator m_it;
++    wpi::SmallVector<Variable>::iterator m_it;
+   };
+ 
+   class const_iterator {
+@@ -875,7 +876,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+     constexpr const_iterator() noexcept = default;
+ 
+     explicit constexpr const_iterator(
+-        small_vector<Variable>::const_iterator it) noexcept
++        wpi::SmallVector<Variable>::const_iterator it) noexcept
+         : m_it{it} {}
+ 
+     constexpr const_iterator& operator++() noexcept {
+@@ -894,7 +895,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+     constexpr const_reference operator*() const noexcept { return *m_it; }
+ 
+    private:
+-    small_vector<Variable>::const_iterator m_it;
++    wpi::SmallVector<Variable>::const_iterator m_it;
+   };
+ 
+   /**
+@@ -920,12 +921,12 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+   /**
+    * Returns begin iterator.
+    */
+-  const_iterator cbegin() const { return const_iterator{m_storage.cbegin()}; }
++  const_iterator cbegin() const { return const_iterator{m_storage.begin()}; }
+ 
+   /**
+    * Returns end iterator.
+    */
+-  const_iterator cend() const { return const_iterator{m_storage.cend()}; }
++  const_iterator cend() const { return const_iterator{m_storage.end()}; }
+ 
+   /**
+    * Returns number of elements in matrix.
+@@ -965,7 +966,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    }
  
   private:
@@ -302,7 +428,7 @@ index 47452b8988b3a1a96a78d28644200b1c4cdc89c9..57b09913d15e9590873ad7bf62e2baff
    int m_cols = 0;
  };
 diff --git a/include/sleipnir/optimization/Multistart.hpp b/include/sleipnir/optimization/Multistart.hpp
-index 8055713a2492a9c8473f047a2fb9fe7ca57753c3..09b1b2f3bf33c35ae0aeddb9b5d47c0d80c68cec 100644
+index f803edb17467e5f29e20392766ac2cae682ab959..ca6945d269182a5f6b20c639a727a4f79d21092f 100644
 --- a/include/sleipnir/optimization/Multistart.hpp
 +++ b/include/sleipnir/optimization/Multistart.hpp
 @@ -6,9 +6,10 @@
@@ -335,10 +461,10 @@ index 8055713a2492a9c8473f047a2fb9fe7ca57753c3..09b1b2f3bf33c35ae0aeddb9b5d47c0d
  
    for (auto& future : futures) {
 diff --git a/include/sleipnir/optimization/OptimizationProblem.hpp b/include/sleipnir/optimization/OptimizationProblem.hpp
-index 569dcdee507881ceb412585ca811927072552c15..66883fed98ad087010fb153bd91effce6e047928 100644
+index 240cb755e924ca10f1b84bd57f4711c1178cdc8e..7bcff73587fce3b98c4ef407c354a16e962c5827 100644
 --- a/include/sleipnir/optimization/OptimizationProblem.hpp
 +++ b/include/sleipnir/optimization/OptimizationProblem.hpp
-@@ -11,6 +11,7 @@
+@@ -10,6 +10,7 @@
  #include <utility>
  
  #include <Eigen/Core>
@@ -346,15 +472,15 @@ index 569dcdee507881ceb412585ca811927072552c15..66883fed98ad087010fb153bd91effce
  
  #include "sleipnir/autodiff/Variable.hpp"
  #include "sleipnir/autodiff/VariableMatrix.hpp"
-@@ -22,7 +23,6 @@
+@@ -21,7 +22,6 @@
+ #include "sleipnir/optimization/solver/Newton.hpp"
  #include "sleipnir/optimization/solver/SQP.hpp"
- #include "sleipnir/util/Print.hpp"
  #include "sleipnir/util/SymbolExports.hpp"
 -#include "sleipnir/util/small_vector.hpp"
  
- namespace sleipnir {
- 
-@@ -364,16 +364,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
+ #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+ #include <array>
+@@ -378,19 +378,19 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
   private:
    // The list of decision variables, which are the root of the problem's
    // expression tree
@@ -373,7 +499,11 @@ index 569dcdee507881ceb412585ca811927072552c15..66883fed98ad087010fb153bd91effce
 +  wpi::SmallVector<Variable> m_inequalityConstraints;
  
    // The user callback
-   std::function<bool(const SolverIterationInfo& info)> m_callback =
+-  small_vector<std::function<bool(const SolverIterationInfo& info)>>
++  wpi::SmallVector<std::function<bool(const SolverIterationInfo& info)>>
+       m_callbacks;
+ 
+   // The solver status
 diff --git a/include/sleipnir/util/Pool.hpp b/include/sleipnir/util/Pool.hpp
 index 441fa701d4972bc14973c6269d56d4a124deaee5..1951bd1ee8f3bee8d4a3c044c99354b0fd10031d 100644
 --- a/include/sleipnir/util/Pool.hpp
@@ -401,29 +531,17 @@ index 441fa701d4972bc14973c6269d56d4a124deaee5..1951bd1ee8f3bee8d4a3c044c99354b0
  
    /**
 diff --git a/include/sleipnir/util/Spy.hpp b/include/sleipnir/util/Spy.hpp
-index cb9b4e191545e96c2bade5f8f99b0bec376b656b..7f526a2d9968e76b385a0ddfb2edf5bab7274fb0 100644
+index 1abeb469a43d3d644efc1ad0e06e43928149ed0a..3cc5fef0674772d28206f82e7f71b94db0af17b0 100644
 --- a/include/sleipnir/util/Spy.hpp
 +++ b/include/sleipnir/util/Spy.hpp
-@@ -7,9 +7,9 @@
+@@ -10,6 +10,7 @@
  #include <string_view>
  
  #include <Eigen/SparseCore>
 +#include <wpi/SmallVector.h>
  
  #include "sleipnir/util/SymbolExports.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- namespace sleipnir {
  
-@@ -32,7 +32,7 @@ SLEIPNIR_DLLEXPORT inline void Spy(std::ostream& file,
-   const int cells_width = mat.cols() + 1;
-   const int cells_height = mat.rows();
- 
--  small_vector<uint8_t> cells;
-+  wpi::SmallVector<uint8_t> cells;
- 
-   // Allocate space for matrix of characters plus trailing newlines
-   cells.reserve(cells_width * cells_height);
 diff --git a/src/.styleguide b/src/.styleguide
 index f3b2f0cf9e60b3a86b9654ff2b381f9c48734ff6..ad739cea6dce6f6cb586f538d1d30b92503484c1 100644
 --- a/src/.styleguide
@@ -435,26 +553,35 @@ index f3b2f0cf9e60b3a86b9654ff2b381f9c48734ff6..ad739cea6dce6f6cb586f538d1d30b92
 +  ^wpi/
  }
 diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp
-index a09d9866d05731c8ce53122b3d1a850803883df4..d3981c59d163927e3e5ba602c3323f6e1429c475 100644
+index af2115b2090dd828bb0dcef48ff27fe3cdf3eac5..0abc539fed840d7f113c2b27f39c7a9f14345b89 100644
 --- a/src/optimization/solver/InteriorPoint.cpp
 +++ b/src/optimization/solver/InteriorPoint.cpp
-@@ -9,6 +9,7 @@
- #include <limits>
+@@ -11,6 +11,7 @@
+ #include <ranges>
  
  #include <Eigen/SparseCholesky>
 +#include <wpi/SmallVector.h>
  
  #include "optimization/RegularizedLDLT.hpp"
  #include "optimization/solver/util/ErrorEstimate.hpp"
-@@ -23,7 +24,6 @@
- #include "sleipnir/optimization/SolverExitCondition.hpp"
- #include "sleipnir/util/Print.hpp"
- #include "sleipnir/util/Spy.hpp"
+@@ -25,7 +26,6 @@
+ #include "sleipnir/util/ScopedProfiler.hpp"
+ #include "sleipnir/util/SetupProfiler.hpp"
+ #include "sleipnir/util/SolveProfiler.hpp"
 -#include "sleipnir/util/small_vector.hpp"
  #include "util/ScopeExit.hpp"
- #include "util/ToMilliseconds.hpp"
  
-@@ -226,7 +226,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
+ #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+@@ -49,7 +49,7 @@ void InteriorPoint(
+     const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
+   const auto solveStartTime = std::chrono::steady_clock::now();
+ 
+-  small_vector<SetupProfiler> setupProfilers;
++  wpi::SmallVector<SetupProfiler> setupProfilers;
+   setupProfilers.emplace_back("setup").Start();
+ 
+   setupProfilers.emplace_back("  ↳ s,y,z setup").Start();
+@@ -241,7 +241,7 @@ void InteriorPoint(
    };
  
    // Kept outside the loop so its storage can be reused
@@ -463,27 +590,67 @@ index a09d9866d05731c8ce53122b3d1a850803883df4..d3981c59d163927e3e5ba602c3323f6e
  
    RegularizedLDLT solver;
  
+@@ -258,7 +258,7 @@ void InteriorPoint(
+ 
+   setupProfilers[0].Stop();
+ 
+-  small_vector<SolveProfiler> solveProfilers;
++  wpi::SmallVector<SolveProfiler> solveProfilers;
+   solveProfilers.emplace_back("solve");
+   solveProfilers.emplace_back("  ↳ feasibility ✓");
+   solveProfilers.emplace_back("  ↳ user callbacks");
+diff --git a/src/optimization/solver/Newton.cpp b/src/optimization/solver/Newton.cpp
+index 7656ba683dbe062e31391c3cd210690b7bfc0616..185d3abb5b4ca03b6e0f72af2846927d2c223210 100644
+--- a/src/optimization/solver/Newton.cpp
++++ b/src/optimization/solver/Newton.cpp
+@@ -40,7 +40,7 @@ void Newton(
+     const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
+   const auto solveStartTime = std::chrono::steady_clock::now();
+ 
+-  small_vector<SetupProfiler> setupProfilers;
++  wpi::SmallVector<SetupProfiler> setupProfilers;
+   setupProfilers.emplace_back("setup").Start();
+ 
+   // Map decision variables and constraints to VariableMatrices for Lagrangian
+@@ -123,7 +123,7 @@ void Newton(
+ 
+   setupProfilers[0].Stop();
+ 
+-  small_vector<SolveProfiler> solveProfilers;
++  wpi::SmallVector<SolveProfiler> solveProfilers;
+   solveProfilers.emplace_back("solve");
+   solveProfilers.emplace_back("  ↳ feasibility ✓");
+   solveProfilers.emplace_back("  ↳ user callbacks");
 diff --git a/src/optimization/solver/SQP.cpp b/src/optimization/solver/SQP.cpp
-index 77b9632e1da37361c995a8579d1d83a2756d6d88..662abc2fb6e311767b0fbb3a61121ce78549a3f6 100644
+index e5f32b6b93f13e904085d0323fbda77726c0a873..b40c0708754f7540b262d04d6effe7b3ec2ab48f 100644
 --- a/src/optimization/solver/SQP.cpp
 +++ b/src/optimization/solver/SQP.cpp
-@@ -9,6 +9,7 @@
- #include <limits>
+@@ -11,6 +11,7 @@
+ #include <ranges>
  
  #include <Eigen/SparseCholesky>
 +#include <wpi/SmallVector.h>
  
  #include "optimization/RegularizedLDLT.hpp"
  #include "optimization/solver/util/ErrorEstimate.hpp"
-@@ -22,7 +23,6 @@
- #include "sleipnir/optimization/SolverExitCondition.hpp"
- #include "sleipnir/util/Print.hpp"
- #include "sleipnir/util/Spy.hpp"
+@@ -24,7 +25,6 @@
+ #include "sleipnir/util/ScopedProfiler.hpp"
+ #include "sleipnir/util/SetupProfiler.hpp"
+ #include "sleipnir/util/SolveProfiler.hpp"
 -#include "sleipnir/util/small_vector.hpp"
  #include "util/ScopeExit.hpp"
- #include "util/ToMilliseconds.hpp"
  
-@@ -155,7 +155,7 @@ void SQP(std::span<Variable> decisionVariables,
+ #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+@@ -44,7 +44,7 @@ void SQP(
+     const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
+   const auto solveStartTime = std::chrono::steady_clock::now();
+ 
+-  small_vector<SetupProfiler> setupProfilers;
++  wpi::SmallVector<SetupProfiler> setupProfilers;
+   setupProfilers.emplace_back("setup").Start();
+ 
+   setupProfilers.emplace_back("  ↳ y setup").Start();
+@@ -167,7 +167,7 @@ void SQP(
    Filter filter{f};
  
    // Kept outside the loop so its storage can be reused
@@ -492,100 +659,17 @@ index 77b9632e1da37361c995a8579d1d83a2756d6d88..662abc2fb6e311767b0fbb3a61121ce7
  
    RegularizedLDLT solver;
  
-diff --git a/src/optimization/solver/util/FeasibilityRestoration.hpp b/src/optimization/solver/util/FeasibilityRestoration.hpp
-index feefe137adf0832b094a36d61201b15962138ded..79b5d99ae27de6049ba098888a965291e6b677fa 100644
---- a/src/optimization/solver/util/FeasibilityRestoration.hpp
-+++ b/src/optimization/solver/util/FeasibilityRestoration.hpp
-@@ -8,6 +8,7 @@
- #include <span>
+@@ -183,7 +183,7 @@ void SQP(
  
- #include <Eigen/Core>
-+#include <wpi/SmallVector.h>
+   setupProfilers[0].Stop();
  
- #include "sleipnir/autodiff/Variable.hpp"
- #include "sleipnir/autodiff/VariableMatrix.hpp"
-@@ -16,7 +17,6 @@
- #include "sleipnir/optimization/SolverStatus.hpp"
- #include "sleipnir/optimization/solver/InteriorPoint.hpp"
- #include "sleipnir/util/FunctionRef.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- namespace sleipnir {
- 
-@@ -57,7 +57,7 @@ inline void FeasibilityRestoration(
-   constexpr double ρ = 1000.0;
-   double μ = config.tolerance / 10.0;
- 
--  small_vector<Variable> fr_decisionVariables;
-+  wpi::SmallVector<Variable> fr_decisionVariables;
-   fr_decisionVariables.reserve(decisionVariables.size() +
-                                2 * equalityConstraints.size());
- 
-@@ -70,7 +70,7 @@ inline void FeasibilityRestoration(
-     fr_decisionVariables.emplace_back();
-   }
- 
--  auto it = fr_decisionVariables.cbegin();
-+  auto it = fr_decisionVariables.begin();
- 
-   VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
-   it += decisionVariables.size();
-@@ -128,7 +128,7 @@ inline void FeasibilityRestoration(
-   }
- 
-   // cₑ(x) - pₑ + nₑ = 0
--  small_vector<Variable> fr_equalityConstraints;
-+  wpi::SmallVector<Variable> fr_equalityConstraints;
-   fr_equalityConstraints.assign(equalityConstraints.begin(),
-                                 equalityConstraints.end());
-   for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
-@@ -137,7 +137,7 @@ inline void FeasibilityRestoration(
-   }
- 
-   // cᵢ(x) - s - pᵢ + nᵢ = 0
--  small_vector<Variable> fr_inequalityConstraints;
-+  wpi::SmallVector<Variable> fr_inequalityConstraints;
- 
-   // pₑ ≥ 0
-   std::copy(p_e.begin(), p_e.end(),
-@@ -227,7 +227,7 @@ inline void FeasibilityRestoration(
- 
-   constexpr double ρ = 1000.0;
- 
--  small_vector<Variable> fr_decisionVariables;
-+  wpi::SmallVector<Variable> fr_decisionVariables;
-   fr_decisionVariables.reserve(decisionVariables.size() +
-                                2 * equalityConstraints.size() +
-                                2 * inequalityConstraints.size());
-@@ -243,7 +243,7 @@ inline void FeasibilityRestoration(
-     fr_decisionVariables.emplace_back();
-   }
- 
--  auto it = fr_decisionVariables.cbegin();
-+  auto it = fr_decisionVariables.begin();
- 
-   VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
-   it += decisionVariables.size();
-@@ -319,7 +319,7 @@ inline void FeasibilityRestoration(
-   }
- 
-   // cₑ(x) - pₑ + nₑ = 0
--  small_vector<Variable> fr_equalityConstraints;
-+  wpi::SmallVector<Variable> fr_equalityConstraints;
-   fr_equalityConstraints.assign(equalityConstraints.begin(),
-                                 equalityConstraints.end());
-   for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
-@@ -328,7 +328,7 @@ inline void FeasibilityRestoration(
-   }
- 
-   // cᵢ(x) - s - pᵢ + nᵢ = 0
--  small_vector<Variable> fr_inequalityConstraints;
-+  wpi::SmallVector<Variable> fr_inequalityConstraints;
-   fr_inequalityConstraints.assign(inequalityConstraints.begin(),
-                                   inequalityConstraints.end());
-   for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) {
+-  small_vector<SolveProfiler> solveProfilers;
++  wpi::SmallVector<SolveProfiler> solveProfilers;
+   solveProfilers.emplace_back("solve");
+   solveProfilers.emplace_back("  ↳ feasibility ✓");
+   solveProfilers.emplace_back("  ↳ user callbacks");
 diff --git a/src/optimization/solver/util/Filter.hpp b/src/optimization/solver/util/Filter.hpp
-index f19236928c59623bc0a3ce87b659f0756997f821..0c14efd7b8afa6cef398f5a7d383c54dbf64ec68 100644
+index 3c63661b1264db346df05bf5db86bae72b5dd4a5..ba625b5a85c195cfe8e7b5f1f26bc25ec7b4e867 100644
 --- a/src/optimization/solver/util/Filter.hpp
 +++ b/src/optimization/solver/util/Filter.hpp
 @@ -8,9 +8,9 @@
@@ -599,12 +683,49 @@ index f19236928c59623bc0a3ce87b659f0756997f821..0c14efd7b8afa6cef398f5a7d383c54d
  
  // See docs/algorithms.md#Works_cited for citation definitions.
  
-@@ -177,7 +177,7 @@ class Filter {
+@@ -189,7 +189,7 @@ class Filter {
+   static constexpr double γConstraint = 1e-5;
  
-  private:
    Variable* m_f = nullptr;
 -  small_vector<FilterEntry> m_filter;
 +  wpi::SmallVector<FilterEntry> m_filter;
  };
  
  }  // namespace sleipnir
+diff --git a/src/util/PrintDiagnostics.hpp b/src/util/PrintDiagnostics.hpp
+index 0886797a217e997f8f463ecb0671fb7c154de70f..00fb7c6ca9321d6f1551de99573c9ff7a9e07567 100644
+--- a/src/util/PrintDiagnostics.hpp
++++ b/src/util/PrintDiagnostics.hpp
+@@ -11,10 +11,11 @@
+ #include <string>
+ #include <utility>
+ 
++#include <wpi/SmallVector.h>
++
+ #include "sleipnir/util/Print.hpp"
+ #include "sleipnir/util/SetupProfiler.hpp"
+ #include "sleipnir/util/SolveProfiler.hpp"
+-#include "sleipnir/util/small_vector.hpp"
+ #include "util/ToMs.hpp"
+ 
+ namespace sleipnir {
+@@ -88,7 +89,7 @@ void PrintIterationDiagnostics(int iterations, IterationMode mode,
+     } else {
+       // Gather regularization exponent digits
+       int n = std::abs(exponent);
+-      small_vector<int> digits;
++      wpi::SmallVector<int> digits;
+       do {
+         digits.emplace_back(n % 10);
+         n /= 10;
+@@ -157,8 +158,8 @@ inline std::string Histogram(double value) {
+  * @param solveProfilers Solve profilers.
+  */
+ inline void PrintFinalDiagnostics(
+-    int iterations, const small_vector<SetupProfiler>& setupProfilers,
+-    const small_vector<SolveProfiler>& solveProfilers) {
++    int iterations, const wpi::SmallVector<SetupProfiler>& setupProfilers,
++    const wpi::SmallVector<SolveProfiler>& solveProfilers) {
+   // Print bottom of iteration diagnostics table
+   sleipnir::println("└{:─^106}┘", "");
+ 
diff --git a/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch b/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch
new file mode 100644
index 00000000000..acffc6822d5
--- /dev/null
+++ b/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch
@@ -0,0 +1,30 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Tue, 28 Jan 2025 22:19:14 -0800
+Subject: [PATCH 3/6] Use wpi::byteswap()
+
+---
+ include/sleipnir/util/Spy.hpp | 3 ++-
+ 1 file changed, 2 insertions(+), 1 deletion(-)
+
+diff --git a/include/sleipnir/util/Spy.hpp b/include/sleipnir/util/Spy.hpp
+index 3cc5fef0674772d28206f82e7f71b94db0af17b0..a32728d8433a042e60bf8cc6c3b472a1292ee1de 100644
+--- a/include/sleipnir/util/Spy.hpp
++++ b/include/sleipnir/util/Spy.hpp
+@@ -11,6 +11,7 @@
+ 
+ #include <Eigen/SparseCore>
+ #include <wpi/SmallVector.h>
++#include <wpi/bit.h>
+ 
+ #include "sleipnir/util/SymbolExports.hpp"
+ 
+@@ -113,7 +114,7 @@ class SLEIPNIR_DLLEXPORT Spy {
+    */
+   void Write32le(int32_t num) {
+     if constexpr (std::endian::native != std::endian::little) {
+-      num = std::byteswap(num);
++      num = wpi::byteswap(num);
+     }
+     m_file.write(reinterpret_cast<char*>(&num), sizeof(num));
+   }
diff --git a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch
new file mode 100644
index 00000000000..0a9259cf494
--- /dev/null
+++ b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch
@@ -0,0 +1,30 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Tue, 28 Jan 2025 22:19:31 -0800
+Subject: [PATCH 4/6] Replace std::to_underlying()
+
+---
+ src/util/PrintDiagnostics.hpp | 3 +--
+ 1 file changed, 1 insertion(+), 2 deletions(-)
+
+diff --git a/src/util/PrintDiagnostics.hpp b/src/util/PrintDiagnostics.hpp
+index 00fb7c6ca9321d6f1551de99573c9ff7a9e07567..083d221da8f67ce51eeb2e6caf98c38f9d866ab1 100644
+--- a/src/util/PrintDiagnostics.hpp
++++ b/src/util/PrintDiagnostics.hpp
+@@ -9,7 +9,6 @@
+ #include <cmath>
+ #include <ranges>
+ #include <string>
+-#include <utility>
+ 
+ #include <wpi/SmallVector.h>
+ 
+@@ -73,7 +72,7 @@ void PrintIterationDiagnostics(int iterations, IterationMode mode,
+ 
+   constexpr const char* kIterationModes[] = {"norm", "SOC"};
+   sleipnir::print("│ {:4}   {:4}   {:9.3f}   {:12e}   {:12e}   {:13e}   ",
+-                  iterations, kIterationModes[std::to_underlying(mode)],
++                  iterations, kIterationModes[static_cast<uint8_t>(mode)],
+                   ToMs(time), error, cost, infeasibility);
+ 
+   // Print regularization
diff --git a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch
new file mode 100644
index 00000000000..9999f7d6d54
--- /dev/null
+++ b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch
@@ -0,0 +1,25 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Sat, 8 Feb 2025 13:42:36 -0800
+Subject: [PATCH 5/6] Replace std::views::zip()
+
+---
+ include/sleipnir/autodiff/AdjointExpressionGraph.hpp | 5 ++++-
+ 1 file changed, 4 insertions(+), 1 deletion(-)
+
+diff --git a/include/sleipnir/autodiff/AdjointExpressionGraph.hpp b/include/sleipnir/autodiff/AdjointExpressionGraph.hpp
+index 4e67df3dfb19b28e0da522cdb9b76f69138d3143..7cf93a7152867af242c8ef344fd4a429b1f84174 100644
+--- a/include/sleipnir/autodiff/AdjointExpressionGraph.hpp
++++ b/include/sleipnir/autodiff/AdjointExpressionGraph.hpp
+@@ -102,7 +102,10 @@ class AdjointExpressionGraph {
+                              int row) const {
+     detail::UpdateAdjoints(m_topList);
+ 
+-    for (const auto& [node, col] : std::views::zip(m_topList, m_colList)) {
++    for (size_t i = 0; i < m_topList.size(); ++i) {
++      const auto& node = m_topList[i];
++      const auto& col = m_colList[i];
++
+       // Append adjoints of wrt to sparse matrix triplets
+       if (col != -1 && node->adjoint != 0.0) {
+         triplets.emplace_back(row, col, node->adjoint);
diff --git a/upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch b/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch
similarity index 80%
rename from upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch
rename to upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch
index 90ae6e261bd..c3acf9f09ef 100644
--- a/upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch
+++ b/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch
@@ -1,17 +1,17 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Wed, 26 Jun 2024 12:13:33 -0700
-Subject: [PATCH 3/3] Suppress clang-tidy false positives
+Subject: [PATCH 6/6] Suppress clang-tidy false positives
 
 ---
  include/sleipnir/autodiff/Variable.hpp | 4 ++--
  1 file changed, 2 insertions(+), 2 deletions(-)
 
 diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp
-index d192fb96e7984b7c0ca30262668c41e5e84ca34e..f25c6d153310a01700ee2390ecf35ffa8af7df11 100644
+index c9dd5a824a8c22bc0bd645d77fea0ee149f54c90..e9f5fa12baf8f4797034a638d061588a5869bf7f 100644
 --- a/include/sleipnir/autodiff/Variable.hpp
 +++ b/include/sleipnir/autodiff/Variable.hpp
-@@ -541,7 +541,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
+@@ -573,7 +573,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
     *
     * @param equalityConstraints The list of EqualityConstraints to concatenate.
     */
@@ -20,7 +20,7 @@ index d192fb96e7984b7c0ca30262668c41e5e84ca34e..f25c6d153310a01700ee2390ecf35ffa
        std::initializer_list<EqualityConstraints> equalityConstraints) {
      for (const auto& elem : equalityConstraints) {
        constraints.insert(constraints.end(), elem.constraints.begin(),
-@@ -604,7 +604,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
+@@ -636,7 +636,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
     * @param inequalityConstraints The list of InequalityConstraints to
     * concatenate.
     */
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/.clang-tidy b/wpimath/src/main/native/thirdparty/sleipnir/.clang-tidy
index cfb2b12f2a7..097db816f88 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/.clang-tidy
+++ b/wpimath/src/main/native/thirdparty/sleipnir/.clang-tidy
@@ -46,6 +46,7 @@ Checks:
   -clang-diagnostic-pedantic,
   clang-analyzer-*,
   -clang-analyzer-core.uninitialized.UndefReturn,
+  -clang-analyzer-optin.core.EnumCastOutOfRange,
   -clang-analyzer-optin.cplusplus.UninitializedObject,
   -clang-analyzer-optin.portability.UnixAPI,
   -clang-analyzer-unix.Malloc,
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/AdjointExpressionGraph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/AdjointExpressionGraph.hpp
new file mode 100644
index 00000000000..4e67df3dfb1
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/AdjointExpressionGraph.hpp
@@ -0,0 +1,121 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <ranges>
+#include <utility>
+
+#include <Eigen/SparseCore>
+#include <wpi/SmallVector.h>
+
+#include "sleipnir/autodiff/ExpressionGraph.hpp"
+#include "sleipnir/autodiff/Variable.hpp"
+#include "sleipnir/autodiff/VariableMatrix.hpp"
+
+namespace sleipnir::detail {
+
+/**
+ * This class is an adaptor type that performs value updates of an expression's
+ * adjoint graph.
+ */
+class AdjointExpressionGraph {
+ public:
+  /**
+   * Generates the adjoint graph for the given expression.
+   *
+   * @param root The root node of the expression.
+   */
+  explicit AdjointExpressionGraph(const Variable& root)
+      : m_topList{TopologicalSort(root.expr)} {
+    for (const auto& node : m_topList) {
+      m_colList.emplace_back(node->col);
+    }
+  }
+
+  /**
+   * Update the values of all nodes in this adjoint graph based on the values of
+   * their dependent nodes.
+   */
+  void UpdateValues() { detail::UpdateValues(m_topList); }
+
+  /**
+   * Returns the variable's gradient tree.
+   *
+   * This function lazily allocates variables, so elements of the returned
+   * VariableMatrix will be empty if the corresponding element of wrt had no
+   * adjoint. Ensure Variable::expr isn't nullptr before calling member
+   * functions.
+   *
+   * @param wrt Variables with respect to which to compute the gradient.
+   */
+  VariableMatrix GenerateGradientTree(const VariableMatrix& wrt) const {
+    // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
+    // for background on reverse accumulation automatic differentiation.
+
+    if (m_topList.empty()) {
+      return VariableMatrix{VariableMatrix::empty, wrt.Rows(), 1};
+    }
+
+    // Set root node's adjoint to 1 since df/df is 1
+    m_topList[0]->adjointExpr = MakeExpressionPtr<ConstExpression>(1.0);
+
+    // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y
+    // multiplied by dy/dx. If there are multiple "paths" from the root node to
+    // variable; the variable's adjoint is the sum of each path's adjoint
+    // contribution.
+    for (auto& node : m_topList) {
+      auto& lhs = node->args[0];
+      auto& rhs = node->args[1];
+
+      if (lhs != nullptr) {
+        lhs->adjointExpr += node->GradExprL(lhs, rhs, node->adjointExpr);
+        if (rhs != nullptr) {
+          rhs->adjointExpr += node->GradExprR(lhs, rhs, node->adjointExpr);
+        }
+      }
+    }
+
+    // Move gradient tree to return value
+    VariableMatrix grad{VariableMatrix::empty, wrt.Rows(), 1};
+    for (int row = 0; row < grad.Rows(); ++row) {
+      grad(row) = Variable{std::move(wrt(row).expr->adjointExpr)};
+    }
+
+    // Unlink adjoints to avoid circular references between them and their
+    // parent expressions. This ensures all expressions are returned to the free
+    // list.
+    for (auto& node : m_topList) {
+      node->adjointExpr = nullptr;
+    }
+
+    return grad;
+  }
+
+  /**
+   * Updates the adjoints in the expression graph (computes the gradient) then
+   * appends the adjoints of wrt to the sparse matrix triplets.
+   *
+   * @param triplets The sparse matrix triplets.
+   * @param row The row of wrt.
+   */
+  void AppendAdjointTriplets(wpi::SmallVector<Eigen::Triplet<double>>& triplets,
+                             int row) const {
+    detail::UpdateAdjoints(m_topList);
+
+    for (const auto& [node, col] : std::views::zip(m_topList, m_colList)) {
+      // Append adjoints of wrt to sparse matrix triplets
+      if (col != -1 && node->adjoint != 0.0) {
+        triplets.emplace_back(row, col, node->adjoint);
+      }
+    }
+  }
+
+ private:
+  // Topological sort of graph from parent to child
+  wpi::SmallVector<Expression*> m_topList;
+
+  // List that maps nodes to their respective column
+  wpi::SmallVector<int> m_colList;
+};
+
+}  // namespace sleipnir::detail
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp
index ef9a15bf69d..8a2d1ca81d1 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp
@@ -16,7 +16,6 @@
 #include "sleipnir/autodiff/ExpressionType.hpp"
 #include "sleipnir/util/IntrusiveSharedPtr.hpp"
 #include "sleipnir/util/Pool.hpp"
-#include "sleipnir/util/SymbolExports.hpp"
 
 namespace sleipnir::detail {
 
@@ -28,7 +27,7 @@ inline constexpr bool kUsePoolAllocator = false;
 inline constexpr bool kUsePoolAllocator = true;
 #endif
 
-struct SLEIPNIR_DLLEXPORT Expression;
+struct Expression;
 
 inline void IntrusiveSharedPtrIncRefCount(Expression* expr);
 inline void IntrusiveSharedPtrDecRefCount(Expression* expr);
@@ -42,88 +41,59 @@ using ExpressionPtr = IntrusiveSharedPtr<Expression>;
  * Creates an intrusive shared pointer to an expression from the global pool
  * allocator.
  *
+ * @tparam T The derived expression type.
  * @param args Constructor arguments for Expression.
  */
-template <typename... Args>
+template <typename T, typename... Args>
 static ExpressionPtr MakeExpressionPtr(Args&&... args) {
   if constexpr (kUsePoolAllocator) {
-    return AllocateIntrusiveShared<Expression>(
-        GlobalPoolAllocator<Expression>(), std::forward<Args>(args)...);
+    return AllocateIntrusiveShared<T>(GlobalPoolAllocator<T>(),
+                                      std::forward<Args>(args)...);
   } else {
-    return MakeIntrusiveShared<Expression>(std::forward<Args>(args)...);
+    return MakeIntrusiveShared<T>(std::forward<Args>(args)...);
   }
 }
 
-/**
- * An autodiff expression node.
- */
-struct SLEIPNIR_DLLEXPORT Expression {
-  /**
-   * Binary function taking two doubles and returning a double.
-   */
-  using BinaryFuncDouble = double (*)(double, double);
+template <ExpressionType T>
+struct BinaryMinusExpression;
 
-  /**
-   * Trinary function taking three doubles and returning a double.
-   */
-  using TrinaryFuncDouble = double (*)(double, double, double);
+template <ExpressionType T>
+struct BinaryPlusExpression;
 
-  /**
-   * Trinary function taking three expressions and returning an expression.
-   */
-  using TrinaryFuncExpr = ExpressionPtr (*)(const ExpressionPtr&,
-                                            const ExpressionPtr&,
-                                            const ExpressionPtr&);
+struct ConstExpression;
+
+template <ExpressionType T>
+struct DivExpression;
+
+template <ExpressionType T>
+struct MultExpression;
+
+template <ExpressionType T>
+struct UnaryMinusExpression;
 
+/**
+ * An autodiff expression node.
+ */
+struct Expression {
   /// The value of the expression node.
   double value = 0.0;
 
   /// The adjoint of the expression node used during autodiff.
   double adjoint = 0.0;
 
-  /// Tracks the number of instances of this expression yet to be encountered in
-  /// an expression tree.
-  uint32_t duplications = 0;
+  /// Counts incoming edges for this node.
+  uint32_t incomingEdges = 0;
 
-  /// This expression's row in wrt for autodiff gradient, Jacobian, or Hessian.
-  /// This is -1 if the expression isn't in wrt.
-  int32_t row = -1;
+  /// This expression's column in a Jacobian, or -1 otherwise.
+  int32_t col = -1;
 
   /// The adjoint of the expression node used during gradient expression tree
   /// generation.
   ExpressionPtr adjointExpr;
 
-  /// Expression argument type.
-  ExpressionType type = ExpressionType::kConstant;
-
   /// Reference count for intrusive shared pointer.
   uint32_t refCount = 0;
 
-  /// Either nullary operator with no arguments, unary operator with one
-  /// argument, or binary operator with two arguments. This operator is
-  /// used to update the node's value.
-  BinaryFuncDouble valueFunc = nullptr;
-
-  /// Functions returning double adjoints of the children expressions.
-  ///
-  /// Parameters:
-  /// <ul>
-  ///   <li>lhs: Left argument to binary operator.</li>
-  ///   <li>rhs: Right argument to binary operator.</li>
-  ///   <li>parentAdjoint: Adjoint of parent expression.</li>
-  /// </ul>
-  std::array<TrinaryFuncDouble, 2> gradientValueFuncs{nullptr, nullptr};
-
-  /// Functions returning Variable adjoints of the children expressions.
-  ///
-  /// Parameters:
-  /// <ul>
-  ///   <li>lhs: Left argument to binary operator.</li>
-  ///   <li>rhs: Right argument to binary operator.</li>
-  ///   <li>parentAdjoint: Adjoint of parent expression.</li>
-  /// </ul>
-  std::array<TrinaryFuncExpr, 2> gradientFuncs{nullptr, nullptr};
-
   /// Expression arguments.
   std::array<ExpressionPtr, 2> args{nullptr, nullptr};
 
@@ -136,56 +106,27 @@ struct SLEIPNIR_DLLEXPORT Expression {
    * Constructs a nullary expression (an operator with no arguments).
    *
    * @param value The expression value.
-   * @param type The expression type. It should be either constant (the default)
-   *             or linear.
    */
-  explicit constexpr Expression(double value,
-                                ExpressionType type = ExpressionType::kConstant)
-      : value{value}, type{type} {}
+  explicit constexpr Expression(double value) : value{value} {}
 
   /**
    * Constructs an unary expression (an operator with one argument).
    *
-   * @param type The expression's type.
-   * @param valueFunc Unary operator that produces this expression's value.
-   * @param lhsGradientValueFunc Gradient with respect to the operand.
-   * @param lhsGradientFunc Gradient with respect to the operand.
    * @param lhs Unary operator's operand.
    */
-  constexpr Expression(ExpressionType type, BinaryFuncDouble valueFunc,
-                       TrinaryFuncDouble lhsGradientValueFunc,
-                       TrinaryFuncExpr lhsGradientFunc, ExpressionPtr lhs)
-      : value{valueFunc(lhs->value, 0.0)},
-        type{type},
-        valueFunc{valueFunc},
-        gradientValueFuncs{lhsGradientValueFunc, nullptr},
-        gradientFuncs{lhsGradientFunc, nullptr},
-        args{lhs, nullptr} {}
+  explicit constexpr Expression(ExpressionPtr lhs)
+      : args{std::move(lhs), nullptr} {}
 
   /**
    * Constructs a binary expression (an operator with two arguments).
    *
-   * @param type The expression's type.
-   * @param valueFunc Unary operator that produces this expression's value.
-   * @param lhsGradientValueFunc Gradient with respect to the left operand.
-   * @param rhsGradientValueFunc Gradient with respect to the right operand.
-   * @param lhsGradientFunc Gradient with respect to the left operand.
-   * @param rhsGradientFunc Gradient with respect to the right operand.
    * @param lhs Binary operator's left operand.
    * @param rhs Binary operator's right operand.
    */
-  constexpr Expression(ExpressionType type, BinaryFuncDouble valueFunc,
-                       TrinaryFuncDouble lhsGradientValueFunc,
-                       TrinaryFuncDouble rhsGradientValueFunc,
-                       TrinaryFuncExpr lhsGradientFunc,
-                       TrinaryFuncExpr rhsGradientFunc, ExpressionPtr lhs,
-                       ExpressionPtr rhs)
-      : value{valueFunc(lhs->value, rhs->value)},
-        type{type},
-        valueFunc{valueFunc},
-        gradientValueFuncs{lhsGradientValueFunc, rhsGradientValueFunc},
-        gradientFuncs{lhsGradientFunc, rhsGradientFunc},
-        args{lhs, rhs} {}
+  constexpr Expression(ExpressionPtr lhs, ExpressionPtr rhs)
+      : args{std::move(lhs), std::move(rhs)} {}
+
+  virtual ~Expression() = default;
 
   /**
    * Returns true if the expression is the given constant.
@@ -193,7 +134,7 @@ struct SLEIPNIR_DLLEXPORT Expression {
    * @param constant The constant.
    */
   constexpr bool IsConstant(double constant) const {
-    return type == ExpressionType::kConstant && value == constant;
+    return Type() == ExpressionType::kConstant && value == constant;
   }
 
   /**
@@ -202,8 +143,8 @@ struct SLEIPNIR_DLLEXPORT Expression {
    * @param lhs Operator left-hand side.
    * @param rhs Operator right-hand side.
    */
-  friend SLEIPNIR_DLLEXPORT ExpressionPtr operator*(const ExpressionPtr& lhs,
-                                                    const ExpressionPtr& rhs) {
+  friend ExpressionPtr operator*(const ExpressionPtr& lhs,
+                                 const ExpressionPtr& rhs) {
     using enum ExpressionType;
 
     // Prune expression
@@ -220,35 +161,32 @@ struct SLEIPNIR_DLLEXPORT Expression {
     }
 
     // Evaluate constant
-    if (lhs->type == kConstant && rhs->type == kConstant) {
-      return MakeExpressionPtr(lhs->value * rhs->value);
+    if (lhs->Type() == kConstant && rhs->Type() == kConstant) {
+      return MakeExpressionPtr<ConstExpression>(lhs->value * rhs->value);
     }
 
     // Evaluate expression type
-    ExpressionType type;
-    if (lhs->type == kConstant) {
-      type = rhs->type;
-    } else if (rhs->type == kConstant) {
-      type = lhs->type;
-    } else if (lhs->type == kLinear && rhs->type == kLinear) {
-      type = kQuadratic;
+    if (lhs->Type() == kConstant) {
+      if (rhs->Type() == kLinear) {
+        return MakeExpressionPtr<MultExpression<kLinear>>(lhs, rhs);
+      } else if (rhs->Type() == kQuadratic) {
+        return MakeExpressionPtr<MultExpression<kQuadratic>>(lhs, rhs);
+      } else {
+        return MakeExpressionPtr<MultExpression<kNonlinear>>(lhs, rhs);
+      }
+    } else if (rhs->Type() == kConstant) {
+      if (lhs->Type() == kLinear) {
+        return MakeExpressionPtr<MultExpression<kLinear>>(lhs, rhs);
+      } else if (lhs->Type() == kQuadratic) {
+        return MakeExpressionPtr<MultExpression<kQuadratic>>(lhs, rhs);
+      } else {
+        return MakeExpressionPtr<MultExpression<kNonlinear>>(lhs, rhs);
+      }
+    } else if (lhs->Type() == kLinear && rhs->Type() == kLinear) {
+      return MakeExpressionPtr<MultExpression<kQuadratic>>(lhs, rhs);
     } else {
-      type = kNonlinear;
+      return MakeExpressionPtr<MultExpression<kNonlinear>>(lhs, rhs);
     }
-
-    return MakeExpressionPtr(
-        type, [](double lhs, double rhs) { return lhs * rhs; },
-        [](double, double rhs, double parentAdjoint) {
-          return parentAdjoint * rhs;
-        },
-        [](double lhs, double, double parentAdjoint) {
-          return parentAdjoint * lhs;
-        },
-        [](const ExpressionPtr&, const ExpressionPtr& rhs,
-           const ExpressionPtr& parentAdjoint) { return parentAdjoint * rhs; },
-        [](const ExpressionPtr& lhs, const ExpressionPtr&,
-           const ExpressionPtr& parentAdjoint) { return parentAdjoint * lhs; },
-        lhs, rhs);
   }
 
   /**
@@ -257,8 +195,8 @@ struct SLEIPNIR_DLLEXPORT Expression {
    * @param lhs Operator left-hand side.
    * @param rhs Operator right-hand side.
    */
-  friend SLEIPNIR_DLLEXPORT ExpressionPtr operator/(const ExpressionPtr& lhs,
-                                                    const ExpressionPtr& rhs) {
+  friend ExpressionPtr operator/(const ExpressionPtr& lhs,
+                                 const ExpressionPtr& rhs) {
     using enum ExpressionType;
 
     // Prune expression
@@ -270,33 +208,22 @@ struct SLEIPNIR_DLLEXPORT Expression {
     }
 
     // Evaluate constant
-    if (lhs->type == kConstant && rhs->type == kConstant) {
-      return MakeExpressionPtr(lhs->value / rhs->value);
+    if (lhs->Type() == kConstant && rhs->Type() == kConstant) {
+      return MakeExpressionPtr<ConstExpression>(lhs->value / rhs->value);
     }
 
     // Evaluate expression type
-    ExpressionType type;
-    if (rhs->type == kConstant) {
-      type = lhs->type;
+    if (rhs->Type() == kConstant) {
+      if (lhs->Type() == kLinear) {
+        return MakeExpressionPtr<DivExpression<kLinear>>(lhs, rhs);
+      } else if (lhs->Type() == kQuadratic) {
+        return MakeExpressionPtr<DivExpression<kQuadratic>>(lhs, rhs);
+      } else {
+        return MakeExpressionPtr<DivExpression<kNonlinear>>(lhs, rhs);
+      }
     } else {
-      type = kNonlinear;
+      return MakeExpressionPtr<DivExpression<kNonlinear>>(lhs, rhs);
     }
-
-    return MakeExpressionPtr(
-        type, [](double lhs, double rhs) { return lhs / rhs; },
-        [](double, double rhs, double parentAdjoint) {
-          return parentAdjoint / rhs;
-        },
-        [](double lhs, double rhs, double parentAdjoint) {
-          return parentAdjoint * -lhs / (rhs * rhs);
-        },
-        [](const ExpressionPtr&, const ExpressionPtr& rhs,
-           const ExpressionPtr& parentAdjoint) { return parentAdjoint / rhs; },
-        [](const ExpressionPtr& lhs, const ExpressionPtr& rhs,
-           const ExpressionPtr& parentAdjoint) {
-          return parentAdjoint * -lhs / (rhs * rhs);
-        },
-        lhs, rhs);
   }
 
   /**
@@ -305,8 +232,8 @@ struct SLEIPNIR_DLLEXPORT Expression {
    * @param lhs Operator left-hand side.
    * @param rhs Operator right-hand side.
    */
-  friend SLEIPNIR_DLLEXPORT ExpressionPtr operator+(const ExpressionPtr& lhs,
-                                                    const ExpressionPtr& rhs) {
+  friend ExpressionPtr operator+(const ExpressionPtr& lhs,
+                                 const ExpressionPtr& rhs) {
     using enum ExpressionType;
 
     // Prune expression
@@ -317,20 +244,29 @@ struct SLEIPNIR_DLLEXPORT Expression {
     }
 
     // Evaluate constant
-    if (lhs->type == kConstant && rhs->type == kConstant) {
-      return MakeExpressionPtr(lhs->value + rhs->value);
+    if (lhs->Type() == kConstant && rhs->Type() == kConstant) {
+      return MakeExpressionPtr<ConstExpression>(lhs->value + rhs->value);
+    }
+
+    auto type = std::max(lhs->Type(), rhs->Type());
+    if (type == kLinear) {
+      return MakeExpressionPtr<BinaryPlusExpression<kLinear>>(lhs, rhs);
+    } else if (type == kQuadratic) {
+      return MakeExpressionPtr<BinaryPlusExpression<kQuadratic>>(lhs, rhs);
+    } else {
+      return MakeExpressionPtr<BinaryPlusExpression<kNonlinear>>(lhs, rhs);
     }
+  }
 
-    return MakeExpressionPtr(
-        std::max(lhs->type, rhs->type),
-        [](double lhs, double rhs) { return lhs + rhs; },
-        [](double, double, double parentAdjoint) { return parentAdjoint; },
-        [](double, double, double parentAdjoint) { return parentAdjoint; },
-        [](const ExpressionPtr&, const ExpressionPtr&,
-           const ExpressionPtr& parentAdjoint) { return parentAdjoint; },
-        [](const ExpressionPtr&, const ExpressionPtr&,
-           const ExpressionPtr& parentAdjoint) { return parentAdjoint; },
-        lhs, rhs);
+  /**
+   * Expression-Expression compound addition operator.
+   *
+   * @param lhs Operator left-hand side.
+   * @param rhs Operator right-hand side.
+   */
+  friend ExpressionPtr operator+=(ExpressionPtr& lhs,
+                                  const ExpressionPtr& rhs) {
+    return lhs = lhs + rhs;
   }
 
   /**
@@ -339,8 +275,8 @@ struct SLEIPNIR_DLLEXPORT Expression {
    * @param lhs Operator left-hand side.
    * @param rhs Operator right-hand side.
    */
-  friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs,
-                                                    const ExpressionPtr& rhs) {
+  friend ExpressionPtr operator-(const ExpressionPtr& lhs,
+                                 const ExpressionPtr& rhs) {
     using enum ExpressionType;
 
     // Prune expression
@@ -356,20 +292,18 @@ struct SLEIPNIR_DLLEXPORT Expression {
     }
 
     // Evaluate constant
-    if (lhs->type == kConstant && rhs->type == kConstant) {
-      return MakeExpressionPtr(lhs->value - rhs->value);
+    if (lhs->Type() == kConstant && rhs->Type() == kConstant) {
+      return MakeExpressionPtr<ConstExpression>(lhs->value - rhs->value);
     }
 
-    return MakeExpressionPtr(
-        std::max(lhs->type, rhs->type),
-        [](double lhs, double rhs) { return lhs - rhs; },
-        [](double, double, double parentAdjoint) { return parentAdjoint; },
-        [](double, double, double parentAdjoint) { return -parentAdjoint; },
-        [](const ExpressionPtr&, const ExpressionPtr&,
-           const ExpressionPtr& parentAdjoint) { return parentAdjoint; },
-        [](const ExpressionPtr&, const ExpressionPtr&,
-           const ExpressionPtr& parentAdjoint) { return -parentAdjoint; },
-        lhs, rhs);
+    auto type = std::max(lhs->Type(), rhs->Type());
+    if (type == kLinear) {
+      return MakeExpressionPtr<BinaryMinusExpression<kLinear>>(lhs, rhs);
+    } else if (type == kQuadratic) {
+      return MakeExpressionPtr<BinaryMinusExpression<kQuadratic>>(lhs, rhs);
+    } else {
+      return MakeExpressionPtr<BinaryMinusExpression<kNonlinear>>(lhs, rhs);
+    }
   }
 
   /**
@@ -377,7 +311,7 @@ struct SLEIPNIR_DLLEXPORT Expression {
    *
    * @param lhs Operand of unary minus.
    */
-  friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs) {
+  friend ExpressionPtr operator-(const ExpressionPtr& lhs) {
     using enum ExpressionType;
 
     // Prune expression
@@ -387,16 +321,17 @@ struct SLEIPNIR_DLLEXPORT Expression {
     }
 
     // Evaluate constant
-    if (lhs->type == kConstant) {
-      return MakeExpressionPtr(-lhs->value);
+    if (lhs->Type() == kConstant) {
+      return MakeExpressionPtr<ConstExpression>(-lhs->value);
     }
 
-    return MakeExpressionPtr(
-        lhs->type, [](double lhs, double) { return -lhs; },
-        [](double, double, double parentAdjoint) { return -parentAdjoint; },
-        [](const ExpressionPtr&, const ExpressionPtr&,
-           const ExpressionPtr& parentAdjoint) { return -parentAdjoint; },
-        lhs);
+    if (lhs->Type() == kLinear) {
+      return MakeExpressionPtr<UnaryMinusExpression<kLinear>>(lhs);
+    } else if (lhs->Type() == kQuadratic) {
+      return MakeExpressionPtr<UnaryMinusExpression<kQuadratic>>(lhs);
+    } else {
+      return MakeExpressionPtr<UnaryMinusExpression<kNonlinear>>(lhs);
+    }
   }
 
   /**
@@ -404,15 +339,293 @@ struct SLEIPNIR_DLLEXPORT Expression {
    *
    * @param lhs Operand of unary plus.
    */
-  friend SLEIPNIR_DLLEXPORT ExpressionPtr operator+(const ExpressionPtr& lhs) {
-    return lhs;
+  friend ExpressionPtr operator+(const ExpressionPtr& lhs) { return lhs; }
+
+  /**
+   * Either nullary operator with no arguments, unary operator with one
+   * argument, or binary operator with two arguments. This operator is used to
+   * update the node's value.
+   *
+   * @param lhs Left argument to binary operator.
+   * @param rhs Right argument to binary operator.
+   */
+  virtual double Value([[maybe_unused]] double lhs,
+                       [[maybe_unused]] double rhs) const = 0;
+
+  /**
+   * Returns the type of this expression (constant, linear, quadratic, or
+   * nonlinear).
+   */
+  virtual ExpressionType Type() const = 0;
+
+  /**
+   * Returns double adjoint of the left child expression.
+   *
+   * @param lhs Left argument to binary operator.
+   * @param rhs Right argument to binary operator.
+   * @param parentAdjoint Adjoint of parent expression.
+   */
+  virtual double GradL([[maybe_unused]] double lhs, [[maybe_unused]] double rhs,
+                       [[maybe_unused]] double parentAdjoint) const {
+    return 0.0;
+  }
+
+  /**
+   * Returns double adjoint of the right child expression.
+   *
+   * @param lhs Left argument to binary operator.
+   * @param rhs Right argument to binary operator.
+   * @param parentAdjoint Adjoint of parent expression.
+   */
+  virtual double GradR([[maybe_unused]] double lhs, [[maybe_unused]] double rhs,
+                       [[maybe_unused]] double parentAdjoint) const {
+    return 0.0;
+  }
+
+  /**
+   * Returns Expression adjoint of the left child expression.
+   *
+   * @param lhs Left argument to binary operator.
+   * @param rhs Right argument to binary operator.
+   * @param parentAdjoint Adjoint of parent expression.
+   */
+  virtual ExpressionPtr GradExprL(
+      [[maybe_unused]] const ExpressionPtr& lhs,
+      [[maybe_unused]] const ExpressionPtr& rhs,
+      [[maybe_unused]] const ExpressionPtr& parentAdjoint) const {
+    return MakeExpressionPtr<ConstExpression>();
+  }
+
+  /**
+   * Returns Expression adjoint of the right child expression.
+   *
+   * @param lhs Left argument to binary operator.
+   * @param rhs Right argument to binary operator.
+   * @param parentAdjoint Adjoint of parent expression.
+   */
+  virtual ExpressionPtr GradExprR(
+      [[maybe_unused]] const ExpressionPtr& lhs,
+      [[maybe_unused]] const ExpressionPtr& rhs,
+      [[maybe_unused]] const ExpressionPtr& parentAdjoint) const {
+    return MakeExpressionPtr<ConstExpression>();
+  }
+};
+
+template <ExpressionType T>
+struct BinaryMinusExpression final : Expression {
+  /**
+   * Constructs a binary expression (an operator with two arguments).
+   *
+   * @param lhs Binary operator's left operand.
+   * @param rhs Binary operator's right operand.
+   */
+  constexpr BinaryMinusExpression(ExpressionPtr lhs, ExpressionPtr rhs)
+      : Expression{std::move(lhs), std::move(rhs)} {
+    value = args[0]->value - args[1]->value;
+  }
+
+  double Value(double lhs, double rhs) const override { return lhs - rhs; }
+
+  ExpressionType Type() const override { return T; }
+
+  double GradL(double, double, double parentAdjoint) const override {
+    return parentAdjoint;
+  }
+
+  double GradR(double, double, double parentAdjoint) const override {
+    return -parentAdjoint;
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr&, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint;
+  }
+
+  ExpressionPtr GradExprR(const ExpressionPtr&, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return -parentAdjoint;
+  }
+};
+
+template <ExpressionType T>
+struct BinaryPlusExpression final : Expression {
+  /**
+   * Constructs a binary expression (an operator with two arguments).
+   *
+   * @param lhs Binary operator's left operand.
+   * @param rhs Binary operator's right operand.
+   */
+  constexpr BinaryPlusExpression(ExpressionPtr lhs, ExpressionPtr rhs)
+      : Expression{std::move(lhs), std::move(rhs)} {
+    value = args[0]->value + args[1]->value;
+  }
+
+  double Value(double lhs, double rhs) const override { return lhs + rhs; }
+
+  ExpressionType Type() const override { return T; }
+
+  double GradL(double, double, double parentAdjoint) const override {
+    return parentAdjoint;
+  }
+
+  double GradR(double, double, double parentAdjoint) const override {
+    return parentAdjoint;
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr&, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint;
+  }
+
+  ExpressionPtr GradExprR(const ExpressionPtr&, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint;
+  }
+};
+
+struct ConstExpression final : Expression {
+  /**
+   * Constructs a constant expression with a value of zero.
+   */
+  constexpr ConstExpression() = default;
+
+  /**
+   * Constructs a nullary expression (an operator with no arguments).
+   *
+   * @param value The expression value.
+   */
+  explicit constexpr ConstExpression(double value) : Expression{value} {}
+
+  double Value(double, double) const override { return value; }
+
+  ExpressionType Type() const override { return ExpressionType::kConstant; }
+};
+
+struct DecisionVariableExpression final : Expression {
+  /**
+   * Constructs a decision variable expression with a value of zero.
+   */
+  constexpr DecisionVariableExpression() = default;
+
+  /**
+   * Constructs a nullary expression (an operator with no arguments).
+   *
+   * @param value The expression value.
+   */
+  explicit constexpr DecisionVariableExpression(double value)
+      : Expression{value} {}
+
+  double Value(double, double) const override { return value; }
+
+  ExpressionType Type() const override { return ExpressionType::kLinear; }
+};
+
+template <ExpressionType T>
+struct DivExpression final : Expression {
+  /**
+   * Constructs a binary expression (an operator with two arguments).
+   *
+   * @param lhs Binary operator's left operand.
+   * @param rhs Binary operator's right operand.
+   */
+  constexpr DivExpression(ExpressionPtr lhs, ExpressionPtr rhs)
+      : Expression{std::move(lhs), std::move(rhs)} {
+    value = args[0]->value / args[1]->value;
+  }
+
+  double Value(double lhs, double rhs) const override { return lhs / rhs; }
+
+  ExpressionType Type() const override { return T; }
+
+  double GradL(double, double rhs, double parentAdjoint) const override {
+    return parentAdjoint / rhs;
+  };
+
+  double GradR(double lhs, double rhs, double parentAdjoint) const override {
+    return parentAdjoint * -lhs / (rhs * rhs);
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr&, const ExpressionPtr& rhs,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint / rhs;
+  }
+
+  ExpressionPtr GradExprR(const ExpressionPtr& lhs, const ExpressionPtr& rhs,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint * -lhs / (rhs * rhs);
+  }
+};
+
+template <ExpressionType T>
+struct MultExpression final : Expression {
+  /**
+   * Constructs a binary expression (an operator with two arguments).
+   *
+   * @param lhs Binary operator's left operand.
+   * @param rhs Binary operator's right operand.
+   */
+  constexpr MultExpression(ExpressionPtr lhs, ExpressionPtr rhs)
+      : Expression{std::move(lhs), std::move(rhs)} {
+    value = args[0]->value * args[1]->value;
+  }
+
+  double Value(double lhs, double rhs) const override { return lhs * rhs; }
+
+  ExpressionType Type() const override { return T; }
+
+  double GradL([[maybe_unused]] double lhs, double rhs,
+               double parentAdjoint) const override {
+    return parentAdjoint * rhs;
+  }
+
+  double GradR(double lhs, [[maybe_unused]] double rhs,
+               double parentAdjoint) const override {
+    return parentAdjoint * lhs;
+  }
+
+  ExpressionPtr GradExprL([[maybe_unused]] const ExpressionPtr& lhs,
+                          const ExpressionPtr& rhs,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint * rhs;
+  }
+
+  ExpressionPtr GradExprR(const ExpressionPtr& lhs,
+                          [[maybe_unused]] const ExpressionPtr& rhs,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint * lhs;
+  }
+};
+
+template <ExpressionType T>
+struct UnaryMinusExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit constexpr UnaryMinusExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {
+    value = -args[0]->value;
+  }
+
+  double Value(double lhs, double) const override { return -lhs; }
+
+  ExpressionType Type() const override { return T; }
+
+  double GradL(double, double, double parentAdjoint) const override {
+    return -parentAdjoint;
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr&, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return -parentAdjoint;
   }
 };
 
-SLEIPNIR_DLLEXPORT inline ExpressionPtr exp(const ExpressionPtr& x);
-SLEIPNIR_DLLEXPORT inline ExpressionPtr sin(const ExpressionPtr& x);
-SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x);
-SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x);
+inline ExpressionPtr exp(const ExpressionPtr& x);
+inline ExpressionPtr sin(const ExpressionPtr& x);
+inline ExpressionPtr sinh(const ExpressionPtr& x);
+inline ExpressionPtr sqrt(const ExpressionPtr& x);
 
 /**
  * Refcount increment for intrusive shared pointer.
@@ -446,7 +659,7 @@ inline void IntrusiveSharedPtrDecRefCount(Expression* expr) {
       if (elem->adjointExpr != nullptr) {
         stack.emplace_back(elem->adjointExpr.Get());
       }
-      for (auto&& arg : elem->args) {
+      for (auto& arg : elem->args) {
         if (arg != nullptr) {
           stack.emplace_back(arg.Get());
         }
@@ -463,13 +676,50 @@ inline void IntrusiveSharedPtrDecRefCount(Expression* expr) {
   }
 }
 
+struct AbsExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit constexpr AbsExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {
+    value = args[0]->value < 0 ? -args[0]->value : args[0]->value;
+  }
+
+  double Value(double x, double) const override { return std::abs(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    if (x < 0.0) {
+      return -parentAdjoint;
+    } else if (x > 0.0) {
+      return parentAdjoint;
+    } else {
+      return 0.0;
+    }
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    if (x->value < 0.0) {
+      return -parentAdjoint;
+    } else if (x->value > 0.0) {
+      return parentAdjoint;
+    } else {
+      // Return zero
+      return MakeExpressionPtr<ConstExpression>();
+    }
+  }
+};
+
 /**
  * std::abs() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr abs(  // NOLINT
-    const ExpressionPtr& x) {
+inline ExpressionPtr abs(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
@@ -479,74 +729,91 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs(  // NOLINT
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::abs(x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::abs(x); },
-      [](double x, double, double parentAdjoint) {
-        if (x < 0.0) {
-          return -parentAdjoint;
-        } else if (x > 0.0) {
-          return parentAdjoint;
-        } else {
-          return 0.0;
-        }
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        if (x->value < 0.0) {
-          return -parentAdjoint;
-        } else if (x->value > 0.0) {
-          return parentAdjoint;
-        } else {
-          // Return zero
-          return MakeExpressionPtr();
-        }
-      },
-      x);
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::abs(x->value));
+  }
+
+  return MakeExpressionPtr<AbsExpression>(x);
 }
 
+struct AcosExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit AcosExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::acos(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::acos(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return -parentAdjoint / std::sqrt(1.0 - x * x);
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return -parentAdjoint /
+           sleipnir::detail::sqrt(MakeExpressionPtr<ConstExpression>(1.0) -
+                                  x * x);
+  }
+};
+
 /**
  * std::acos() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr acos(  // NOLINT
-    const ExpressionPtr& x) {
+inline ExpressionPtr acos(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
   if (x->IsConstant(0.0)) {
-    return MakeExpressionPtr(std::numbers::pi / 2.0);
+    return MakeExpressionPtr<ConstExpression>(std::numbers::pi / 2.0);
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::acos(x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::acos(x); },
-      [](double x, double, double parentAdjoint) {
-        return -parentAdjoint / std::sqrt(1.0 - x * x);
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        return -parentAdjoint /
-               sleipnir::detail::sqrt(MakeExpressionPtr(1.0) - x * x);
-      },
-      x);
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::acos(x->value));
+  }
+
+  return MakeExpressionPtr<AcosExpression>(x);
 }
 
+struct AsinExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit AsinExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::asin(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::asin(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return parentAdjoint / std::sqrt(1.0 - x * x);
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint / sleipnir::detail::sqrt(
+                               MakeExpressionPtr<ConstExpression>(1.0) - x * x);
+  }
+};
+
 /**
  * std::asin() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr asin(  // NOLINT
-    const ExpressionPtr& x) {
+inline ExpressionPtr asin(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
@@ -556,30 +823,43 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin(  // NOLINT
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::asin(x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::asin(x); },
-      [](double x, double, double parentAdjoint) {
-        return parentAdjoint / std::sqrt(1.0 - x * x);
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint /
-               sleipnir::detail::sqrt(MakeExpressionPtr(1.0) - x * x);
-      },
-      x);
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::asin(x->value));
+  }
+
+  return MakeExpressionPtr<AsinExpression>(x);
 }
 
+struct AtanExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit AtanExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::atan(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::atan(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return parentAdjoint / (1.0 + x * x);
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint / (MakeExpressionPtr<ConstExpression>(1.0) + x * x);
+  }
+};
+
 /**
  * std::atan() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr atan(  // NOLINT
-    const ExpressionPtr& x) {
+inline ExpressionPtr atan(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
@@ -589,30 +869,55 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan(  // NOLINT
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::atan(x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::atan(x); },
-      [](double x, double, double parentAdjoint) {
-        return parentAdjoint / (1.0 + x * x);
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint / (MakeExpressionPtr(1.0) + x * x);
-      },
-      x);
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::atan(x->value));
+  }
+
+  return MakeExpressionPtr<AtanExpression>(x);
 }
 
-/**
- * std::atan2() for Expressions.
- *
- * @param y The y argument.
- * @param x The x argument.
+struct Atan2Expression final : Expression {
+  /**
+   * Constructs a binary expression (an operator with two arguments).
+   *
+   * @param lhs Binary operator's left operand.
+   * @param rhs Binary operator's right operand.
+   */
+  Atan2Expression(ExpressionPtr lhs, ExpressionPtr rhs)
+      : Expression{std::move(lhs), std::move(rhs)} {
+    value = std::atan2(args[0]->value, args[1]->value);
+  }
+
+  double Value(double y, double x) const override { return std::atan2(y, x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double y, double x, double parentAdjoint) const override {
+    return parentAdjoint * x / (y * y + x * x);
+  }
+
+  double GradR(double y, double x, double parentAdjoint) const override {
+    return parentAdjoint * -y / (y * y + x * x);
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& y, const ExpressionPtr& x,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint * x / (y * y + x * x);
+  }
+
+  ExpressionPtr GradExprR(const ExpressionPtr& y, const ExpressionPtr& x,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint * -y / (y * y + x * x);
+  }
+};
+
+/**
+ * std::atan2() for Expressions.
+ *
+ * @param y The y argument.
+ * @param x The x argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2(  // NOLINT
-    const ExpressionPtr& y, const ExpressionPtr& x) {
+inline ExpressionPtr atan2(const ExpressionPtr& y, const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
@@ -620,102 +925,139 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2(  // NOLINT
     // Return zero
     return y;
   } else if (x->IsConstant(0.0)) {
-    return MakeExpressionPtr(std::numbers::pi / 2.0);
+    return MakeExpressionPtr<ConstExpression>(std::numbers::pi / 2.0);
   }
 
   // Evaluate constant
-  if (y->type == kConstant && x->type == kConstant) {
-    return MakeExpressionPtr(std::atan2(y->value, x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double y, double x) { return std::atan2(y, x); },
-      [](double y, double x, double parentAdjoint) {
-        return parentAdjoint * x / (y * y + x * x);
-      },
-      [](double y, double x, double parentAdjoint) {
-        return parentAdjoint * -y / (y * y + x * x);
-      },
-      [](const ExpressionPtr& y, const ExpressionPtr& x,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint * x / (y * y + x * x);
-      },
-      [](const ExpressionPtr& y, const ExpressionPtr& x,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint * -y / (y * y + x * x);
-      },
-      y, x);
+  if (y->Type() == kConstant && x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::atan2(y->value, x->value));
+  }
+
+  return MakeExpressionPtr<Atan2Expression>(y, x);
 }
 
+struct CosExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit CosExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::cos(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::cos(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return -parentAdjoint * std::sin(x);
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint * -sleipnir::detail::sin(x);
+  }
+};
+
 /**
  * std::cos() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr cos(  // NOLINT
-    const ExpressionPtr& x) {
+inline ExpressionPtr cos(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
   if (x->IsConstant(0.0)) {
-    return MakeExpressionPtr(1.0);
+    return MakeExpressionPtr<ConstExpression>(1.0);
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::cos(x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::cos(x); },
-      [](double x, double, double parentAdjoint) {
-        return -parentAdjoint * std::sin(x);
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint * -sleipnir::detail::sin(x);
-      },
-      x);
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::cos(x->value));
+  }
+
+  return MakeExpressionPtr<CosExpression>(x);
 }
 
+struct CoshExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit CoshExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::cosh(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::cosh(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return parentAdjoint * std::sinh(x);
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint * sleipnir::detail::sinh(x);
+  }
+};
+
 /**
  * std::cosh() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh(  // NOLINT
-    const ExpressionPtr& x) {
+inline ExpressionPtr cosh(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
   if (x->IsConstant(0.0)) {
-    return MakeExpressionPtr(1.0);
+    return MakeExpressionPtr<ConstExpression>(1.0);
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::cosh(x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::cosh(x); },
-      [](double x, double, double parentAdjoint) {
-        return parentAdjoint * std::sinh(x);
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint * sleipnir::detail::sinh(x);
-      },
-      x);
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::cosh(x->value));
+  }
+
+  return MakeExpressionPtr<CoshExpression>(x);
 }
 
+struct ErfExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit ErfExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::erf(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::erf(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return parentAdjoint * 2.0 * std::numbers::inv_sqrtpi * std::exp(-x * x);
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint *
+           MakeExpressionPtr<ConstExpression>(2.0 * std::numbers::inv_sqrtpi) *
+           sleipnir::detail::exp(-x * x);
+  }
+};
+
 /**
  * std::erf() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr erf(  // NOLINT
-    const ExpressionPtr& x) {
+inline ExpressionPtr erf(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
@@ -725,64 +1067,102 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf(  // NOLINT
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::erf(x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::erf(x); },
-      [](double x, double, double parentAdjoint) {
-        return parentAdjoint * 2.0 * std::numbers::inv_sqrtpi *
-               std::exp(-x * x);
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint *
-               MakeExpressionPtr(2.0 * std::numbers::inv_sqrtpi) *
-               sleipnir::detail::exp(-x * x);
-      },
-      x);
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::erf(x->value));
+  }
+
+  return MakeExpressionPtr<ErfExpression>(x);
 }
 
+struct ExpExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit ExpExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::exp(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::exp(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return parentAdjoint * std::exp(x);
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint * sleipnir::detail::exp(x);
+  }
+};
+
 /**
  * std::exp() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr exp(  // NOLINT
-    const ExpressionPtr& x) {
+inline ExpressionPtr exp(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
   if (x->IsConstant(0.0)) {
-    return MakeExpressionPtr(1.0);
+    return MakeExpressionPtr<ConstExpression>(1.0);
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::exp(x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::exp(x); },
-      [](double x, double, double parentAdjoint) {
-        return parentAdjoint * std::exp(x);
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint * sleipnir::detail::exp(x);
-      },
-      x);
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::exp(x->value));
+  }
+
+  return MakeExpressionPtr<ExpExpression>(x);
 }
 
+inline ExpressionPtr hypot(const ExpressionPtr& x, const ExpressionPtr& y);
+
+struct HypotExpression final : Expression {
+  /**
+   * Constructs a binary expression (an operator with two arguments).
+   *
+   * @param lhs Binary operator's left operand.
+   * @param rhs Binary operator's right operand.
+   */
+  HypotExpression(ExpressionPtr lhs, ExpressionPtr rhs)
+      : Expression{std::move(lhs), std::move(rhs)} {
+    value = std::hypot(args[0]->value, args[1]->value);
+  }
+
+  double Value(double x, double y) const override { return std::hypot(x, y); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double y, double parentAdjoint) const override {
+    return parentAdjoint * x / std::hypot(x, y);
+  }
+
+  double GradR(double x, double y, double parentAdjoint) const override {
+    return parentAdjoint * y / std::hypot(x, y);
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr& y,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint * x / sleipnir::detail::hypot(x, y);
+  }
+
+  ExpressionPtr GradExprR(const ExpressionPtr& x, const ExpressionPtr& y,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint * y / sleipnir::detail::hypot(x, y);
+  }
+};
+
 /**
  * std::hypot() for Expressions.
  *
  * @param x The x argument.
  * @param y The y argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot(  // NOLINT
-    const ExpressionPtr& x, const ExpressionPtr& y) {
+inline ExpressionPtr hypot(const ExpressionPtr& x, const ExpressionPtr& y) {
   using enum ExpressionType;
 
   // Prune expression
@@ -793,36 +1173,43 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot(  // NOLINT
   }
 
   // Evaluate constant
-  if (x->type == kConstant && y->type == kConstant) {
-    return MakeExpressionPtr(std::hypot(x->value, y->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double y) { return std::hypot(x, y); },
-      [](double x, double y, double parentAdjoint) {
-        return parentAdjoint * x / std::hypot(x, y);
-      },
-      [](double x, double y, double parentAdjoint) {
-        return parentAdjoint * y / std::hypot(x, y);
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr& y,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint * x / sleipnir::detail::hypot(x, y);
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr& y,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint * y / sleipnir::detail::hypot(x, y);
-      },
-      x, y);
+  if (x->Type() == kConstant && y->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::hypot(x->value, y->value));
+  }
+
+  return MakeExpressionPtr<HypotExpression>(x, y);
 }
 
+struct LogExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit LogExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::log(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::log(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return parentAdjoint / x;
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint / x;
+  }
+};
+
 /**
  * std::log() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr log(  // NOLINT
-    const ExpressionPtr& x) {
+inline ExpressionPtr log(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
@@ -832,25 +1219,44 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log(  // NOLINT
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::log(x->value));
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::log(x->value));
   }
 
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::log(x); },
-      [](double x, double, double parentAdjoint) { return parentAdjoint / x; },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) { return parentAdjoint / x; },
-      x);
+  return MakeExpressionPtr<LogExpression>(x);
 }
 
+struct Log10Expression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit Log10Expression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::log10(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::log10(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return parentAdjoint / (std::numbers::ln10 * x);
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint /
+           (MakeExpressionPtr<ConstExpression>(std::numbers::ln10) * x);
+  }
+};
+
 /**
  * std::log10() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr log10(  // NOLINT
-    const ExpressionPtr& x) {
+inline ExpressionPtr log10(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
@@ -860,30 +1266,78 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10(  // NOLINT
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::log10(x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::log10(x); },
-      [](double x, double, double parentAdjoint) {
-        return parentAdjoint / (std::numbers::ln10 * x);
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint / (MakeExpressionPtr(std::numbers::ln10) * x);
-      },
-      x);
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::log10(x->value));
+  }
+
+  return MakeExpressionPtr<Log10Expression>(x);
 }
 
+inline ExpressionPtr pow(const ExpressionPtr& base, const ExpressionPtr& power);
+
+template <ExpressionType T>
+struct PowExpression final : Expression {
+  /**
+   * Constructs a binary expression (an operator with two arguments).
+   *
+   * @param lhs Binary operator's left operand.
+   * @param rhs Binary operator's right operand.
+   */
+  PowExpression(ExpressionPtr lhs, ExpressionPtr rhs)
+      : Expression{std::move(lhs), std::move(rhs)} {
+    value = std::pow(args[0]->value, args[1]->value);
+  }
+
+  double Value(double base, double power) const override {
+    return std::pow(base, power);
+  }
+
+  ExpressionType Type() const override { return T; }
+
+  double GradL(double base, double power, double parentAdjoint) const override {
+    return parentAdjoint * std::pow(base, power - 1) * power;
+  }
+
+  double GradR(double base, double power, double parentAdjoint) const override {
+    // Since x * std::log(x) -> 0 as x -> 0
+    if (base == 0.0) {
+      return 0.0;
+    } else {
+      return parentAdjoint * std::pow(base, power - 1) * base * std::log(base);
+    }
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& base, const ExpressionPtr& power,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint *
+           sleipnir::detail::pow(
+               base, power - MakeExpressionPtr<ConstExpression>(1.0)) *
+           power;
+  }
+
+  ExpressionPtr GradExprR(const ExpressionPtr& base, const ExpressionPtr& power,
+                          const ExpressionPtr& parentAdjoint) const override {
+    // Since x * std::log(x) -> 0 as x -> 0
+    if (base->value == 0.0) {
+      // Return zero
+      return base;
+    } else {
+      return parentAdjoint *
+             sleipnir::detail::pow(
+                 base, power - MakeExpressionPtr<ConstExpression>(1.0)) *
+             base * sleipnir::detail::log(base);
+    }
+  }
+};
+
 /**
  * std::pow() for Expressions.
  *
  * @param base The base.
  * @param power The power.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr pow(  // NOLINT
-    const ExpressionPtr& base, const ExpressionPtr& power) {
+inline ExpressionPtr pow(const ExpressionPtr& base,
+                         const ExpressionPtr& power) {
   using enum ExpressionType;
 
   // Prune expression
@@ -891,101 +1345,123 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow(  // NOLINT
     // Return zero
     return base;
   } else if (base->IsConstant(1.0)) {
+    // Return one
     return base;
   }
   if (power->IsConstant(0.0)) {
-    return MakeExpressionPtr(1.0);
+    return MakeExpressionPtr<ConstExpression>(1.0);
   } else if (power->IsConstant(1.0)) {
     return base;
   }
 
   // Evaluate constant
-  if (base->type == kConstant && power->type == kConstant) {
-    return MakeExpressionPtr(std::pow(base->value, power->value));
-  }
-
-  return MakeExpressionPtr(
-      base->type == kLinear && power->IsConstant(2.0) ? kQuadratic : kNonlinear,
-      [](double base, double power) { return std::pow(base, power); },
-      [](double base, double power, double parentAdjoint) {
-        return parentAdjoint * std::pow(base, power - 1) * power;
-      },
-      [](double base, double power, double parentAdjoint) {
-        // Since x * std::log(x) -> 0 as x -> 0
-        if (base == 0.0) {
-          return 0.0;
-        } else {
-          return parentAdjoint * std::pow(base, power - 1) * base *
-                 std::log(base);
-        }
-      },
-      [](const ExpressionPtr& base, const ExpressionPtr& power,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint *
-               sleipnir::detail::pow(base, power - MakeExpressionPtr(1.0)) *
-               power;
-      },
-      [](const ExpressionPtr& base, const ExpressionPtr& power,
-         const ExpressionPtr& parentAdjoint) {
-        // Since x * std::log(x) -> 0 as x -> 0
-        if (base->value == 0.0) {
-          // Return zero
-          return base;
-        } else {
-          return parentAdjoint *
-                 sleipnir::detail::pow(base, power - MakeExpressionPtr(1.0)) *
-                 base * sleipnir::detail::log(base);
-        }
-      },
-      base, power);
+  if (base->Type() == kConstant && power->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(
+        std::pow(base->value, power->value));
+  }
+
+  if (power->IsConstant(2.0)) {
+    if (base->Type() == kLinear) {
+      return MakeExpressionPtr<MultExpression<kQuadratic>>(base, base);
+    } else {
+      return MakeExpressionPtr<MultExpression<kNonlinear>>(base, base);
+    }
+  }
+
+  return MakeExpressionPtr<PowExpression<kNonlinear>>(base, power);
 }
 
+struct SignExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit constexpr SignExpression(ExpressionPtr lhs)
+      : Expression{std::move(lhs)} {
+    if (args[0]->value < 0.0) {
+      value = -1.0;
+    } else if (args[0]->value == 0.0) {
+      value = 0.0;
+    } else {
+      value = 1.0;
+    }
+  }
+
+  double Value(double x, double) const override {
+    if (x < 0.0) {
+      return -1.0;
+    } else if (x == 0.0) {
+      return 0.0;
+    } else {
+      return 1.0;
+    }
+  }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double, double, double) const override { return 0.0; }
+
+  ExpressionPtr GradExprL(const ExpressionPtr&, const ExpressionPtr&,
+                          const ExpressionPtr&) const override {
+    // Return zero
+    return MakeExpressionPtr<ConstExpression>();
+  }
+};
+
 /**
  * sign() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) {
+inline ExpressionPtr sign(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Evaluate constant
-  if (x->type == kConstant) {
+  if (x->Type() == kConstant) {
     if (x->value < 0.0) {
-      return MakeExpressionPtr(-1.0);
+      return MakeExpressionPtr<ConstExpression>(-1.0);
     } else if (x->value == 0.0) {
       // Return zero
       return x;
     } else {
-      return MakeExpressionPtr(1.0);
+      return MakeExpressionPtr<ConstExpression>(1.0);
     }
   }
 
-  return MakeExpressionPtr(
-      kNonlinear,
-      [](double x, double) {
-        if (x < 0.0) {
-          return -1.0;
-        } else if (x == 0.0) {
-          return 0.0;
-        } else {
-          return 1.0;
-        }
-      },
-      [](double, double, double) { return 0.0; },
-      [](const ExpressionPtr&, const ExpressionPtr&, const ExpressionPtr&) {
-        // Return zero
-        return MakeExpressionPtr();
-      },
-      x);
+  return MakeExpressionPtr<SignExpression>(x);
 }
 
+struct SinExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit SinExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::sin(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::sin(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return parentAdjoint * std::cos(x);
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint * sleipnir::detail::cos(x);
+  }
+};
+
 /**
  * std::sin() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr sin(  // NOLINT
-    const ExpressionPtr& x) {
+inline ExpressionPtr sin(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
@@ -995,28 +1471,43 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin(  // NOLINT
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::sin(x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::sin(x); },
-      [](double x, double, double parentAdjoint) {
-        return parentAdjoint * std::cos(x);
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint * sleipnir::detail::cos(x);
-      },
-      x);
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::sin(x->value));
+  }
+
+  return MakeExpressionPtr<SinExpression>(x);
 }
 
+struct SinhExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit SinhExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::sinh(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::sinh(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return parentAdjoint * std::cosh(x);
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint * sleipnir::detail::cosh(x);
+  }
+};
+
 /**
  * std::sinh() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) {
+inline ExpressionPtr sinh(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
@@ -1026,63 +1517,92 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) {
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::sinh(x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::sinh(x); },
-      [](double x, double, double parentAdjoint) {
-        return parentAdjoint * std::cosh(x);
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint * sleipnir::detail::cosh(x);
-      },
-      x);
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::sinh(x->value));
+  }
+
+  return MakeExpressionPtr<SinhExpression>(x);
 }
 
+struct SqrtExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit SqrtExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::sqrt(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::sqrt(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return parentAdjoint / (2.0 * std::sqrt(x));
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint / (MakeExpressionPtr<ConstExpression>(2.0) *
+                            sleipnir::detail::sqrt(x));
+  }
+};
+
 /**
  * std::sqrt() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(  // NOLINT
-    const ExpressionPtr& x) {
+inline ExpressionPtr sqrt(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Evaluate constant
-  if (x->type == kConstant) {
+  if (x->Type() == kConstant) {
     if (x->value == 0.0) {
       // Return zero
       return x;
     } else if (x->value == 1.0) {
       return x;
     } else {
-      return MakeExpressionPtr(std::sqrt(x->value));
+      return MakeExpressionPtr<ConstExpression>(std::sqrt(x->value));
     }
   }
 
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::sqrt(x); },
-      [](double x, double, double parentAdjoint) {
-        return parentAdjoint / (2.0 * std::sqrt(x));
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint /
-               (MakeExpressionPtr(2.0) * sleipnir::detail::sqrt(x));
-      },
-      x);
+  return MakeExpressionPtr<SqrtExpression>(x);
 }
 
+struct TanExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit TanExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::tan(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::tan(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return parentAdjoint / (std::cos(x) * std::cos(x));
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint /
+           (sleipnir::detail::cos(x) * sleipnir::detail::cos(x));
+  }
+};
+
 /**
  * std::tan() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr tan(  // NOLINT
-    const ExpressionPtr& x) {
+inline ExpressionPtr tan(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
@@ -1092,29 +1612,44 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan(  // NOLINT
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::tan(x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::tan(x); },
-      [](double x, double, double parentAdjoint) {
-        return parentAdjoint / (std::cos(x) * std::cos(x));
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint /
-               (sleipnir::detail::cos(x) * sleipnir::detail::cos(x));
-      },
-      x);
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::tan(x->value));
+  }
+
+  return MakeExpressionPtr<TanExpression>(x);
 }
 
+struct TanhExpression final : Expression {
+  /**
+   * Constructs an unary expression (an operator with one argument).
+   *
+   * @param lhs Unary operator's operand.
+   */
+  explicit TanhExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {
+    value = std::tanh(args[0]->value);
+  }
+
+  double Value(double x, double) const override { return std::tanh(x); }
+
+  ExpressionType Type() const override { return ExpressionType::kNonlinear; }
+
+  double GradL(double x, double, double parentAdjoint) const override {
+    return parentAdjoint / (std::cosh(x) * std::cosh(x));
+  }
+
+  ExpressionPtr GradExprL(const ExpressionPtr& x, const ExpressionPtr&,
+                          const ExpressionPtr& parentAdjoint) const override {
+    return parentAdjoint /
+           (sleipnir::detail::cosh(x) * sleipnir::detail::cosh(x));
+  }
+};
+
 /**
  * std::tanh() for Expressions.
  *
  * @param x The argument.
  */
-SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) {
+inline ExpressionPtr tanh(const ExpressionPtr& x) {
   using enum ExpressionType;
 
   // Prune expression
@@ -1124,21 +1659,11 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) {
   }
 
   // Evaluate constant
-  if (x->type == kConstant) {
-    return MakeExpressionPtr(std::tanh(x->value));
-  }
-
-  return MakeExpressionPtr(
-      kNonlinear, [](double x, double) { return std::tanh(x); },
-      [](double x, double, double parentAdjoint) {
-        return parentAdjoint / (std::cosh(x) * std::cosh(x));
-      },
-      [](const ExpressionPtr& x, const ExpressionPtr&,
-         const ExpressionPtr& parentAdjoint) {
-        return parentAdjoint /
-               (sleipnir::detail::cosh(x) * sleipnir::detail::cosh(x));
-      },
-      x);
+  if (x->Type() == kConstant) {
+    return MakeExpressionPtr<ConstExpression>(std::tanh(x->value));
+  }
+
+  return MakeExpressionPtr<TanhExpression>(x);
 }
 
 }  // namespace sleipnir::detail
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp
index 714bcbb8290..0a6037279cb 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp
@@ -2,243 +2,132 @@
 
 #pragma once
 
-#include <span>
+#include <ranges>
 
 #include <wpi/SmallVector.h>
 
 #include "sleipnir/autodiff/Expression.hpp"
-#include "sleipnir/util/FunctionRef.hpp"
-#include "sleipnir/util/SymbolExports.hpp"
 
 namespace sleipnir::detail {
 
 /**
- * This class is an adaptor type that performs value updates of an expression's
- * computational graph in a way that skips duplicates.
+ * Generate a topological sort of an expression graph from parent to child.
+ *
+ * https://en.wikipedia.org/wiki/Topological_sorting
+ *
+ * @param root The root node of the expression.
  */
-class SLEIPNIR_DLLEXPORT ExpressionGraph {
- public:
-  /**
-   * Generates the deduplicated computational graph for the given expression.
-   *
-   * @param root The root node of the expression.
-   */
-  explicit ExpressionGraph(ExpressionPtr& root) {
-    // If the root type is a constant, Update() is a no-op, so there's no work
-    // to do
-    if (root == nullptr || root->type == ExpressionType::kConstant) {
-      return;
-    }
-
-    // Breadth-first search (BFS) is used as opposed to a depth-first search
-    // (DFS) to avoid counting duplicate nodes multiple times. A list of nodes
-    // ordered from parent to child with no duplicates is generated.
-    //
-    // https://en.wikipedia.org/wiki/Breadth-first_search
-
-    // BFS list sorted from parent to child.
-    wpi::SmallVector<Expression*> stack;
-
-    stack.emplace_back(root.Get());
-
-    // Initialize the number of instances of each node in the tree
-    // (Expression::duplications)
-    while (!stack.empty()) {
-      auto currentNode = stack.back();
-      stack.pop_back();
-
-      for (auto&& arg : currentNode->args) {
-        // Only continue if the node is not a constant and hasn't already been
-        // explored.
-        if (arg != nullptr && arg->type != ExpressionType::kConstant) {
-          // If this is the first instance of the node encountered (it hasn't
-          // been explored yet), add it to stack so it's recursed upon
-          if (arg->duplications == 0) {
-            stack.push_back(arg.Get());
-          }
-          ++arg->duplications;
-        }
-      }
-    }
+inline wpi::SmallVector<Expression*> TopologicalSort(const ExpressionPtr& root) {
+  wpi::SmallVector<Expression*> list;
 
-    stack.emplace_back(root.Get());
+  // If the root type is a constant, Update() is a no-op, so there's no work
+  // to do
+  if (root == nullptr || root->Type() == ExpressionType::kConstant) {
+    return list;
+  }
 
-    while (!stack.empty()) {
-      auto currentNode = stack.back();
-      stack.pop_back();
+  // Stack of nodes to explore
+  wpi::SmallVector<Expression*> stack;
 
-      // BFS lists sorted from parent to child.
-      m_rowList.emplace_back(currentNode->row);
-      m_adjointList.emplace_back(currentNode);
-      if (currentNode->valueFunc != nullptr) {
-        // Constants are skipped because they have no valueFunc and don't need
-        // to be updated
-        m_valueList.emplace_back(currentNode);
-      }
+  // Enumerate incoming edges for each node via depth-first search
+  stack.emplace_back(root.Get());
+  while (!stack.empty()) {
+    auto node = stack.back();
+    stack.pop_back();
 
-      for (auto&& arg : currentNode->args) {
-        // Only add node if it's not a constant and doesn't already exist in the
-        // tape.
-        if (arg != nullptr && arg->type != ExpressionType::kConstant) {
-          // Once the number of node visitations equals the number of
-          // duplications (the counter hits zero), add it to the stack. Note
-          // that this means the node is only enqueued once.
-          --arg->duplications;
-          if (arg->duplications == 0) {
-            stack.push_back(arg.Get());
-          }
-        }
+    for (auto& arg : node->args) {
+      // If the node hasn't been explored yet, add it to the stack
+      if (arg != nullptr && ++arg->incomingEdges == 1) {
+        stack.push_back(arg.Get());
       }
     }
   }
 
-  /**
-   * Update the values of all nodes in this computational tree based on the
-   * values of their dependent nodes.
-   */
-  void Update() {
-    // Traverse the BFS list backward from child to parent and update the value
-    // of each node.
-    for (auto it = m_valueList.rbegin(); it != m_valueList.rend(); ++it) {
-      auto& node = *it;
-
-      auto& lhs = node->args[0];
-      auto& rhs = node->args[1];
-
-      if (lhs != nullptr) {
-        if (rhs != nullptr) {
-          node->value = node->valueFunc(lhs->value, rhs->value);
-        } else {
-          node->value = node->valueFunc(lhs->value, 0.0);
-        }
+  // Generate topological sort of graph from parent to child.
+  //
+  // A node is only added to the stack after all its incoming edges have been
+  // traversed. Expression::incomingEdges is a decrementing counter for
+  // tracking this.
+  //
+  // https://en.wikipedia.org/wiki/Topological_sorting
+  stack.emplace_back(root.Get());
+  while (!stack.empty()) {
+    auto node = stack.back();
+    stack.pop_back();
+
+    list.emplace_back(node);
+
+    for (auto& arg : node->args) {
+      // If we traversed all this node's incoming edges, add it to the stack
+      if (arg != nullptr && --arg->incomingEdges == 0) {
+        stack.push_back(arg.Get());
       }
     }
   }
 
-  /**
-   * Returns the variable's gradient tree.
-   *
-   * @param wrt Variables with respect to which to compute the gradient.
-   */
-  wpi::SmallVector<ExpressionPtr> GenerateGradientTree(
-      std::span<const ExpressionPtr> wrt) const {
-    // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
-    // for background on reverse accumulation automatic differentiation.
-
-    for (size_t row = 0; row < wrt.size(); ++row) {
-      wrt[row]->row = row;
-    }
-
-    wpi::SmallVector<ExpressionPtr> grad;
-    grad.reserve(wrt.size());
-    for (size_t row = 0; row < wrt.size(); ++row) {
-      grad.emplace_back(MakeExpressionPtr());
-    }
-
-    // Zero adjoints. The root node's adjoint is 1.0 as df/df is always 1.
-    if (m_adjointList.size() > 0) {
-      m_adjointList[0]->adjointExpr = MakeExpressionPtr(1.0);
-      for (auto it = m_adjointList.begin() + 1; it != m_adjointList.end();
-           ++it) {
-        auto& node = *it;
-        node->adjointExpr = MakeExpressionPtr();
-      }
-    }
-
-    // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y
-    // multiplied by dy/dx. If there are multiple "paths" from the root node to
-    // variable; the variable's adjoint is the sum of each path's adjoint
-    // contribution.
-    for (auto node : m_adjointList) {
-      auto& lhs = node->args[0];
-      auto& rhs = node->args[1];
-
-      if (lhs != nullptr && !lhs->IsConstant(0.0)) {
-        lhs->adjointExpr = lhs->adjointExpr +
-                           node->gradientFuncs[0](lhs, rhs, node->adjointExpr);
-      }
-      if (rhs != nullptr && !rhs->IsConstant(0.0)) {
-        rhs->adjointExpr = rhs->adjointExpr +
-                           node->gradientFuncs[1](lhs, rhs, node->adjointExpr);
-      }
-
-      // If variable is a leaf node, assign its adjoint to the gradient.
-      if (node->row != -1) {
-        grad[node->row] = node->adjointExpr;
-      }
-    }
+  return list;
+}
 
-    // Unlink adjoints to avoid circular references between them and their
-    // parent expressions. This ensures all expressions are returned to the free
-    // list.
-    for (auto node : m_adjointList) {
-      for (auto& arg : node->args) {
-        if (arg != nullptr) {
-          arg->adjointExpr = nullptr;
-        }
+/**
+ * Update the values of all nodes in this graph based on the values of
+ * their dependent nodes.
+ *
+ * @param list Topological sort of graph from parent to child.
+ */
+inline void UpdateValues(const wpi::SmallVector<Expression*>& list) {
+  // Traverse graph from child to parent and update values
+  for (auto& node : list | std::views::reverse) {
+    auto& lhs = node->args[0];
+    auto& rhs = node->args[1];
+
+    if (lhs != nullptr) {
+      if (rhs != nullptr) {
+        node->value = node->Value(lhs->value, rhs->value);
+      } else {
+        node->value = node->Value(lhs->value, 0.0);
       }
     }
+  }
+}
 
-    for (size_t row = 0; row < wrt.size(); ++row) {
-      wrt[row]->row = -1;
-    }
+/**
+ * Updates the adjoints in the expression graph (computes the gradient).
+ *
+ * @param list Topological sort of graph from parent to child.
+ */
+inline void UpdateAdjoints(const wpi::SmallVector<Expression*>& list) {
+  // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
+  // for background on reverse accumulation automatic differentiation.
 
-    return grad;
+  if (list.empty()) {
+    return;
   }
 
-  /**
-   * Updates the adjoints in the expression graph, effectively computing the
-   * gradient.
-   *
-   * @param func A function that takes two arguments: an int for the gradient
-   *   row, and a double for the adjoint (gradient value).
-   */
-  void ComputeAdjoints(function_ref<void(int row, double adjoint)> func) {
-    // Zero adjoints. The root node's adjoint is 1.0 as df/df is always 1.
-    m_adjointList[0]->adjoint = 1.0;
-    for (auto it = m_adjointList.begin() + 1; it != m_adjointList.end(); ++it) {
-      auto& node = *it;
-      node->adjoint = 0.0;
-    }
+  // Set root node's adjoint to 1 since df/df is 1
+  list[0]->adjoint = 1.0;
 
-    // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y
-    // multiplied by dy/dx. If there are multiple "paths" from the root node to
-    // variable; the variable's adjoint is the sum of each path's adjoint
-    // contribution.
-    for (size_t col = 0; col < m_adjointList.size(); ++col) {
-      auto& node = m_adjointList[col];
-      auto& lhs = node->args[0];
-      auto& rhs = node->args[1];
-
-      if (lhs != nullptr) {
-        if (rhs != nullptr) {
-          lhs->adjoint += node->gradientValueFuncs[0](lhs->value, rhs->value,
-                                                      node->adjoint);
-          rhs->adjoint += node->gradientValueFuncs[1](lhs->value, rhs->value,
-                                                      node->adjoint);
-        } else {
-          lhs->adjoint +=
-              node->gradientValueFuncs[0](lhs->value, 0.0, node->adjoint);
-        }
-      }
+  // Zero the rest of the adjoints
+  for (auto& node : list | std::views::drop(1)) {
+    node->adjoint = 0.0;
+  }
 
-      // If variable is a leaf node, assign its adjoint to the gradient.
-      int row = m_rowList[col];
-      if (row != -1) {
-        func(row, node->adjoint);
+  // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y
+  // multiplied by dy/dx. If there are multiple "paths" from the root node to
+  // variable; the variable's adjoint is the sum of each path's adjoint
+  // contribution.
+  for (const auto& node : list) {
+    auto& lhs = node->args[0];
+    auto& rhs = node->args[1];
+
+    if (lhs != nullptr) {
+      if (rhs != nullptr) {
+        lhs->adjoint += node->GradL(lhs->value, rhs->value, node->adjoint);
+        rhs->adjoint += node->GradR(lhs->value, rhs->value, node->adjoint);
+      } else {
+        lhs->adjoint += node->GradL(lhs->value, 0.0, node->adjoint);
       }
     }
   }
-
- private:
-  // List that maps nodes to their respective row.
-  wpi::SmallVector<int> m_rowList;
-
-  // List for updating adjoints
-  wpi::SmallVector<Expression*> m_adjointList;
-
-  // List for updating values
-  wpi::SmallVector<Expression*> m_valueList;
-};
+}
 
 }  // namespace sleipnir::detail
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp
index cf6a4171e75..3f9bfeac8c7 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp
@@ -5,11 +5,12 @@
 #include <utility>
 
 #include <Eigen/SparseCore>
+#include <wpi/SmallVector.h>
 
 #include "sleipnir/autodiff/Jacobian.hpp"
-#include "sleipnir/autodiff/Profiler.hpp"
 #include "sleipnir/autodiff/Variable.hpp"
 #include "sleipnir/autodiff/VariableMatrix.hpp"
+#include "sleipnir/util/SolveProfiler.hpp"
 #include "sleipnir/util/SymbolExports.hpp"
 
 namespace sleipnir {
@@ -30,7 +31,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
    * @param wrt Variable with respect to which to compute the gradient.
    */
   Gradient(Variable variable, Variable wrt) noexcept
-      : Gradient{std::move(variable), VariableMatrix{wrt}} {}
+      : m_jacobian{std::move(variable), VariableMatrix{std::move(wrt)}} {}
 
   /**
    * Constructs a Gradient object.
@@ -39,8 +40,8 @@ class SLEIPNIR_DLLEXPORT Gradient {
    * @param wrt Vector of variables with respect to which to compute the
    *   gradient.
    */
-  Gradient(Variable variable, const VariableMatrix& wrt) noexcept
-      : m_jacobian{variable, wrt} {}
+  Gradient(Variable variable, VariableMatrix wrt) noexcept
+      : m_jacobian{std::move(variable), std::move(wrt)} {}
 
   /**
    * Returns the gradient as a VariableMatrix.
@@ -62,7 +63,9 @@ class SLEIPNIR_DLLEXPORT Gradient {
   /**
    * Returns the profiler.
    */
-  Profiler& GetProfiler() { return m_jacobian.GetProfiler(); }
+  const wpi::SmallVector<SolveProfiler>& GetProfilers() const {
+    return m_jacobian.GetProfilers();
+  }
 
  private:
   Eigen::SparseVector<double> m_g;
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp
index 2e60d89e952..30bb88bd117 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp
@@ -2,17 +2,14 @@
 
 #pragma once
 
-#include <utility>
-
-#include <Eigen/Core>
 #include <Eigen/SparseCore>
 #include <wpi/SmallVector.h>
 
-#include "sleipnir/autodiff/ExpressionGraph.hpp"
+#include "sleipnir/autodiff/AdjointExpressionGraph.hpp"
 #include "sleipnir/autodiff/Jacobian.hpp"
-#include "sleipnir/autodiff/Profiler.hpp"
 #include "sleipnir/autodiff/Variable.hpp"
 #include "sleipnir/autodiff/VariableMatrix.hpp"
+#include "sleipnir/util/SolveProfiler.hpp"
 #include "sleipnir/util/SymbolExports.hpp"
 
 namespace sleipnir {
@@ -33,25 +30,9 @@ class SLEIPNIR_DLLEXPORT Hessian {
    * @param wrt Vector of variables with respect to which to compute the
    *   Hessian.
    */
-  Hessian(Variable variable, const VariableMatrix& wrt) noexcept
+  Hessian(Variable variable, VariableMatrix wrt) noexcept
       : m_jacobian{
-            [&] {
-              wpi::SmallVector<detail::ExpressionPtr> wrtVec;
-              wrtVec.reserve(wrt.size());
-              for (auto& elem : wrt) {
-                wrtVec.emplace_back(elem.expr);
-              }
-
-              auto grad =
-                  detail::ExpressionGraph{variable.expr}.GenerateGradientTree(
-                      wrtVec);
-
-              VariableMatrix ret{wrt.Rows()};
-              for (int row = 0; row < ret.Rows(); ++row) {
-                ret(row) = Variable{std::move(grad[row])};
-              }
-              return ret;
-            }(),
+            detail::AdjointExpressionGraph{variable}.GenerateGradientTree(wrt),
             wrt} {}
 
   /**
@@ -68,9 +49,11 @@ class SLEIPNIR_DLLEXPORT Hessian {
   const Eigen::SparseMatrix<double>& Value() { return m_jacobian.Value(); }
 
   /**
-   * Returns the profiler.
+   * Returns the profilers.
    */
-  Profiler& GetProfiler() { return m_jacobian.GetProfiler(); }
+  const wpi::SmallVector<SolveProfiler>& GetProfilers() const {
+    return m_jacobian.GetProfilers();
+  }
 
  private:
   Jacobian m_jacobian;
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp
index 0c660156c80..70af2691a3d 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp
@@ -7,10 +7,11 @@
 #include <Eigen/SparseCore>
 #include <wpi/SmallVector.h>
 
-#include "sleipnir/autodiff/ExpressionGraph.hpp"
-#include "sleipnir/autodiff/Profiler.hpp"
+#include "sleipnir/autodiff/AdjointExpressionGraph.hpp"
 #include "sleipnir/autodiff/Variable.hpp"
 #include "sleipnir/autodiff/VariableMatrix.hpp"
+#include "sleipnir/util/ScopedProfiler.hpp"
+#include "sleipnir/util/SolveProfiler.hpp"
 #include "sleipnir/util/SymbolExports.hpp"
 
 namespace sleipnir {
@@ -31,29 +32,32 @@ class SLEIPNIR_DLLEXPORT Jacobian {
    * @param wrt Vector of variables with respect to which to compute the
    *   Jacobian.
    */
-  Jacobian(const VariableMatrix& variables, const VariableMatrix& wrt) noexcept
+  Jacobian(VariableMatrix variables, VariableMatrix wrt) noexcept
       : m_variables{std::move(variables)}, m_wrt{std::move(wrt)} {
-    m_profiler.StartSetup();
-
-    for (int row = 0; row < m_wrt.Rows(); ++row) {
-      m_wrt(row).expr->row = row;
+    // Initialize column each expression's adjoint occupies in the Jacobian
+    for (size_t col = 0; col < m_wrt.size(); ++col) {
+      m_wrt(col).expr->col = col;
     }
 
-    for (Variable variable : m_variables) {
-      m_graphs.emplace_back(variable.expr);
+    for (auto& variable : m_variables) {
+      m_graphs.emplace_back(variable);
     }
 
-    // Reserve triplet space for 99% sparsity
-    m_cachedTriplets.reserve(m_variables.Rows() * m_wrt.Rows() * 0.01);
+    // Reset col to -1
+    for (auto& node : m_wrt) {
+      node.expr->col = -1;
+    }
 
     for (int row = 0; row < m_variables.Rows(); ++row) {
+      if (m_variables(row).expr == nullptr) {
+        continue;
+      }
+
       if (m_variables(row).Type() == ExpressionType::kLinear) {
         // If the row is linear, compute its gradient once here and cache its
         // triplets. Constant rows are ignored because their gradients have no
         // nonzero triplets.
-        m_graphs[row].ComputeAdjoints([&](int col, double adjoint) {
-          m_cachedTriplets.emplace_back(row, col, adjoint);
-        });
+        m_graphs[row].AppendAdjointTriplets(m_cachedTriplets, row);
       } else if (m_variables(row).Type() > ExpressionType::kLinear) {
         // If the row is quadratic or nonlinear, add it to the list of nonlinear
         // rows to be recomputed in Value().
@@ -61,15 +65,14 @@ class SLEIPNIR_DLLEXPORT Jacobian {
       }
     }
 
-    for (int row = 0; row < m_wrt.Rows(); ++row) {
-      m_wrt(row).expr->row = -1;
-    }
-
     if (m_nonlinearRows.empty()) {
       m_J.setFromTriplets(m_cachedTriplets.begin(), m_cachedTriplets.end());
     }
 
-    m_profiler.StopSetup();
+    m_profilers.emplace_back("");
+    m_profilers.emplace_back("    ↳ graph update");
+    m_profilers.emplace_back("    ↳ adjoints");
+    m_profilers.emplace_back("    ↳ matrix build");
   }
 
   /**
@@ -79,18 +82,17 @@ class SLEIPNIR_DLLEXPORT Jacobian {
    * them.
    */
   VariableMatrix Get() const {
-    VariableMatrix result{m_variables.Rows(), m_wrt.Rows()};
-
-    wpi::SmallVector<detail::ExpressionPtr> wrtVec;
-    wrtVec.reserve(m_wrt.size());
-    for (auto& elem : m_wrt) {
-      wrtVec.emplace_back(elem.expr);
-    }
+    VariableMatrix result{VariableMatrix::empty, m_variables.Rows(),
+                          m_wrt.Rows()};
 
     for (int row = 0; row < m_variables.Rows(); ++row) {
-      auto grad = m_graphs[row].GenerateGradientTree(wrtVec);
+      auto grad = m_graphs[row].GenerateGradientTree(m_wrt);
       for (int col = 0; col < m_wrt.Rows(); ++col) {
-        result(row, col) = Variable{std::move(grad[col])};
+        if (grad(col).expr != nullptr) {
+          result(row, col) = std::move(grad(col));
+        } else {
+          result(row, col) = Variable{0.0};
+        }
       }
     }
 
@@ -101,44 +103,56 @@ class SLEIPNIR_DLLEXPORT Jacobian {
    * Evaluates the Jacobian at wrt's value.
    */
   const Eigen::SparseMatrix<double>& Value() {
+    ScopedProfiler valueProfiler{m_profilers[0]};
+
     if (m_nonlinearRows.empty()) {
       return m_J;
     }
 
-    m_profiler.StartSolve();
+    ScopedProfiler graphUpdateProfiler{m_profilers[1]};
 
     for (auto& graph : m_graphs) {
-      graph.Update();
+      graph.UpdateValues();
     }
 
+    graphUpdateProfiler.Stop();
+    ScopedProfiler adjointsProfiler{m_profilers[2]};
+
     // Copy the cached triplets so triplets added for the nonlinear rows are
     // thrown away at the end of the function
     auto triplets = m_cachedTriplets;
 
     // Compute each nonlinear row of the Jacobian
     for (int row : m_nonlinearRows) {
-      m_graphs[row].ComputeAdjoints([&](int col, double adjoint) {
-        triplets.emplace_back(row, col, adjoint);
-      });
+      m_graphs[row].AppendAdjointTriplets(triplets, row);
     }
 
-    m_J.setFromTriplets(triplets.begin(), triplets.end());
+    adjointsProfiler.Stop();
+    ScopedProfiler matrixBuildProfiler{m_profilers[3]};
 
-    m_profiler.StopSolve();
+    if (!triplets.empty()) {
+      m_J.setFromTriplets(triplets.begin(), triplets.end());
+    } else {
+      // setFromTriplets() is a no-op on empty triplets, so explicitly zero out
+      // the storage
+      m_J.setZero();
+    }
 
     return m_J;
   }
 
   /**
-   * Returns the profiler.
+   * Returns the profilers.
    */
-  Profiler& GetProfiler() { return m_profiler; }
+  const wpi::SmallVector<SolveProfiler>& GetProfilers() const {
+    return m_profilers;
+  }
 
  private:
   VariableMatrix m_variables;
   VariableMatrix m_wrt;
 
-  wpi::SmallVector<detail::ExpressionGraph> m_graphs;
+  wpi::SmallVector<detail::AdjointExpressionGraph> m_graphs;
 
   Eigen::SparseMatrix<double> m_J{m_variables.Rows(), m_wrt.Rows()};
 
@@ -149,7 +163,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
   // Value()
   wpi::SmallVector<int> m_nonlinearRows;
 
-  Profiler m_profiler;
+  wpi::SmallVector<SolveProfiler> m_profilers;
 };
 
 }  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp
deleted file mode 100644
index 5c22d145b3d..00000000000
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp
+++ /dev/null
@@ -1,79 +0,0 @@
-// Copyright (c) Sleipnir contributors
-
-#pragma once
-
-#include <chrono>
-
-#include "sleipnir/util/SymbolExports.hpp"
-
-namespace sleipnir {
-
-/**
- * Records the number of profiler measurements (start/stop pairs) and the
- * average duration between each start and stop call.
- */
-class SLEIPNIR_DLLEXPORT Profiler {
- public:
-  /**
-   * Tell the profiler to start measuring setup time.
-   */
-  void StartSetup() { m_setupStartTime = std::chrono::system_clock::now(); }
-
-  /**
-   * Tell the profiler to stop measuring setup time.
-   */
-  void StopSetup() {
-    m_setupDuration = std::chrono::system_clock::now() - m_setupStartTime;
-  }
-
-  /**
-   * Tell the profiler to start measuring solve time.
-   */
-  void StartSolve() { m_solveStartTime = std::chrono::system_clock::now(); }
-
-  /**
-   * Tell the profiler to stop measuring solve time, increment the number of
-   * averages, and incorporate the latest measurement into the average.
-   */
-  void StopSolve() {
-    auto now = std::chrono::system_clock::now();
-    ++m_solveMeasurements;
-    m_averageSolveDuration =
-        (m_solveMeasurements - 1.0) / m_solveMeasurements *
-            m_averageSolveDuration +
-        1.0 / m_solveMeasurements * (now - m_solveStartTime);
-  }
-
-  /**
-   * The setup duration in milliseconds as a double.
-   */
-  double SetupDuration() const {
-    using std::chrono::duration_cast;
-    using std::chrono::nanoseconds;
-    return duration_cast<nanoseconds>(m_setupDuration).count() / 1e6;
-  }
-
-  /**
-   * The number of solve measurements taken.
-   */
-  int SolveMeasurements() const { return m_solveMeasurements; }
-
-  /**
-   * The average solve duration in milliseconds as a double.
-   */
-  double AverageSolveDuration() const {
-    using std::chrono::duration_cast;
-    using std::chrono::nanoseconds;
-    return duration_cast<nanoseconds>(m_averageSolveDuration).count() / 1e6;
-  }
-
- private:
-  std::chrono::system_clock::time_point m_setupStartTime;
-  std::chrono::duration<double> m_setupDuration{0.0};
-
-  int m_solveMeasurements = 0;
-  std::chrono::duration<double> m_averageSolveDuration{0.0};
-  std::chrono::system_clock::time_point m_solveStartTime;
-};
-
-}  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp
index f25c6d15331..e9f5fa12baf 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp
@@ -5,6 +5,8 @@
 #include <algorithm>
 #include <concepts>
 #include <initializer_list>
+#include <optional>
+#include <source_location>
 #include <type_traits>
 #include <utility>
 #include <vector>
@@ -16,12 +18,18 @@
 #include "sleipnir/autodiff/ExpressionGraph.hpp"
 #include "sleipnir/util/Assert.hpp"
 #include "sleipnir/util/Concepts.hpp"
-#include "sleipnir/util/Print.hpp"
 #include "sleipnir/util/SymbolExports.hpp"
 
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+#include "sleipnir/util/Print.hpp"
+#endif
+
 namespace sleipnir {
 
 // Forward declarations for friend declarations in Variable
+namespace detail {
+class AdjointExpressionGraph;
+}  // namespace detail
 class SLEIPNIR_DLLEXPORT Hessian;
 class SLEIPNIR_DLLEXPORT Jacobian;
 
@@ -36,11 +44,25 @@ class SLEIPNIR_DLLEXPORT Variable {
   Variable() = default;
 
   /**
-   * Constructs a Variable from a double.
+   * Constructs an empty Variable.
+   */
+  explicit constexpr Variable(std::nullptr_t) : expr{nullptr} {}
+
+  /**
+   * Constructs a Variable from a floating point type.
    *
    * @param value The value of the Variable.
    */
-  Variable(double value) : expr{detail::MakeExpressionPtr(value)} {}  // NOLINT
+  Variable(std::floating_point auto value)  // NOLINT
+      : expr{detail::MakeExpressionPtr<detail::ConstExpression>(value)} {}
+
+  /**
+   * Constructs a Variable from an integral type.
+   *
+   * @param value The value of the Variable.
+   */
+  Variable(std::integral auto value)  // NOLINT
+      : expr{detail::MakeExpressionPtr<detail::ConstExpression>(value)} {}
 
   /**
    * Constructs a Variable pointing to the specified expression.
@@ -54,7 +76,8 @@ class SLEIPNIR_DLLEXPORT Variable {
    *
    * @param expr The autodiff variable.
    */
-  explicit Variable(detail::ExpressionPtr&& expr) : expr{std::move(expr)} {}
+  explicit constexpr Variable(detail::ExpressionPtr&& expr)
+      : expr{std::move(expr)} {}
 
   /**
    * Assignment operator for double.
@@ -62,7 +85,7 @@ class SLEIPNIR_DLLEXPORT Variable {
    * @param value The value of the Variable.
    */
   Variable& operator=(double value) {
-    expr = detail::MakeExpressionPtr(value);
+    expr = detail::MakeExpressionPtr<detail::ConstExpression>(value);
 
     return *this;
   }
@@ -74,16 +97,19 @@ class SLEIPNIR_DLLEXPORT Variable {
    */
   void SetValue(double value) {
     if (expr->IsConstant(0.0)) {
-      expr = detail::MakeExpressionPtr(value);
+      expr = detail::MakeExpressionPtr<detail::ConstExpression>(value);
     } else {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
       // We only need to check the first argument since unary and binary
       // operators both use it
-      if (expr->args[0] != nullptr && !expr->args[0]->IsConstant(0.0)) {
+      if (expr->args[0] != nullptr) {
+        auto location = std::source_location::current();
         sleipnir::println(
             stderr,
-            "WARNING: {}:{}: Modified the value of a dependent variable",
-            __FILE__, __LINE__);
+            "WARNING: {}:{}: {}: Modified the value of a dependent variable",
+            location.file_name(), location.line(), location.function_name());
       }
+#endif
       expr->value = value;
     }
   }
@@ -194,9 +220,10 @@ class SLEIPNIR_DLLEXPORT Variable {
    * Returns the value of this variable.
    */
   double Value() {
-    // Updates the value of this variable based on the values of its dependent
-    // variables
-    detail::ExpressionGraph{expr}.Update();
+    if (!m_graph) {
+      m_graph = detail::TopologicalSort(expr);
+    }
+    detail::UpdateValues(m_graph.value());
 
     return expr->value;
   }
@@ -205,12 +232,16 @@ class SLEIPNIR_DLLEXPORT Variable {
    * Returns the type of this expression (constant, linear, quadratic, or
    * nonlinear).
    */
-  ExpressionType Type() const { return expr->type; }
+  ExpressionType Type() const { return expr->Type(); }
 
  private:
   /// The expression node.
   detail::ExpressionPtr expr =
-      detail::MakeExpressionPtr(0.0, ExpressionType::kLinear);
+      detail::MakeExpressionPtr<detail::DecisionVariableExpression>();
+
+  /// Updates the value of this variable based on the values of its dependent
+  /// variables
+  std::optional<wpi::SmallVector<detail::Expression*>> m_graph;
 
   friend SLEIPNIR_DLLEXPORT Variable abs(const Variable& x);
   friend SLEIPNIR_DLLEXPORT Variable acos(const Variable& x);
@@ -237,6 +268,7 @@ class SLEIPNIR_DLLEXPORT Variable {
   friend SLEIPNIR_DLLEXPORT Variable hypot(const Variable& x, const Variable& y,
                                            const Variable& z);
 
+  friend class detail::AdjointExpressionGraph;
   friend class SLEIPNIR_DLLEXPORT Hessian;
   friend class SLEIPNIR_DLLEXPORT Jacobian;
 };
@@ -585,9 +617,9 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
    * Implicit conversion operator to bool.
    */
   operator bool() {  // NOLINT
-    return std::all_of(
-        constraints.begin(), constraints.end(),
-        [](auto& constraint) { return constraint.Value() == 0.0; });
+    return std::ranges::all_of(constraints, [](auto& constraint) {
+      return constraint.Value() == 0.0;
+    });
   }
 };
 
@@ -649,9 +681,9 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
    * Implicit conversion operator to bool.
    */
   operator bool() {  // NOLINT
-    return std::all_of(
-        constraints.begin(), constraints.end(),
-        [](auto& constraint) { return constraint.Value() >= 0.0; });
+    return std::ranges::all_of(constraints, [](auto& constraint) {
+      return constraint.Value() >= 0.0;
+    });
   }
 };
 
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp
index 72bb2359177..2c5223f10ee 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp
@@ -478,7 +478,7 @@ class VariableBlock {
 
     for (int i = 0; i < Rows(); ++i) {
       for (int j = 0; j < rhs.Cols(); ++j) {
-        Variable sum;
+        Variable sum{0.0};
         for (int k = 0; k < Cols(); ++k) {
           sum += (*this)(i, k) * rhs(k, j);
         }
@@ -573,7 +573,7 @@ class VariableBlock {
    * Returns the transpose of the variable matrix.
    */
   std::remove_cv_t<Mat> T() const {
-    std::remove_cv_t<Mat> result{Cols(), Rows()};
+    std::remove_cv_t<Mat> result{Mat::empty, Cols(), Rows()};
 
     for (int row = 0; row < Rows(); ++row) {
       for (int col = 0; col < Cols(); ++col) {
@@ -640,7 +640,7 @@ class VariableBlock {
    */
   std::remove_cv_t<Mat> CwiseTransform(
       function_ref<Variable(const Variable& x)> unaryOp) const {
-    std::remove_cv_t<Mat> result{Rows(), Cols()};
+    std::remove_cv_t<Mat> result{Mat::empty, Rows(), Cols()};
 
     for (int row = 0; row < Rows(); ++row) {
       for (int col = 0; col < Cols(); ++col) {
@@ -659,29 +659,29 @@ class VariableBlock {
     using pointer = Variable*;
     using reference = Variable&;
 
-    iterator(VariableBlock<Mat>* mat, int row, int col)
-        : m_mat{mat}, m_row{row}, m_col{col} {}
+    constexpr iterator() noexcept = default;
 
-    iterator& operator++() {
-      ++m_col;
-      if (m_col == m_mat->Cols()) {
-        m_col = 0;
-        ++m_row;
-      }
+    constexpr iterator(VariableBlock<Mat>* mat, int index) noexcept
+        : m_mat{mat}, m_index{index} {}
+
+    constexpr iterator& operator++() noexcept {
+      ++m_index;
       return *this;
     }
-    iterator operator++(int) {
+
+    constexpr iterator operator++(int) noexcept {
       iterator retval = *this;
       ++(*this);
       return retval;
     }
-    bool operator==(const iterator&) const = default;
-    reference operator*() { return (*m_mat)(m_row, m_col); }
+
+    constexpr bool operator==(const iterator&) const noexcept = default;
+
+    constexpr reference operator*() const noexcept { return (*m_mat)(m_index); }
 
    private:
-    VariableBlock<Mat>* m_mat;
-    int m_row;
-    int m_col;
+    VariableBlock<Mat>* m_mat = nullptr;
+    int m_index = 0;
   };
 
   class const_iterator {
@@ -692,60 +692,62 @@ class VariableBlock {
     using pointer = Variable*;
     using const_reference = const Variable&;
 
-    const_iterator(const VariableBlock<Mat>* mat, int row, int col)
-        : m_mat{mat}, m_row{row}, m_col{col} {}
+    constexpr const_iterator() noexcept = default;
 
-    const_iterator& operator++() {
-      ++m_col;
-      if (m_col == m_mat->Cols()) {
-        m_col = 0;
-        ++m_row;
-      }
+    constexpr const_iterator(const VariableBlock<Mat>* mat, int index) noexcept
+        : m_mat{mat}, m_index{index} {}
+
+    constexpr const_iterator& operator++() noexcept {
+      ++m_index;
       return *this;
     }
-    const_iterator operator++(int) {
+
+    constexpr const_iterator operator++(int) noexcept {
       const_iterator retval = *this;
       ++(*this);
       return retval;
     }
-    bool operator==(const const_iterator&) const = default;
-    const_reference operator*() const { return (*m_mat)(m_row, m_col); }
+
+    constexpr bool operator==(const const_iterator&) const noexcept = default;
+
+    constexpr const_reference operator*() const noexcept {
+      return (*m_mat)(m_index);
+    }
 
    private:
-    const VariableBlock<Mat>* m_mat;
-    int m_row;
-    int m_col;
+    const VariableBlock<Mat>* m_mat = nullptr;
+    int m_index = 0;
   };
 
   /**
    * Returns begin iterator.
    */
-  iterator begin() { return iterator(this, 0, 0); }
+  iterator begin() { return iterator(this, 0); }
 
   /**
    * Returns end iterator.
    */
-  iterator end() { return iterator(this, Rows(), 0); }
+  iterator end() { return iterator(this, Rows() * Cols()); }
 
   /**
    * Returns begin iterator.
    */
-  const_iterator begin() const { return const_iterator(this, 0, 0); }
+  const_iterator begin() const { return const_iterator(this, 0); }
 
   /**
    * Returns end iterator.
    */
-  const_iterator end() const { return const_iterator(this, Rows(), 0); }
+  const_iterator end() const { return const_iterator(this, Rows() * Cols()); }
 
   /**
    * Returns begin iterator.
    */
-  const_iterator cbegin() const { return const_iterator(this, 0, 0); }
+  const_iterator cbegin() const { return const_iterator(this, 0); }
 
   /**
    * Returns end iterator.
    */
-  const_iterator cend() const { return const_iterator(this, Rows(), 0); }
+  const_iterator cend() const { return const_iterator(this, Rows() * Cols()); }
 
   /**
    * Returns number of elements in matrix.
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp
index 57b09913d15..091bd720d81 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp
@@ -27,6 +27,9 @@ namespace sleipnir {
  */
 class SLEIPNIR_DLLEXPORT VariableMatrix {
  public:
+  struct empty_t {};
+  static constexpr empty_t empty{};
+
   /**
    * Constructs an empty VariableMatrix.
    */
@@ -38,22 +41,35 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    * @param rows The number of matrix rows.
    */
   explicit VariableMatrix(int rows) : m_rows{rows}, m_cols{1} {
-    for (int row = 0; row < rows; ++row) {
+    m_storage.reserve(Rows());
+    for (int row = 0; row < Rows(); ++row) {
       m_storage.emplace_back();
     }
   }
 
   /**
-   * Constructs a VariableMatrix with the given dimensions.
+   * Constructs a zero-initialized VariableMatrix with the given dimensions.
    *
    * @param rows The number of matrix rows.
    * @param cols The number of matrix columns.
    */
   VariableMatrix(int rows, int cols) : m_rows{rows}, m_cols{cols} {
-    for (int row = 0; row < rows; ++row) {
-      for (int col = 0; col < cols; ++col) {
-        m_storage.emplace_back();
-      }
+    m_storage.reserve(Rows() * Cols());
+    for (int index = 0; index < Rows() * Cols(); ++index) {
+      m_storage.emplace_back();
+    }
+  }
+
+  /**
+   * Constructs an empty VariableMatrix with the given dimensions.
+   *
+   * @param rows The number of matrix rows.
+   * @param cols The number of matrix columns.
+   */
+  VariableMatrix(empty_t, int rows, int cols) : m_rows{rows}, m_cols{cols} {
+    m_storage.reserve(Rows() * Cols());
+    for (int index = 0; index < Rows() * Cols(); ++index) {
+      m_storage.emplace_back(nullptr);
     }
   }
 
@@ -79,7 +95,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
 
     m_storage.reserve(Rows() * Cols());
     for (const auto& row : list) {
-      std::copy(row.begin(), row.end(), std::back_inserter(m_storage));
+      std::ranges::copy(row, std::back_inserter(m_storage));
     }
   }
 
@@ -106,7 +122,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
 
     m_storage.reserve(Rows() * Cols());
     for (const auto& row : list) {
-      std::copy(row.begin(), row.end(), std::back_inserter(m_storage));
+      std::ranges::copy(row, std::back_inserter(m_storage));
     }
   }
 
@@ -133,7 +149,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
 
     m_storage.reserve(Rows() * Cols());
     for (const auto& row : list) {
-      std::copy(row.begin(), row.end(), std::back_inserter(m_storage));
+      std::ranges::copy(row, std::back_inserter(m_storage));
     }
   }
 
@@ -238,6 +254,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    */
   VariableMatrix(const VariableBlock<VariableMatrix>& values)  // NOLINT
       : m_rows{values.Rows()}, m_cols{values.Cols()} {
+    m_storage.reserve(Rows() * Cols());
     for (int row = 0; row < Rows(); ++row) {
       for (int col = 0; col < Cols(); ++col) {
         m_storage.emplace_back(values(row, col));
@@ -252,6 +269,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    */
   VariableMatrix(const VariableBlock<const VariableMatrix>& values)  // NOLINT
       : m_rows{values.Rows()}, m_cols{values.Cols()} {
+    m_storage.reserve(Rows() * Cols());
     for (int row = 0; row < Rows(); ++row) {
       for (int col = 0; col < Cols(); ++col) {
         m_storage.emplace_back(values(row, col));
@@ -266,6 +284,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    */
   explicit VariableMatrix(std::span<const Variable> values)
       : m_rows{static_cast<int>(values.size())}, m_cols{1} {
+    m_storage.reserve(Rows() * Cols());
     for (int row = 0; row < Rows(); ++row) {
       for (int col = 0; col < Cols(); ++col) {
         m_storage.emplace_back(values[row * Cols() + col]);
@@ -283,6 +302,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
   VariableMatrix(std::span<const Variable> values, int rows, int cols)
       : m_rows{rows}, m_cols{cols} {
     Assert(static_cast<int>(values.size()) == Rows() * Cols());
+    m_storage.reserve(Rows() * Cols());
     for (int row = 0; row < Rows(); ++row) {
       for (int col = 0; col < Cols(); ++col) {
         m_storage.emplace_back(values[row * Cols() + col]);
@@ -507,11 +527,11 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
   operator*(const VariableMatrix& lhs, const VariableMatrix& rhs) {
     Assert(lhs.Cols() == rhs.Rows());
 
-    VariableMatrix result{lhs.Rows(), rhs.Cols()};
+    VariableMatrix result{VariableMatrix::empty, lhs.Rows(), rhs.Cols()};
 
     for (int i = 0; i < lhs.Rows(); ++i) {
       for (int j = 0; j < rhs.Cols(); ++j) {
-        Variable sum;
+        Variable sum{0.0};
         for (int k = 0; k < lhs.Cols(); ++k) {
           sum += lhs(i, k) * rhs(k, j);
         }
@@ -530,7 +550,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    */
   friend SLEIPNIR_DLLEXPORT VariableMatrix operator*(const VariableMatrix& lhs,
                                                      const Variable& rhs) {
-    VariableMatrix result{lhs.Rows(), lhs.Cols()};
+    VariableMatrix result{VariableMatrix::empty, lhs.Rows(), lhs.Cols()};
 
     for (int row = 0; row < result.Rows(); ++row) {
       for (int col = 0; col < result.Cols(); ++col) {
@@ -560,7 +580,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    */
   friend SLEIPNIR_DLLEXPORT VariableMatrix
   operator*(const Variable& lhs, const VariableMatrix& rhs) {
-    VariableMatrix result{rhs.Rows(), rhs.Cols()};
+    VariableMatrix result{VariableMatrix::empty, rhs.Rows(), rhs.Cols()};
 
     for (int row = 0; row < result.Rows(); ++row) {
       for (int col = 0; col < result.Cols(); ++col) {
@@ -592,7 +612,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
 
     for (int i = 0; i < Rows(); ++i) {
       for (int j = 0; j < rhs.Cols(); ++j) {
-        Variable sum;
+        Variable sum{0.0};
         for (int k = 0; k < Cols(); ++k) {
           sum += (*this)(i, k) * rhs(k, j);
         }
@@ -611,7 +631,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    */
   friend SLEIPNIR_DLLEXPORT VariableMatrix operator/(const VariableMatrix& lhs,
                                                      const Variable& rhs) {
-    VariableMatrix result{lhs.Rows(), lhs.Cols()};
+    VariableMatrix result{VariableMatrix::empty, lhs.Rows(), lhs.Cols()};
 
     for (int row = 0; row < result.Rows(); ++row) {
       for (int col = 0; col < result.Cols(); ++col) {
@@ -646,7 +666,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    */
   friend SLEIPNIR_DLLEXPORT VariableMatrix
   operator+(const VariableMatrix& lhs, const VariableMatrix& rhs) {
-    VariableMatrix result{lhs.Rows(), lhs.Cols()};
+    VariableMatrix result{VariableMatrix::empty, lhs.Rows(), lhs.Cols()};
 
     for (int row = 0; row < result.Rows(); ++row) {
       for (int col = 0; col < result.Cols(); ++col) {
@@ -680,7 +700,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    */
   friend SLEIPNIR_DLLEXPORT VariableMatrix
   operator-(const VariableMatrix& lhs, const VariableMatrix& rhs) {
-    VariableMatrix result{lhs.Rows(), lhs.Cols()};
+    VariableMatrix result{VariableMatrix::empty, lhs.Rows(), lhs.Cols()};
 
     for (int row = 0; row < result.Rows(); ++row) {
       for (int col = 0; col < result.Cols(); ++col) {
@@ -713,7 +733,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    */
   friend SLEIPNIR_DLLEXPORT VariableMatrix
   operator-(const VariableMatrix& lhs) {
-    VariableMatrix result{lhs.Rows(), lhs.Cols()};
+    VariableMatrix result{VariableMatrix::empty, lhs.Rows(), lhs.Cols()};
 
     for (int row = 0; row < result.Rows(); ++row) {
       for (int col = 0; col < result.Cols(); ++col) {
@@ -736,7 +756,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    * Returns the transpose of the variable matrix.
    */
   VariableMatrix T() const {
-    VariableMatrix result{Cols(), Rows()};
+    VariableMatrix result{VariableMatrix::empty, Cols(), Rows()};
 
     for (int row = 0; row < Rows(); ++row) {
       for (int col = 0; col < Cols(); ++col) {
@@ -801,7 +821,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    */
   VariableMatrix CwiseTransform(
       function_ref<Variable(const Variable& x)> unaryOp) const {
-    VariableMatrix result{Rows(), Cols()};
+    VariableMatrix result{VariableMatrix::empty, Rows(), Cols()};
 
     for (int row = 0; row < Rows(); ++row) {
       for (int col = 0; col < Cols(); ++col) {
@@ -820,29 +840,29 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
     using pointer = Variable*;
     using reference = Variable&;
 
-    iterator(VariableMatrix* mat, int row, int col)
-        : m_mat{mat}, m_row{row}, m_col{col} {}
+    constexpr iterator() noexcept = default;
 
-    iterator& operator++() {
-      ++m_col;
-      if (m_col == m_mat->Cols()) {
-        m_col = 0;
-        ++m_row;
-      }
+    explicit constexpr iterator(
+        wpi::SmallVector<Variable>::iterator it) noexcept
+        : m_it{it} {}
+
+    constexpr iterator& operator++() noexcept {
+      ++m_it;
       return *this;
     }
-    iterator operator++(int) {
+
+    constexpr iterator operator++(int) noexcept {
       iterator retval = *this;
       ++(*this);
       return retval;
     }
-    bool operator==(const iterator&) const = default;
-    reference operator*() { return (*m_mat)(m_row, m_col); }
+
+    constexpr bool operator==(const iterator&) const noexcept = default;
+
+    constexpr reference operator*() const noexcept { return *m_it; }
 
    private:
-    VariableMatrix* m_mat;
-    int m_row;
-    int m_col;
+    wpi::SmallVector<Variable>::iterator m_it;
   };
 
   class const_iterator {
@@ -853,65 +873,65 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
     using pointer = Variable*;
     using const_reference = const Variable&;
 
-    const_iterator(const VariableMatrix* mat, int row, int col)
-        : m_mat{mat}, m_row{row}, m_col{col} {}
+    constexpr const_iterator() noexcept = default;
 
-    const_iterator& operator++() {
-      ++m_col;
-      if (m_col == m_mat->Cols()) {
-        m_col = 0;
-        ++m_row;
-      }
+    explicit constexpr const_iterator(
+        wpi::SmallVector<Variable>::const_iterator it) noexcept
+        : m_it{it} {}
+
+    constexpr const_iterator& operator++() noexcept {
+      ++m_it;
       return *this;
     }
-    const_iterator operator++(int) {
+
+    constexpr const_iterator operator++(int) noexcept {
       const_iterator retval = *this;
       ++(*this);
       return retval;
     }
-    bool operator==(const const_iterator&) const = default;
-    const_reference operator*() const { return (*m_mat)(m_row, m_col); }
+
+    constexpr bool operator==(const const_iterator&) const noexcept = default;
+
+    constexpr const_reference operator*() const noexcept { return *m_it; }
 
    private:
-    const VariableMatrix* m_mat;
-    int m_row;
-    int m_col;
+    wpi::SmallVector<Variable>::const_iterator m_it;
   };
 
   /**
    * Returns begin iterator.
    */
-  iterator begin() { return iterator(this, 0, 0); }
+  iterator begin() { return iterator{m_storage.begin()}; }
 
   /**
    * Returns end iterator.
    */
-  iterator end() { return iterator(this, Rows(), 0); }
+  iterator end() { return iterator{m_storage.end()}; }
 
   /**
    * Returns begin iterator.
    */
-  const_iterator begin() const { return const_iterator(this, 0, 0); }
+  const_iterator begin() const { return const_iterator{m_storage.begin()}; }
 
   /**
    * Returns end iterator.
    */
-  const_iterator end() const { return const_iterator(this, Rows(), 0); }
+  const_iterator end() const { return const_iterator{m_storage.end()}; }
 
   /**
    * Returns begin iterator.
    */
-  const_iterator cbegin() const { return const_iterator(this, 0, 0); }
+  const_iterator cbegin() const { return const_iterator{m_storage.begin()}; }
 
   /**
    * Returns end iterator.
    */
-  const_iterator cend() const { return const_iterator(this, Rows(), 0); }
+  const_iterator cend() const { return const_iterator{m_storage.end()}; }
 
   /**
    * Returns number of elements in matrix.
    */
-  size_t size() const { return m_rows * m_cols; }
+  size_t size() const { return m_storage.size(); }
 
   /**
    * Returns a variable matrix filled with zeroes.
@@ -920,7 +940,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    * @param cols The number of matrix columns.
    */
   static VariableMatrix Zero(int rows, int cols) {
-    VariableMatrix result{rows, cols};
+    VariableMatrix result{VariableMatrix::empty, rows, cols};
 
     for (auto& elem : result) {
       elem = 0.0;
@@ -936,7 +956,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
    * @param cols The number of matrix columns.
    */
   static VariableMatrix Ones(int rows, int cols) {
-    VariableMatrix result{rows, cols};
+    VariableMatrix result{VariableMatrix::empty, rows, cols};
 
     for (auto& elem : result) {
       elem = 1.0;
@@ -964,7 +984,7 @@ SLEIPNIR_DLLEXPORT inline VariableMatrix CwiseReduce(
   Assert(lhs.Rows() == rhs.Rows());
   Assert(lhs.Rows() == rhs.Rows());
 
-  VariableMatrix result{lhs.Rows(), lhs.Cols()};
+  VariableMatrix result{VariableMatrix::empty, lhs.Rows(), lhs.Cols()};
 
   for (int row = 0; row < lhs.Rows(); ++row) {
     for (int col = 0; col < lhs.Cols(); ++col) {
@@ -1013,7 +1033,7 @@ SLEIPNIR_DLLEXPORT inline VariableMatrix Block(
     }
   }
 
-  VariableMatrix result{rows, cols};
+  VariableMatrix result{VariableMatrix::empty, rows, cols};
 
   int rowOffset = 0;
   for (const auto& row : list) {
@@ -1068,7 +1088,7 @@ SLEIPNIR_DLLEXPORT inline VariableMatrix Block(
     }
   }
 
-  VariableMatrix result{rows, cols};
+  VariableMatrix result{VariableMatrix::empty, rows, cols};
 
   int rowOffset = 0;
   for (const auto& row : list) {
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp
index 09b1b2f3bf3..ca6945d2691 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp
@@ -59,17 +59,16 @@ MultistartResult<DecisionVariables> Multistart(
     results.emplace_back(future.get());
   }
 
-  return *std::min_element(
-      results.cbegin(), results.cend(), [](const auto& a, const auto& b) {
-        // Prioritize successful solve
-        if (a.status.exitCondition == SolverExitCondition::kSuccess &&
-            b.status.exitCondition != SolverExitCondition::kSuccess) {
-          return true;
-        }
+  return *std::ranges::min_element(results, [](const auto& a, const auto& b) {
+    // Prioritize successful solve
+    if (a.status.exitCondition == SolverExitCondition::kSuccess &&
+        b.status.exitCondition != SolverExitCondition::kSuccess) {
+      return true;
+    }
 
-        // Otherwise prioritize solution with lower cost
-        return a.status.cost < b.status.cost;
-      });
+    // Otherwise prioritize solution with lower cost
+    return a.status.cost < b.status.cost;
+  });
 }
 
 }  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp
index 66883fed98a..7bcff73587f 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp
@@ -3,7 +3,6 @@
 #pragma once
 
 #include <algorithm>
-#include <array>
 #include <concepts>
 #include <functional>
 #include <iterator>
@@ -20,10 +19,15 @@
 #include "sleipnir/optimization/SolverIterationInfo.hpp"
 #include "sleipnir/optimization/SolverStatus.hpp"
 #include "sleipnir/optimization/solver/InteriorPoint.hpp"
+#include "sleipnir/optimization/solver/Newton.hpp"
 #include "sleipnir/optimization/solver/SQP.hpp"
-#include "sleipnir/util/Print.hpp"
 #include "sleipnir/util/SymbolExports.hpp"
 
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+#include <array>
+#include "sleipnir/util/Print.hpp"
+#endif
+
 namespace sleipnir {
 
 /**
@@ -192,8 +196,8 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
 
     m_equalityConstraints.reserve(m_equalityConstraints.size() +
                                   constraint.constraints.size());
-    std::copy(constraint.constraints.begin(), constraint.constraints.end(),
-              std::back_inserter(m_equalityConstraints));
+    std::ranges::copy(constraint.constraints,
+                      std::back_inserter(m_equalityConstraints));
   }
 
   /**
@@ -211,8 +215,8 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
 
     m_equalityConstraints.reserve(m_equalityConstraints.size() +
                                   constraint.constraints.size());
-    std::copy(constraint.constraints.begin(), constraint.constraints.end(),
-              std::back_inserter(m_equalityConstraints));
+    std::ranges::copy(constraint.constraints,
+                      std::back_inserter(m_equalityConstraints));
   }
 
   /**
@@ -230,8 +234,8 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
 
     m_inequalityConstraints.reserve(m_inequalityConstraints.size() +
                                     constraint.constraints.size());
-    std::copy(constraint.constraints.begin(), constraint.constraints.end(),
-              std::back_inserter(m_inequalityConstraints));
+    std::ranges::copy(constraint.constraints,
+                      std::back_inserter(m_inequalityConstraints));
   }
 
   /**
@@ -249,8 +253,8 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
 
     m_inequalityConstraints.reserve(m_inequalityConstraints.size() +
                                     constraint.constraints.size());
-    std::copy(constraint.constraints.begin(), constraint.constraints.end(),
-              std::back_inserter(m_inequalityConstraints));
+    std::ranges::copy(constraint.constraints,
+                      std::back_inserter(m_inequalityConstraints));
   }
 
   /**
@@ -273,6 +277,7 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
       m_f = Variable();
     }
 
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
     if (config.diagnostics) {
       constexpr std::array kExprTypeToName{"empty", "constant", "linear",
                                            "quadratic", "nonlinear"};
@@ -297,6 +302,7 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
       sleipnir::println("Number of inequality constraints: {}\n",
                         m_inequalityConstraints.size());
     }
+#endif
 
     // If the problem is empty or constant, there's nothing to do
     if (status.costFunctionType <= ExpressionType::kConstant &&
@@ -306,19 +312,22 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
     }
 
     // Solve the optimization problem
-    if (m_inequalityConstraints.empty()) {
-      SQP(m_decisionVariables, m_equalityConstraints, m_f.value(), m_callback,
+    if (m_equalityConstraints.empty() && m_inequalityConstraints.empty()) {
+      Newton(m_decisionVariables, m_f.value(), m_callbacks, config, x, &status);
+    } else if (m_inequalityConstraints.empty()) {
+      SQP(m_decisionVariables, m_equalityConstraints, m_f.value(), m_callbacks,
           config, x, &status);
     } else {
-      Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size());
       InteriorPoint(m_decisionVariables, m_equalityConstraints,
-                    m_inequalityConstraints, m_f.value(), m_callback, config,
-                    false, x, s, &status);
+                    m_inequalityConstraints, m_f.value(), m_callbacks, config,
+                    x, &status);
     }
 
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
     if (config.diagnostics) {
       sleipnir::println("Exit condition: {}", ToMessage(status.exitCondition));
     }
+#endif
 
     // Assign the solution to the original Variable instances
     VariableMatrix{m_decisionVariables}.SetValue(x);
@@ -327,7 +336,7 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
   }
 
   /**
-   * Sets a callback to be called at each solver iteration.
+   * Adds a callback to be called at each solver iteration.
    *
    * The callback for this overload should return void.
    *
@@ -337,16 +346,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
     requires requires(F callback, const SolverIterationInfo& info) {
       { callback(info) } -> std::same_as<void>;
     }
-  void Callback(F&& callback) {
-    m_callback = [=, callback = std::forward<F>(callback)](
-                     const SolverIterationInfo& info) {
+  void AddCallback(F&& callback) {
+    m_callbacks.emplace_back([=, callback = std::forward<F>(callback)](
+                                 const SolverIterationInfo& info) {
       callback(info);
       return false;
-    };
+    });
   }
 
   /**
-   * Sets a callback to be called at each solver iteration.
+   * Adds a callback to be called at each solver iteration.
    *
    * The callback for this overload should return bool.
    *
@@ -357,10 +366,15 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
     requires requires(F callback, const SolverIterationInfo& info) {
       { callback(info) } -> std::same_as<bool>;
     }
-  void Callback(F&& callback) {
-    m_callback = std::forward<F>(callback);
+  void AddCallback(F&& callback) {
+    m_callbacks.emplace_back(std::forward<F>(callback));
   }
 
+  /**
+   * Clears the registered callbacks.
+   */
+  void ClearCallbacks() { m_callbacks.clear(); }
+
  private:
   // The list of decision variables, which are the root of the problem's
   // expression tree
@@ -376,8 +390,8 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
   wpi::SmallVector<Variable> m_inequalityConstraints;
 
   // The user callback
-  std::function<bool(const SolverIterationInfo& info)> m_callback =
-      [](const SolverIterationInfo&) { return false; };
+  wpi::SmallVector<std::function<bool(const SolverIterationInfo& info)>>
+      m_callbacks;
 
   // The solver status
   SolverStatus status;
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp
index f7323f73881..eaec3338166 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp
@@ -42,6 +42,53 @@ struct SLEIPNIR_DLLEXPORT SolverConfig {
   bool feasibleIPM = false;
 
   /// Enables diagnostic prints.
+  ///
+  /// <table>
+  ///   <tr>
+  ///     <th>Heading</th>
+  ///     <th>Description</th>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>iter</td>
+  ///     <td>Iteration number</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>mode</td>
+  ///     <td>Iteration mode (normal, second-order correction aka SOC)</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>time (ms)</td>
+  ///     <td>Duration of iteration in milliseconds</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>error</td>
+  ///     <td>Error estimate</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>cost</td>
+  ///     <td>Cost function value at current iterate</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>infeasibility</td>
+  ///     <td>Constraint infeasibility at current iterate</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>reg</td>
+  ///     <td>Iteration matrix regularization</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>primal α</td>
+  ///     <td>Primal step size</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>dual α</td>
+  ///     <td>Dual step size</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>↩</td>
+  ///     <td>Number of line search backtracks</td>
+  ///   </tr>
+  /// </table>
   bool diagnostics = false;
 
   /// Enables writing sparsity patterns of H, Aₑ, and Aᵢ to files named H.spy,
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp
index 7d1445297e3..c267ed599f6 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp
@@ -24,19 +24,21 @@ enum class SolverExitCondition : int8_t {
   kTooFewDOFs = -1,
   /// The solver determined the problem to be locally infeasible and gave up.
   kLocallyInfeasible = -2,
-  /// The solver failed to reach the desired tolerance, and feasibility
-  /// restoration failed to converge.
-  kFeasibilityRestorationFailed = -3,
+  /// The linear system factorization failed.
+  kFactorizationFailed = -3,
+  /// The backtracking line search failed, and the problem isn't locally
+  /// infeasible.
+  kLineSearchFailed = -4,
   /// The solver encountered nonfinite initial cost or constraints and gave up.
-  kNonfiniteInitialCostOrConstraints = -4,
+  kNonfiniteInitialCostOrConstraints = -5,
   /// The solver encountered diverging primal iterates xₖ and/or sₖ and gave up.
-  kDivergingIterates = -5,
+  kDivergingIterates = -6,
   /// The solver returned its solution so far after exceeding the maximum number
   /// of iterations.
-  kMaxIterationsExceeded = -6,
+  kMaxIterationsExceeded = -7,
   /// The solver returned its solution so far after exceeding the maximum
   /// elapsed wall clock time.
-  kTimeout = -7
+  kTimeout = -8
 };
 
 /**
@@ -59,9 +61,11 @@ SLEIPNIR_DLLEXPORT constexpr std::string_view ToMessage(
       return "problem has too few degrees of freedom";
     case kLocallyInfeasible:
       return "problem is locally infeasible";
-    case kFeasibilityRestorationFailed:
-      return "solver failed to reach the desired tolerance, and feasibility "
-             "restoration failed to converge";
+    case kFactorizationFailed:
+      return "linear system factorization failed";
+    case kLineSearchFailed:
+      return "backtracking line search failed, and the problem isn't locally "
+             "infeasible";
     case kNonfiniteInitialCostOrConstraints:
       return "solver encountered nonfinite initial cost or constraints and "
              "gave up";
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp
index 51d8f973058..ff44c7efcab 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp
@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include <functional>
 #include <span>
 
 #include <Eigen/Core>
@@ -10,7 +11,6 @@
 #include "sleipnir/optimization/SolverConfig.hpp"
 #include "sleipnir/optimization/SolverIterationInfo.hpp"
 #include "sleipnir/optimization/SolverStatus.hpp"
-#include "sleipnir/util/FunctionRef.hpp"
 #include "sleipnir/util/SymbolExports.hpp"
 
 namespace sleipnir {
@@ -34,22 +34,17 @@ are the inequality constraints.
 @param[in] equalityConstraints The list of equality constraints.
 @param[in] inequalityConstraints The list of inequality constraints.
 @param[in] f The cost function.
-@param[in] callback The user callback.
+@param[in] callbacks The list of user callbacks.
 @param[in] config Configuration options for the solver.
-@param[in] feasibilityRestoration Whether to use feasibility restoration instead
-  of the normal algorithm.
 @param[in,out] x The initial guess and output location for the decision
   variables.
-@param[in,out] s The initial guess and output location for the inequality
-  constraint slack variables.
 @param[out] status The solver status.
 */
 SLEIPNIR_DLLEXPORT void InteriorPoint(
     std::span<Variable> decisionVariables,
     std::span<Variable> equalityConstraints,
     std::span<Variable> inequalityConstraints, Variable& f,
-    function_ref<bool(const SolverIterationInfo& info)> callback,
-    const SolverConfig& config, bool feasibilityRestoration, Eigen::VectorXd& x,
-    Eigen::VectorXd& s, SolverStatus* status);
+    std::span<std::function<bool(const SolverIterationInfo& info)>> callbacks,
+    const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status);
 
 }  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/Newton.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/Newton.hpp
new file mode 100644
index 00000000000..7608fe85906
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/Newton.hpp
@@ -0,0 +1,42 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <functional>
+#include <span>
+
+#include <Eigen/Core>
+
+#include "sleipnir/autodiff/Variable.hpp"
+#include "sleipnir/optimization/SolverConfig.hpp"
+#include "sleipnir/optimization/SolverIterationInfo.hpp"
+#include "sleipnir/optimization/SolverStatus.hpp"
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+Finds the optimal solution to a nonlinear program using Newton's method.
+
+A nonlinear program has the form:
+
+@verbatim
+     min_x f(x)
+@endverbatim
+
+where f(x) is the cost function.
+
+@param[in] decisionVariables The list of decision variables.
+@param[in] f The cost function.
+@param[in] callbacks The list of user callbacks.
+@param[in] config Configuration options for the solver.
+@param[in,out] x The initial guess and output location for the decision
+  variables.
+@param[out] status The solver status.
+*/
+SLEIPNIR_DLLEXPORT void Newton(
+    std::span<Variable> decisionVariables, Variable& f,
+    std::span<std::function<bool(const SolverIterationInfo& info)>> callbacks,
+    const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status);
+
+}  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp
index ed10ffedff2..82f3f225eba 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp
@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include <functional>
 #include <span>
 
 #include <Eigen/Core>
@@ -10,7 +11,6 @@
 #include "sleipnir/optimization/SolverConfig.hpp"
 #include "sleipnir/optimization/SolverIterationInfo.hpp"
 #include "sleipnir/optimization/SolverStatus.hpp"
-#include "sleipnir/util/FunctionRef.hpp"
 #include "sleipnir/util/SymbolExports.hpp"
 
 namespace sleipnir {
@@ -31,7 +31,7 @@ where f(x) is the cost function and cₑ(x) are the equality constraints.
 @param[in] decisionVariables The list of decision variables.
 @param[in] equalityConstraints The list of equality constraints.
 @param[in] f The cost function.
-@param[in] callback The user callback.
+@param[in] callbacks The list of user callbacks.
 @param[in] config Configuration options for the solver.
 @param[in,out] x The initial guess and output location for the decision
   variables.
@@ -40,7 +40,7 @@ where f(x) is the cost function and cₑ(x) are the equality constraints.
 SLEIPNIR_DLLEXPORT void SQP(
     std::span<Variable> decisionVariables,
     std::span<Variable> equalityConstraints, Variable& f,
-    function_ref<bool(const SolverIterationInfo& info)> callback,
+    std::span<std::function<bool(const SolverIterationInfo& info)>> callbacks,
     const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status);
 
 }  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp
index ba381ef8f48..ecbd5dcd5fb 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp
@@ -4,17 +4,19 @@
 
 #ifdef JORMUNGANDR
 #include <format>
+#include <source_location>
 #include <stdexcept>
 /**
  * Throw an exception in Python.
  */
-#define Assert(condition)                                                      \
-  do {                                                                         \
-    if (!(condition)) {                                                        \
-      throw std::invalid_argument(                                             \
-          std::format("{}:{}: {}: Assertion `{}' failed.", __FILE__, __LINE__, \
-                      __func__, #condition));                                  \
-    }                                                                          \
+#define Assert(condition)                                            \
+  do {                                                               \
+    if (!(condition)) {                                              \
+      auto location = std::source_location::current();               \
+      throw std::invalid_argument(std::format(                       \
+          "{}:{}: {}: Assertion `{}' failed.", location.file_name(), \
+          location.line(), location.function_name(), #condition));   \
+    }                                                                \
   } while (0);
 #else
 #include <cassert>
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp
index f1290e5837b..552f9e61d20 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp
@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include <concepts>
 #include <cstddef>
 #include <memory>
 #include <utility>
@@ -27,6 +28,9 @@ namespace sleipnir {
 template <typename T>
 class IntrusiveSharedPtr {
  public:
+  template <typename>
+  friend class IntrusiveSharedPtr;
+
   /**
    * Constructs an empty intrusive shared pointer.
    */
@@ -63,6 +67,19 @@ class IntrusiveSharedPtr {
     }
   }
 
+  /**
+   * Copy constructs from the given intrusive shared pointer.
+   */
+  template <typename U>
+    requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
+  constexpr IntrusiveSharedPtr(  // NOLINT
+      const IntrusiveSharedPtr<U>& rhs) noexcept
+      : m_ptr{rhs.m_ptr} {
+    if (m_ptr != nullptr) {
+      IntrusiveSharedPtrIncRefCount(m_ptr);
+    }
+  }
+
   /**
    * Makes a copy of the given intrusive shared pointer.
    */
@@ -85,12 +102,45 @@ class IntrusiveSharedPtr {
     return *this;
   }
 
+  /**
+   * Makes a copy of the given intrusive shared pointer.
+   */
+  template <typename U>
+    requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
+  constexpr IntrusiveSharedPtr<T>& operator=(  // NOLINT
+      const IntrusiveSharedPtr<U>& rhs) noexcept {
+    if (m_ptr == rhs.m_ptr) {
+      return *this;
+    }
+
+    if (m_ptr != nullptr) {
+      IntrusiveSharedPtrDecRefCount(m_ptr);
+    }
+
+    m_ptr = rhs.m_ptr;
+
+    if (m_ptr != nullptr) {
+      IntrusiveSharedPtrIncRefCount(m_ptr);
+    }
+
+    return *this;
+  }
+
   /**
    * Move constructs from the given intrusive shared pointer.
    */
   constexpr IntrusiveSharedPtr(IntrusiveSharedPtr<T>&& rhs) noexcept
       : m_ptr{std::exchange(rhs.m_ptr, nullptr)} {}
 
+  /**
+   * Move constructs from the given intrusive shared pointer.
+   */
+  template <typename U>
+    requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
+  constexpr IntrusiveSharedPtr(  // NOLINT
+      IntrusiveSharedPtr<U>&& rhs) noexcept
+      : m_ptr{std::exchange(rhs.m_ptr, nullptr)} {}
+
   /**
    * Move assigns from the given intrusive shared pointer.
    */
@@ -105,6 +155,22 @@ class IntrusiveSharedPtr {
     return *this;
   }
 
+  /**
+   * Move assigns from the given intrusive shared pointer.
+   */
+  template <typename U>
+    requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
+  constexpr IntrusiveSharedPtr<T>& operator=(
+      IntrusiveSharedPtr<U>&& rhs) noexcept {
+    if (m_ptr == rhs.m_ptr) {
+      return *this;
+    }
+
+    std::swap(m_ptr, rhs.m_ptr);
+
+    return *this;
+  }
+
   /**
    * Returns the internal pointer.
    */
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/ScopedProfiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/ScopedProfiler.hpp
new file mode 100644
index 00000000000..4d449600dfc
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/ScopedProfiler.hpp
@@ -0,0 +1,74 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <concepts>
+#include <utility>
+
+#include "sleipnir/util/SetupProfiler.hpp"
+#include "sleipnir/util/SolveProfiler.hpp"
+
+namespace sleipnir {
+
+/**
+ * Starts a profiler in the constructor and stops it in the destructor.
+ */
+template <typename Profiler>
+  requires std::same_as<Profiler, SetupProfiler> ||
+           std::same_as<Profiler, SolveProfiler>
+class ScopedProfiler {
+ public:
+  /**
+   * Starts a profiler.
+   *
+   * @param profiler The profiler.
+   */
+  explicit ScopedProfiler(Profiler& profiler) noexcept : m_profiler{&profiler} {
+    m_profiler->Start();
+  }
+
+  /**
+   * Stops a profiler.
+   */
+  ~ScopedProfiler() {
+    if (m_active) {
+      m_profiler->Stop();
+    }
+  }
+
+  /**
+   * Move constructor.
+   */
+  ScopedProfiler(ScopedProfiler&& rhs) noexcept
+      : m_profiler{std::move(rhs.m_profiler)}, m_active{rhs.m_active} {
+    rhs.m_active = false;
+  }
+
+  ScopedProfiler(const ScopedProfiler&) = delete;
+  ScopedProfiler& operator=(const ScopedProfiler&) = delete;
+
+  /**
+   * Stops the profiler.
+   *
+   * If this is called, the destructor is a no-op.
+   */
+  void Stop() {
+    if (m_active) {
+      m_profiler->Stop();
+      m_active = false;
+    }
+  }
+
+  /**
+   * The most recent solve duration in milliseconds as a double.
+   */
+  const std::chrono::duration<double>& CurrentDuration() const {
+    return m_profiler->CurrentDuration();
+  }
+
+ private:
+  Profiler* m_profiler;
+  bool m_active = true;
+};
+
+}  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SetupProfiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SetupProfiler.hpp
new file mode 100644
index 00000000000..581f71fb866
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SetupProfiler.hpp
@@ -0,0 +1,58 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <chrono>
+#include <string>
+#include <string_view>
+
+namespace sleipnir {
+
+/**
+ * Records the number of profiler measurements (start/stop pairs) and the
+ * average duration between each start and stop call.
+ */
+class SetupProfiler {
+ public:
+  std::string name;
+
+  /**
+   * Constructs a SetupProfiler.
+   *
+   * @param name Name of measurement to show in diagnostics.
+   */
+  explicit SetupProfiler(std::string_view name) : name{name} {}
+
+  /**
+   * Tell the profiler to start measuring setup time.
+   */
+  void Start() {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    m_setupStartTime = std::chrono::steady_clock::now();
+#endif
+  }
+
+  /**
+   * Tell the profiler to stop measuring setup time.
+   */
+  void Stop() {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    m_setupStopTime = std::chrono::steady_clock::now();
+    m_setupDuration = m_setupStopTime - m_setupStartTime;
+#endif
+  }
+
+  /**
+   * The setup duration in milliseconds as a double.
+   */
+  const std::chrono::duration<double>& Duration() const {
+    return m_setupDuration;
+  }
+
+ private:
+  std::chrono::steady_clock::time_point m_setupStartTime;
+  std::chrono::steady_clock::time_point m_setupStopTime;
+  std::chrono::duration<double> m_setupDuration{0.0};
+};
+
+}  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SolveProfiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SolveProfiler.hpp
new file mode 100644
index 00000000000..0ca242f4a7e
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SolveProfiler.hpp
@@ -0,0 +1,93 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <chrono>
+#include <string>
+#include <string_view>
+
+namespace sleipnir {
+
+/**
+ * Records the number of profiler measurements (start/stop pairs) and the
+ * average duration between each start and stop call.
+ */
+class SolveProfiler {
+ public:
+  std::string name;
+
+  /**
+   * Constructs a SolveProfiler.
+   */
+  SolveProfiler() = default;
+
+  /**
+   * Constructs a SolveProfiler.
+   *
+   * @param name Name of measurement to show in diagnostics.
+   */
+  explicit SolveProfiler(std::string_view name) : name{name} {}
+
+  /**
+   * Tell the profiler to start measuring solve time.
+   */
+  void Start() {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    m_currentSolveStartTime = std::chrono::steady_clock::now();
+#endif
+  }
+
+  /**
+   * Tell the profiler to stop measuring solve time, increment the number of
+   * averages, and incorporate the latest measurement into the average.
+   */
+  void Stop() {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    m_currentSolveStopTime = std::chrono::steady_clock::now();
+    m_currentSolveDuration = m_currentSolveStopTime - m_currentSolveStartTime;
+    m_totalSolveDuration += m_currentSolveDuration;
+
+    ++m_numSolves;
+    m_averageSolveDuration =
+        (m_numSolves - 1.0) / m_numSolves * m_averageSolveDuration +
+        1.0 / m_numSolves * m_currentSolveDuration;
+#endif
+  }
+
+  /**
+   * The number of solves.
+   */
+  int NumSolves() const { return m_numSolves; }
+
+  /**
+   * The most recent solve duration in milliseconds as a double.
+   */
+  const std::chrono::duration<double>& CurrentDuration() const {
+    return m_currentSolveDuration;
+  }
+
+  /**
+   * The average solve duration in milliseconds as a double.
+   */
+  const std::chrono::duration<double>& AverageDuration() const {
+    return m_averageSolveDuration;
+  }
+
+  /**
+   * The sum of all solve durations in milliseconds as a double.
+   */
+  const std::chrono::duration<double>& TotalDuration() const {
+    return m_totalSolveDuration;
+  }
+
+ private:
+  std::chrono::steady_clock::time_point m_currentSolveStartTime;
+  std::chrono::steady_clock::time_point m_currentSolveStopTime;
+  std::chrono::duration<double> m_currentSolveDuration{0.0};
+  std::chrono::duration<double> m_totalSolveDuration{0.0};
+
+  int m_numSolves = 0;
+  std::chrono::duration<double> m_averageSolveDuration{0.0};
+};
+
+}  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp
index 7f526a2d996..a32728d8433 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp
@@ -2,88 +2,122 @@
 
 #pragma once
 
+#include <stdint.h>
+
+#include <bit>
 #include <fstream>
 #include <string>
 #include <string_view>
 
 #include <Eigen/SparseCore>
 #include <wpi/SmallVector.h>
+#include <wpi/bit.h>
 
 #include "sleipnir/util/SymbolExports.hpp"
 
 namespace sleipnir {
 
 /**
- * Write the sparsity pattern of a sparse matrix to a file.
+ * Writes the sparsity pattern of a sparse matrix to a file.
+ *
+ * Each file represents the sparsity pattern of one matrix over time. <a
+ * href="https://github.com/SleipnirGroup/Sleipnir/blob/main/tools/spy.py">spy.py</a>
+ * can display it as an animation.
  *
- * Each character represents an element with '.' representing zero, '+'
- * representing positive, and '-' representing negative. Here's an example for a
- * 3x3 identity matrix.
+ * The file starts with the following header:
+ * <ol>
+ *   <li>Plot title (length as a little-endian int32, then characters)</li>
+ *   <li>Row label (length as a little-endian int32, then characters)</li>
+ *   <li>Column label (length as a little-endian int32, then characters)</li>
+ * </ol>
  *
- * "+.."
- * ".+."
- * "..+"
+ * Then, each sparsity pattern starts with:
+ * <ol>
+ *   <li>Number of coordinates as a little-endian int32</li>
+ * </ol>
+ *
+ * followed by that many coordinates in the following format:
+ * <ol>
+ *   <li>Row index as a little-endian int32</li>
+ *   <li>Column index as a little-endian int32</li>
+ *   <li>Sign as a character ('+' for positive, '-' for negative, or '0' for
+ *       zero)</li>
+ * </ol>
  *
  * @param[out] file A file stream.
  * @param[in] mat The sparse matrix.
  */
-SLEIPNIR_DLLEXPORT inline void Spy(std::ostream& file,
-                                   const Eigen::SparseMatrix<double>& mat) {
-  const int cells_width = mat.cols() + 1;
-  const int cells_height = mat.rows();
+class SLEIPNIR_DLLEXPORT Spy {
+ public:
+  /**
+   * Constructs a Spy instance.
+   *
+   * @param filename The filename.
+   * @param title Plot title.
+   * @param rowLabel Row label.
+   * @param colLabel Column label.
+   * @param rows The sparse matrix's number of rows.
+   * @param cols The sparse matrix's number of columns.
+   */
+  Spy(std::string_view filename, std::string_view title,
+      std::string_view rowLabel, std::string_view colLabel, int rows, int cols)
+      : m_file{std::string{filename}, std::ios::binary} {
+    // Write title
+    Write32le(title.size());
+    m_file.write(title.data(), title.size());
 
-  wpi::SmallVector<uint8_t> cells;
+    // Write row label
+    Write32le(rowLabel.size());
+    m_file.write(rowLabel.data(), rowLabel.size());
 
-  // Allocate space for matrix of characters plus trailing newlines
-  cells.reserve(cells_width * cells_height);
+    // Write column label
+    Write32le(colLabel.size());
+    m_file.write(colLabel.data(), colLabel.size());
 
-  // Initialize cell array
-  for (int row = 0; row < mat.rows(); ++row) {
-    for (int col = 0; col < mat.cols(); ++col) {
-      cells.emplace_back('.');
-    }
-    cells.emplace_back('\n');
+    // Write row and column counts
+    Write32le(rows);
+    Write32le(cols);
   }
 
-  // Fill in non-sparse entries
-  for (int k = 0; k < mat.outerSize(); ++k) {
-    for (Eigen::SparseMatrix<double>::InnerIterator it{mat, k}; it; ++it) {
-      if (it.value() < 0.0) {
-        cells[it.row() * cells_width + it.col()] = '-';
-      } else if (it.value() > 0.0) {
-        cells[it.row() * cells_width + it.col()] = '+';
+  /**
+   * Adds a matrix to the file.
+   *
+   * @param mat The matrix.
+   */
+  void Add(const Eigen::SparseMatrix<double>& mat) {
+    // Write number of coordinates
+    Write32le(mat.nonZeros());
+
+    // Write coordinates
+    for (int k = 0; k < mat.outerSize(); ++k) {
+      for (Eigen::SparseMatrix<double>::InnerIterator it{mat, k}; it; ++it) {
+        Write32le(it.row());
+        Write32le(it.col());
+        if (it.value() > 0.0) {
+          m_file << '+';
+        } else if (it.value() < 0.0) {
+          m_file << '-';
+        } else {
+          m_file << '0';
+        }
       }
     }
   }
 
-  // Write cell array to file
-  for (const auto& c : cells) {
-    file << c;
-  }
-}
+ private:
+  std::ofstream m_file;
 
-/**
- * Write the sparsity pattern of a sparse matrix to a file.
- *
- * Each character represents an element with "." representing zero, "+"
- * representing positive, and "-" representing negative. Here's an example for a
- * 3x3 identity matrix.
- *
- * "+.."
- * ".+."
- * "..+"
- *
- * @param[in] filename The filename.
- * @param[in] mat The sparse matrix.
- */
-SLEIPNIR_DLLEXPORT inline void Spy(std::string_view filename,
-                                   const Eigen::SparseMatrix<double>& mat) {
-  std::ofstream file{std::string{filename}};
-  if (!file.is_open()) {
-    return;
+  /**
+   * Writes a 32-bit signed integer to the file as little-endian.
+   *
+   * @param num A 32-bit signed integer.
+   */
+  void Write32le(int32_t num) {
+    if constexpr (std::endian::native != std::endian::little) {
+      num = wpi::byteswap(num);
+    }
+    m_file.write(reinterpret_cast<char*>(&num), sizeof(num));
   }
-
-  Spy(file, mat);
-}
+};
 
 }  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp
index 589ff8822aa..c1a103f1344 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp
@@ -24,20 +24,20 @@ VariableMatrix Solve(const VariableMatrix& A, const VariableMatrix& B) {
     const auto& c = A(1, 0);
     const auto& d = A(1, 1);
 
-    sleipnir::VariableMatrix Ainv{{d, -b}, {-c, a}};
+    sleipnir::VariableMatrix adjA{{d, -b}, {-c, a}};
     auto detA = a * d - b * c;
-    Ainv /= detA;
-
-    return Ainv * B;
+    return adjA / detA * B;
   } else if (A.Rows() == 3 && A.Cols() == 3) {
     // Compute optimal inverse instead of using Eigen's general solver
     //
     // [a  b  c]⁻¹
     // [d  e  f]
     // [g  h  i]
-    //                       1                 [ei − fh  ch − bi  bf − ce]
-    //     = --------------------------------- [fg − di  ai − cg  cd − af]
-    //       aei − afh − bdi + bfg + cdh − ceg [dh − eg  bg − ah  ae − bd]
+    //                        1                   [ei − fh  ch − bi  bf − ce]
+    //     = ------------------------------------ [fg − di  ai − cg  cd − af]
+    //       a(ei − fh) + b(fg − di) + c(dh − eg) [dh − eg  bg − ah  ae − bd]
+    //
+    // https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%7D%2C+%7Bd%2C+e%2C+f%7D%2C+%7Bg%2C+h%2C+i%7D%7D
 
     const auto& a = A(0, 0);
     const auto& b = A(0, 1);
@@ -49,15 +49,183 @@ VariableMatrix Solve(const VariableMatrix& A, const VariableMatrix& B) {
     const auto& h = A(2, 1);
     const auto& i = A(2, 2);
 
-    sleipnir::VariableMatrix Ainv{
-        {e * i - f * h, c * h - b * i, b * f - c * e},
-        {f * g - d * i, a * i - c * g, c * d - a * f},
-        {d * h - e * g, b * g - a * h, a * e - b * d}};
-    auto detA =
-        a * e * i - a * f * h - b * d * i + b * f * g + c * d * h - c * e * g;
-    Ainv /= detA;
+    auto ae = a * e;
+    auto af = a * f;
+    auto ah = a * h;
+    auto ai = a * i;
+    auto bd = b * d;
+    auto bf = b * f;
+    auto bg = b * g;
+    auto bi = b * i;
+    auto cd = c * d;
+    auto ce = c * e;
+    auto cg = c * g;
+    auto ch = c * h;
+    auto dh = d * h;
+    auto di = d * i;
+    auto eg = e * g;
+    auto ei = e * i;
+    auto fg = f * g;
+    auto fh = f * h;
+
+    auto adjA00 = ei - fh;
+    auto adjA10 = fg - di;
+    auto adjA20 = dh - eg;
+
+    sleipnir::VariableMatrix adjA{{adjA00, ch - bi, bf - ce},
+                                  {adjA10, ai - cg, cd - af},
+                                  {adjA20, bg - ah, ae - bd}};
+    auto detA = a * adjA00 + b * adjA10 + c * adjA20;
+    return adjA / detA * B;
+  } else if (A.Rows() == 4 && A.Cols() == 4) {
+    // Compute optimal inverse instead of using Eigen's general solver
+    //
+    // [a  b  c  d]⁻¹
+    // [e  f  g  h]
+    // [i  j  k  l]
+    // [m  n  o  p]
+    //
+    // https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%2C+d%7D%2C+%7Be%2C+f%2C+g%2C+h%7D%2C+%7Bi%2C+j%2C+k%2C+l%7D%2C+%7Bm%2C+n%2C+o%2C+p%7D%7D
+
+    const auto& a = A(0, 0);
+    const auto& b = A(0, 1);
+    const auto& c = A(0, 2);
+    const auto& d = A(0, 3);
+    const auto& e = A(1, 0);
+    const auto& f = A(1, 1);
+    const auto& g = A(1, 2);
+    const auto& h = A(1, 3);
+    const auto& i = A(2, 0);
+    const auto& j = A(2, 1);
+    const auto& k = A(2, 2);
+    const auto& l = A(2, 3);
+    const auto& m = A(3, 0);
+    const auto& n = A(3, 1);
+    const auto& o = A(3, 2);
+    const auto& p = A(3, 3);
+
+    auto afk = a * f * k;
+    auto afl = a * f * l;
+    auto afo = a * f * o;
+    auto afp = a * f * p;
+    auto agj = a * g * j;
+    auto agl = a * g * l;
+    auto agn = a * g * n;
+    auto agp = a * g * p;
+    auto ahj = a * h * j;
+    auto ahk = a * h * k;
+    auto ahn = a * h * n;
+    auto aho = a * h * o;
+    auto ajo = a * j * o;
+    auto ajp = a * j * p;
+    auto akn = a * k * n;
+    auto akp = a * k * p;
+    auto aln = a * l * n;
+    auto alo = a * l * o;
+    auto bek = b * e * k;
+    auto bel = b * e * l;
+    auto beo = b * e * o;
+    auto bep = b * e * p;
+    auto bgi = b * g * i;
+    auto bgl = b * g * l;
+    auto bgm = b * g * m;
+    auto bgp = b * g * p;
+    auto bhi = b * h * i;
+    auto bhk = b * h * k;
+    auto bhm = b * h * m;
+    auto bho = b * h * o;
+    auto bio = b * i * o;
+    auto bip = b * i * p;
+    auto bjp = b * j * p;
+    auto bkm = b * k * m;
+    auto bkp = b * k * p;
+    auto blm = b * l * m;
+    auto blo = b * l * o;
+    auto cej = c * e * j;
+    auto cel = c * e * l;
+    auto cen = c * e * n;
+    auto cep = c * e * p;
+    auto cfi = c * f * i;
+    auto cfl = c * f * l;
+    auto cfm = c * f * m;
+    auto cfp = c * f * p;
+    auto chi = c * h * i;
+    auto chj = c * h * j;
+    auto chm = c * h * m;
+    auto chn = c * h * n;
+    auto cin = c * i * n;
+    auto cip = c * i * p;
+    auto cjm = c * j * m;
+    auto cjp = c * j * p;
+    auto clm = c * l * m;
+    auto cln = c * l * n;
+    auto dej = d * e * j;
+    auto dek = d * e * k;
+    auto den = d * e * n;
+    auto deo = d * e * o;
+    auto dfi = d * f * i;
+    auto dfk = d * f * k;
+    auto dfm = d * f * m;
+    auto dfo = d * f * o;
+    auto dgi = d * g * i;
+    auto dgj = d * g * j;
+    auto dgm = d * g * m;
+    auto dgn = d * g * n;
+    auto din = d * i * n;
+    auto dio = d * i * o;
+    auto djm = d * j * m;
+    auto djo = d * j * o;
+    auto dkm = d * k * m;
+    auto dkn = d * k * n;
+    auto ejo = e * j * o;
+    auto ejp = e * j * p;
+    auto ekn = e * k * n;
+    auto ekp = e * k * p;
+    auto eln = e * l * n;
+    auto elo = e * l * o;
+    auto fio = f * i * o;
+    auto fip = f * i * p;
+    auto fkm = f * k * m;
+    auto fkp = f * k * p;
+    auto flm = f * l * m;
+    auto flo = f * l * o;
+    auto gin = g * i * n;
+    auto gip = g * i * p;
+    auto gjm = g * j * m;
+    auto gjp = g * j * p;
+    auto glm = g * l * m;
+    auto gln = g * l * n;
+    auto hin = h * i * n;
+    auto hio = h * i * o;
+    auto hjm = h * j * m;
+    auto hjo = h * j * o;
+    auto hkm = h * k * m;
+    auto hkn = h * k * n;
+
+    auto adjA00 = fkp - flo - gjp + gln + hjo - hkn;
+    auto adjA01 = -bkp + blo + cjp - cln - djo + dkn;
+    auto adjA02 = bgp - bho - cfp + chn + dfo - dgn;
+    auto adjA03 = -bgl + bhk + cfl - chj - dfk + dgj;
+    auto adjA10 = -ekp + elo + gip - glm - hio + hkm;
+    auto adjA11 = akp - alo - cip + clm + dio - dkm;
+    auto adjA12 = -agp + aho + cep - chm - deo + dgm;
+    auto adjA13 = agl - ahk - cel + chi + dek - dgi;
+    auto adjA20 = ejp - eln - fip + flm + hin - hjm;
+    auto adjA21 = -ajp + aln + bip - blm - din + djm;
+    auto adjA22 = afp - ahn - bep + bhm + den - dfm;
+    auto adjA23 = -afl + ahj + bel - bhi - dej + dfi;
+    auto adjA30 = -ejo + ekn + fio - fkm - gin + gjm;
+    // NOLINTNEXTLINE
+    auto adjA31 = ajo - akn - bio + bkm + cin - cjm;
+    auto adjA32 = -afo + agn + beo - bgm - cen + cfm;
+    auto adjA33 = afk - agj - bek + bgi + cej - cfi;
 
-    return Ainv * B;
+    sleipnir::VariableMatrix adjA{{adjA00, adjA01, adjA02, adjA03},
+                                  {adjA10, adjA11, adjA12, adjA13},
+                                  {adjA20, adjA21, adjA22, adjA23},
+                                  {adjA30, adjA31, adjA32, adjA33}};
+    auto detA = a * adjA00 + b * adjA10 + c * adjA20 + d * adjA30;
+    return adjA / detA * B;
   } else {
     using MatrixXv = Eigen::Matrix<Variable, Eigen::Dynamic, Eigen::Dynamic>;
 
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp
index d2488b79987..0781926de1f 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp
@@ -103,9 +103,8 @@ class RegularizedLDLT {
       } else {
         δ *= 10.0;
 
-        // If the Hessian perturbation is too high, report failure. This can
-        // happen due to a rank-deficient equality constraint Jacobian with
-        // linearly dependent constraints.
+        // If the Hessian perturbation is too high, report failure. This can be
+        // caused by ill-conditioning.
         if (δ > 1e20) {
           m_info = Eigen::NumericalIssue;
           return;
@@ -115,7 +114,7 @@ class RegularizedLDLT {
   }
 
   /**
-   * Solve the system of equations using a regularized LDLT factorization.
+   * Solves the system of equations using a regularized LDLT factorization.
    *
    * @param rhs Right-hand side of the system.
    */
@@ -124,6 +123,11 @@ class RegularizedLDLT {
     return m_solver.solve(rhs);
   }
 
+  /**
+   * Returns the Hessian regularization factor.
+   */
+  double HessianRegularization() const { return m_δOld; }
+
  private:
   Solver m_solver;
 
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp
index d3981c59d16..0abc539fed8 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp
@@ -5,15 +5,16 @@
 #include <algorithm>
 #include <chrono>
 #include <cmath>
-#include <fstream>
+#include <functional>
 #include <limits>
+#include <memory>
+#include <ranges>
 
 #include <Eigen/SparseCholesky>
 #include <wpi/SmallVector.h>
 
 #include "optimization/RegularizedLDLT.hpp"
 #include "optimization/solver/util/ErrorEstimate.hpp"
-#include "optimization/solver/util/FeasibilityRestoration.hpp"
 #include "optimization/solver/util/Filter.hpp"
 #include "optimization/solver/util/FractionToTheBoundaryRule.hpp"
 #include "optimization/solver/util/IsLocallyInfeasible.hpp"
@@ -22,10 +23,16 @@
 #include "sleipnir/autodiff/Hessian.hpp"
 #include "sleipnir/autodiff/Jacobian.hpp"
 #include "sleipnir/optimization/SolverExitCondition.hpp"
+#include "sleipnir/util/ScopedProfiler.hpp"
+#include "sleipnir/util/SetupProfiler.hpp"
+#include "sleipnir/util/SolveProfiler.hpp"
+#include "util/ScopeExit.hpp"
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
 #include "sleipnir/util/Print.hpp"
 #include "sleipnir/util/Spy.hpp"
-#include "util/ScopeExit.hpp"
-#include "util/ToMilliseconds.hpp"
+#include "util/PrintDiagnostics.hpp"
+#endif
 
 // See docs/algorithms.md#Works_cited for citation definitions.
 //
@@ -34,14 +41,18 @@
 
 namespace sleipnir {
 
-void InteriorPoint(std::span<Variable> decisionVariables,
-                   std::span<Variable> equalityConstraints,
-                   std::span<Variable> inequalityConstraints, Variable& f,
-                   function_ref<bool(const SolverIterationInfo& info)> callback,
-                   const SolverConfig& config, bool feasibilityRestoration,
-                   Eigen::VectorXd& x, Eigen::VectorXd& s,
-                   SolverStatus* status) {
-  const auto solveStartTime = std::chrono::system_clock::now();
+void InteriorPoint(
+    std::span<Variable> decisionVariables,
+    std::span<Variable> equalityConstraints,
+    std::span<Variable> inequalityConstraints, Variable& f,
+    std::span<std::function<bool(const SolverIterationInfo& info)>> callbacks,
+    const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
+  const auto solveStartTime = std::chrono::steady_clock::now();
+
+  wpi::SmallVector<SetupProfiler> setupProfilers;
+  setupProfilers.emplace_back("setup").Start();
+
+  setupProfilers.emplace_back("  ↳ s,y,z setup").Start();
 
   // Map decision variables and constraints to VariableMatrices for Lagrangian
   VariableMatrix xAD{decisionVariables};
@@ -51,7 +62,9 @@ void InteriorPoint(std::span<Variable> decisionVariables,
 
   // Create autodiff variables for s, y, and z for Lagrangian
   VariableMatrix sAD(inequalityConstraints.size());
-  sAD.SetValue(s);
+  for (auto& s : sAD) {
+    s.SetValue(1.0);
+  }
   VariableMatrix yAD(equalityConstraints.size());
   for (auto& y : yAD) {
     y.SetValue(0.0);
@@ -61,11 +74,17 @@ void InteriorPoint(std::span<Variable> decisionVariables,
     z.SetValue(1.0);
   }
 
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ L setup").Start();
+
   // Lagrangian L
   //
   // L(xₖ, sₖ, yₖ, zₖ) = f(xₖ) − yₖᵀcₑ(xₖ) − zₖᵀ(cᵢ(xₖ) − sₖ)
   auto L = f - (yAD.T() * c_eAD)(0) - (zAD.T() * (c_iAD - sAD))(0);
 
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∂cₑ/∂x setup").Start();
+
   // Equality constraint Jacobian Aₑ
   //
   //         [∇ᵀcₑ₁(xₖ)]
@@ -73,8 +92,15 @@ void InteriorPoint(std::span<Variable> decisionVariables,
   //         [    ⋮    ]
   //         [∇ᵀcₑₘ(xₖ)]
   Jacobian jacobianCe{c_eAD, xAD};
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∂cₑ/∂x init solve").Start();
+
   Eigen::SparseMatrix<double> A_e = jacobianCe.Value();
 
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∂cᵢ/∂x setup").Start();
+
   // Inequality constraint Jacobian Aᵢ
   //
   //         [∇ᵀcᵢ₁(xₖ)]
@@ -82,18 +108,40 @@ void InteriorPoint(std::span<Variable> decisionVariables,
   //         [    ⋮    ]
   //         [∇ᵀcᵢₘ(xₖ)]
   Jacobian jacobianCi{c_iAD, xAD};
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∂cᵢ/∂x init solve").Start();
+
   Eigen::SparseMatrix<double> A_i = jacobianCi.Value();
 
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∇f(x) setup").Start();
+
   // Gradient of f ∇f
   Gradient gradientF{f, xAD};
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∇f(x) init solve").Start();
+
   Eigen::SparseVector<double> g = gradientF.Value();
 
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∇²ₓₓL setup").Start();
+
   // Hessian of the Lagrangian H
   //
   // Hₖ = ∇²ₓₓL(xₖ, sₖ, yₖ, zₖ)
   Hessian hessianL{L, xAD};
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∇²ₓₓL init solve").Start();
+
   Eigen::SparseMatrix<double> H = hessianL.Value();
 
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ precondition ✓").Start();
+
+  Eigen::VectorXd s = sAD.Value();
   Eigen::VectorXd y = yAD.Value();
   Eigen::VectorXd z = zAD.Value();
   Eigen::VectorXd c_e = c_eAD.Value();
@@ -101,6 +149,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
 
   // Check for overconstrained problem
   if (equalityConstraints.size() > decisionVariables.size()) {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
     if (config.diagnostics) {
       sleipnir::println("The problem has too few degrees of freedom.");
       sleipnir::println(
@@ -111,6 +160,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
         }
       }
     }
+#endif
 
     status->exitCondition = SolverExitCondition::kTooFewDOFs;
     return;
@@ -123,66 +173,31 @@ void InteriorPoint(std::span<Variable> decisionVariables,
     return;
   }
 
+  setupProfilers.back().Stop();
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
   // Sparsity pattern files written when spy flag is set in SolverConfig
-  std::ofstream H_spy;
-  std::ofstream A_e_spy;
-  std::ofstream A_i_spy;
+  std::unique_ptr<Spy> H_spy;
+  std::unique_ptr<Spy> A_e_spy;
+  std::unique_ptr<Spy> A_i_spy;
+  std::unique_ptr<Spy> lhs_spy;
   if (config.spy) {
-    A_e_spy.open("A_e.spy");
-    A_i_spy.open("A_i.spy");
-    H_spy.open("H.spy");
-  }
-
-  if (config.diagnostics && !feasibilityRestoration) {
-    sleipnir::println("Error tolerance: {}\n", config.tolerance);
+    H_spy = std::make_unique<Spy>("H.spy", "Hessian", "Decision variables",
+                                  "Decision variables", H.rows(), H.cols());
+    A_e_spy = std::make_unique<Spy>("A_e.spy", "Equality constraint Jacobian",
+                                    "Constraints", "Decision variables",
+                                    A_e.rows(), A_e.cols());
+    A_i_spy = std::make_unique<Spy>("A_i.spy", "Inequality constraint Jacobian",
+                                    "Constraints", "Decision variables",
+                                    A_i.rows(), A_i.cols());
+    lhs_spy = std::make_unique<Spy>(
+        "lhs.spy", "Newton-KKT system left-hand side", "Rows", "Columns",
+        H.rows() + A_e.rows(), H.cols() + A_e.rows());
   }
-
-  std::chrono::system_clock::time_point iterationsStartTime;
+#endif
 
   int iterations = 0;
 
-  // Prints final diagnostics when the solver exits
-  scope_exit exit{[&] {
-    status->cost = f.Value();
-
-    if (config.diagnostics && !feasibilityRestoration) {
-      auto solveEndTime = std::chrono::system_clock::now();
-
-      sleipnir::println("\nSolve time: {:.3f} ms",
-                        ToMilliseconds(solveEndTime - solveStartTime));
-      sleipnir::println("  ↳ {:.3f} ms (solver setup)",
-                        ToMilliseconds(iterationsStartTime - solveStartTime));
-      if (iterations > 0) {
-        sleipnir::println(
-            "  ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)",
-            ToMilliseconds(solveEndTime - iterationsStartTime), iterations,
-            ToMilliseconds((solveEndTime - iterationsStartTime) / iterations));
-      }
-      sleipnir::println("");
-
-      sleipnir::println("{:^8}   {:^10}   {:^14}   {:^6}", "autodiff",
-                        "setup (ms)", "avg solve (ms)", "solves");
-      sleipnir::println("{:=^47}", "");
-      constexpr auto format = "{:^8}   {:10.3f}   {:14.3f}   {:6}";
-      sleipnir::println(format, "∇f(x)",
-                        gradientF.GetProfiler().SetupDuration(),
-                        gradientF.GetProfiler().AverageSolveDuration(),
-                        gradientF.GetProfiler().SolveMeasurements());
-      sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(),
-                        hessianL.GetProfiler().AverageSolveDuration(),
-                        hessianL.GetProfiler().SolveMeasurements());
-      sleipnir::println(format, "∂cₑ/∂x",
-                        jacobianCe.GetProfiler().SetupDuration(),
-                        jacobianCe.GetProfiler().AverageSolveDuration(),
-                        jacobianCe.GetProfiler().SolveMeasurements());
-      sleipnir::println(format, "∂cᵢ/∂x",
-                        jacobianCi.GetProfiler().SetupDuration(),
-                        jacobianCi.GetProfiler().AverageSolveDuration(),
-                        jacobianCi.GetProfiler().SolveMeasurements());
-      sleipnir::println("");
-    }
-  }};
-
   // Barrier parameter minimum
   const double μ_min = config.tolerance / 10.0;
 
@@ -232,6 +247,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
 
   // Variables for determining when a step is acceptable
   constexpr double α_red_factor = 0.5;
+  constexpr double α_min = 1e-20;
   int acceptableIterCounter = 0;
 
   int fullStepRejectedCounter = 0;
@@ -240,19 +256,87 @@ void InteriorPoint(std::span<Variable> decisionVariables,
   // Error estimate
   double E_0 = std::numeric_limits<double>::infinity();
 
+  setupProfilers[0].Stop();
+
+  wpi::SmallVector<SolveProfiler> solveProfilers;
+  solveProfilers.emplace_back("solve");
+  solveProfilers.emplace_back("  ↳ feasibility ✓");
+  solveProfilers.emplace_back("  ↳ user callbacks");
+  solveProfilers.emplace_back("  ↳ iter matrix build");
+  solveProfilers.emplace_back("  ↳ iter matrix solve");
+  solveProfilers.emplace_back("  ↳ line search");
+  solveProfilers.emplace_back("    ↳ SOC");
+  solveProfilers.emplace_back("  ↳ spy writes");
+  solveProfilers.emplace_back("  ↳ next iter prep");
+
+  auto& innerIterProf = solveProfilers[0];
+  auto& feasibilityCheckProf = solveProfilers[1];
+  auto& userCallbacksProf = solveProfilers[2];
+  auto& linearSystemBuildProf = solveProfilers[3];
+  auto& linearSystemSolveProf = solveProfilers[4];
+  auto& lineSearchProf = solveProfilers[5];
+  auto& socProf = solveProfilers[6];
+  [[maybe_unused]]
+  auto& spyWritesProf = solveProfilers[7];
+  auto& nextIterPrepProf = solveProfilers[8];
+
+  // Prints final diagnostics when the solver exits
+  scope_exit exit{[&] {
+    status->cost = f.Value();
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    if (config.diagnostics) {
+      // Append gradient profilers
+      solveProfilers.push_back(gradientF.GetProfilers()[0]);
+      solveProfilers.back().name = "  ↳ ∇f(x)";
+      for (const auto& profiler :
+           gradientF.GetProfilers() | std::views::drop(1)) {
+        solveProfilers.push_back(profiler);
+      }
+
+      // Append Hessian profilers
+      solveProfilers.push_back(hessianL.GetProfilers()[0]);
+      solveProfilers.back().name = "  ↳ ∇²ₓₓL";
+      for (const auto& profiler :
+           hessianL.GetProfilers() | std::views::drop(1)) {
+        solveProfilers.push_back(profiler);
+      }
+
+      // Append equality constraint Jacobian profilers
+      solveProfilers.push_back(jacobianCe.GetProfilers()[0]);
+      solveProfilers.back().name = "  ↳ ∂cₑ/∂x";
+      for (const auto& profiler :
+           jacobianCe.GetProfilers() | std::views::drop(1)) {
+        solveProfilers.push_back(profiler);
+      }
+
+      // Append inequality constraint Jacobian profilers
+      solveProfilers.push_back(jacobianCi.GetProfilers()[0]);
+      solveProfilers.back().name = "  ↳ ∂cᵢ/∂x";
+      for (const auto& profiler :
+           jacobianCi.GetProfilers() | std::views::drop(1)) {
+        solveProfilers.push_back(profiler);
+      }
+
+      PrintFinalDiagnostics(iterations, setupProfilers, solveProfilers);
+    }
+#endif
+  }};
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
   if (config.diagnostics) {
-    iterationsStartTime = std::chrono::system_clock::now();
+    sleipnir::println("Error tolerance: {}\n", config.tolerance);
   }
+#endif
 
   while (E_0 > config.tolerance &&
          acceptableIterCounter < config.maxAcceptableIterations) {
-    std::chrono::system_clock::time_point innerIterStartTime;
-    if (config.diagnostics) {
-      innerIterStartTime = std::chrono::system_clock::now();
-    }
+    ScopedProfiler innerIterProfiler{innerIterProf};
+    ScopedProfiler feasibilityCheckProfiler{feasibilityCheckProf};
 
     // Check for local equality constraint infeasibility
     if (IsEqualityLocallyInfeasible(A_e, c_e)) {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
       if (config.diagnostics) {
         sleipnir::println(
             "The problem is locally infeasible due to violated equality "
@@ -265,6 +349,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
           }
         }
       }
+#endif
 
       status->exitCondition = SolverExitCondition::kLocallyInfeasible;
       return;
@@ -272,6 +357,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
 
     // Check for local inequality constraint infeasibility
     if (IsInequalityLocallyInfeasible(A_i, c_i)) {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
       if (config.diagnostics) {
         sleipnir::println(
             "The problem is infeasible due to violated inequality "
@@ -284,6 +370,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
           }
         }
       }
+#endif
 
       status->exitCondition = SolverExitCondition::kLocallyInfeasible;
       return;
@@ -296,44 +383,34 @@ void InteriorPoint(std::span<Variable> decisionVariables,
       return;
     }
 
-    // Write out spy file contents if that's enabled
-    if (config.spy) {
-      // Gap between sparsity patterns
-      if (iterations > 0) {
-        A_e_spy << "\n";
-        A_i_spy << "\n";
-        H_spy << "\n";
-      }
+    feasibilityCheckProfiler.Stop();
+    ScopedProfiler userCallbacksProfiler{userCallbacksProf};
 
-      Spy(H_spy, H);
-      Spy(A_e_spy, A_e);
-      Spy(A_i_spy, A_i);
+    // Call user callbacks
+    for (const auto& callback : callbacks) {
+      if (callback({iterations, x, s, g, H, A_e, A_i})) {
+        status->exitCondition = SolverExitCondition::kCallbackRequestedStop;
+        return;
+      }
     }
 
-    // Call user callback
-    if (callback({iterations, x, s, g, H, A_e, A_i})) {
-      status->exitCondition = SolverExitCondition::kCallbackRequestedStop;
-      return;
-    }
+    userCallbacksProfiler.Stop();
+    ScopedProfiler linearSystemBuildProfiler{linearSystemBuildProf};
 
     //     [s₁ 0 ⋯ 0 ]
     // S = [0  ⋱   ⋮ ]
     //     [⋮    ⋱ 0 ]
     //     [0  ⋯ 0 sₘ]
-    const auto S = s.asDiagonal();
-    Eigen::SparseMatrix<double> Sinv;
-    Sinv = s.cwiseInverse().asDiagonal();
-
+    //
     //     [z₁ 0 ⋯ 0 ]
     // Z = [0  ⋱   ⋮ ]
     //     [⋮    ⋱ 0 ]
     //     [0  ⋯ 0 zₘ]
-    const auto Z = z.asDiagonal();
-    Eigen::SparseMatrix<double> Zinv;
-    Zinv = z.cwiseInverse().asDiagonal();
-
+    //
     // Σ = S⁻¹Z
-    const Eigen::SparseMatrix<double> Σ = Sinv * Z;
+    Eigen::SparseMatrix<double> Sinv;
+    Sinv = s.cwiseInverse().asDiagonal();
+    const Eigen::SparseMatrix<double> Σ = Sinv * z.asDiagonal();
 
     // lhs = [H + AᵢᵀΣAᵢ  Aₑᵀ]
     //       [    Aₑ       0 ]
@@ -363,50 +440,59 @@ void InteriorPoint(std::span<Variable> decisionVariables,
 
     const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows());
 
-    // rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)]
-    //        [                cₑ                ]
+    // rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)]
+    //        [               cₑ                ]
     Eigen::VectorXd rhs{x.rows() + y.rows()};
     rhs.segment(0, x.rows()) =
-        -(g - A_e.transpose() * y +
-          A_i.transpose() * (Sinv * (Z * c_i - μ * e) - z));
+        -(g - A_e.transpose() * y -
+          A_i.transpose() * (-Σ * c_i + μ * Sinv * e + z));
     rhs.segment(x.rows(), y.rows()) = -c_e;
 
+    linearSystemBuildProfiler.Stop();
+    ScopedProfiler linearSystemSolveProfiler{linearSystemSolveProf};
+
+    Eigen::VectorXd p_x;
+    Eigen::VectorXd p_s;
+    Eigen::VectorXd p_y;
+    Eigen::VectorXd p_z;
+    double α_max = 1.0;
+    double α = 1.0;
+    double α_z = 1.0;
+
     // Solve the Newton-KKT system
     //
     // [H + AᵢᵀΣAᵢ  Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)]
     // [    Aₑ       0 ][−pₖʸ]    [                cₑ                ]
     solver.Compute(lhs, equalityConstraints.size(), μ);
-    Eigen::VectorXd step{x.rows() + y.rows()};
-    if (solver.Info() == Eigen::Success) {
-      step = solver.Solve(rhs);
-    } else {
-      // The regularization procedure failed due to a rank-deficient equality
-      // constraint Jacobian with linearly dependent constraints. Set the step
-      // length to zero and let second-order corrections attempt to restore
-      // feasibility.
-      step.setZero();
+    if (solver.Info() != Eigen::Success) [[unlikely]] {
+      status->exitCondition = SolverExitCondition::kFactorizationFailed;
+      return;
     }
 
+    Eigen::VectorXd step = solver.Solve(rhs);
+
+    linearSystemSolveProfiler.Stop();
+    ScopedProfiler lineSearchProfiler{lineSearchProf};
+
     // step = [ pₖˣ]
     //        [−pₖʸ]
-    Eigen::VectorXd p_x = step.segment(0, x.rows());
-    Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows());
+    p_x = step.segment(0, x.rows());
+    p_y = -step.segment(x.rows(), y.rows());
 
-    // pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ
-    Eigen::VectorXd p_z = -Σ * c_i + μ * Sinv * e - Σ * A_i * p_x;
+    // pₖˢ = cᵢ − s + Aᵢpₖˣ
+    p_s = c_i - s + A_i * p_x;
 
-    // pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ
-    Eigen::VectorXd p_s = μ * Zinv * e - s - Zinv * S * p_z;
+    // pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ
+    p_z = -Σ * c_i + μ * Sinv * e - Σ * A_i * p_x;
 
     // αᵐᵃˣ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ)
-    const double α_max = FractionToTheBoundaryRule(s, p_s, τ);
-    double α = α_max;
+    α_max = FractionToTheBoundaryRule(s, p_s, τ);
+    α = α_max;
 
     // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ)
-    double α_z = FractionToTheBoundaryRule(z, p_z, τ);
+    α_z = FractionToTheBoundaryRule(z, p_z, τ);
 
-    // Loop until a step is accepted. If a step becomes acceptable, the loop
-    // will exit early.
+    // Loop until a step is accepted
     while (1) {
       Eigen::VectorXd trial_x = x + α * p_x;
       Eigen::VectorXd trial_y = y + α_z * p_y;
@@ -439,7 +525,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
 
       // Check whether filter accepts trial iterate
       auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ);
-      if (filter.TryAdd(entry)) {
+      if (filter.TryAdd(entry, α)) {
         // Accept step
         break;
       }
@@ -465,10 +551,12 @@ void InteriorPoint(std::span<Variable> decisionVariables,
         bool stepAcceptable = false;
         for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable;
              ++soc_iteration) {
+          ScopedProfiler socProfiler{socProf};
+
           // Rebuild Newton-KKT rhs with updated constraint values.
           //
-          // rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)]
-          //        [              cₑˢᵒᶜ               ]
+          // rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)]
+          //        [              cₑˢᵒᶜ              ]
           //
           // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ)
           c_e_soc = α_soc * c_e_soc + trial_c_e;
@@ -480,12 +568,12 @@ void InteriorPoint(std::span<Variable> decisionVariables,
           p_x_cor = step.segment(0, x.rows());
           p_y_soc = -step.segment(x.rows(), y.rows());
 
+          // pₖˢ = cᵢ − s + Aᵢpₖˣ
+          p_s_soc = c_i - s + A_i * p_x_cor;
+
           // pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ
           p_z_soc = -Σ * c_i + μ * Sinv * e - Σ * A_i * p_x_cor;
 
-          // pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ
-          p_s_soc = μ * Zinv * e - s - Zinv * S * p_z_soc;
-
           // αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ)
           α_soc = FractionToTheBoundaryRule(s, p_s_soc, τ);
           trial_x = x + α_soc * p_x_cor;
@@ -503,7 +591,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
 
           // Check whether filter accepts trial iterate
           entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ);
-          if (filter.TryAdd(entry)) {
+          if (filter.TryAdd(entry, α)) {
             p_x = p_x_cor;
             p_y = p_y_soc;
             p_z = p_z_soc;
@@ -512,6 +600,19 @@ void InteriorPoint(std::span<Variable> decisionVariables,
             α_z = α_z_soc;
             stepAcceptable = true;
           }
+
+          socProfiler.Stop();
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+          if (config.diagnostics) {
+            double E = ErrorEstimate(g, A_e, trial_c_e, trial_y);
+            PrintIterationDiagnostics(
+                iterations, IterationMode::kSecondOrderCorrection,
+                socProfiler.CurrentDuration(), E, f.Value(),
+                trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(),
+                solver.HessianRegularization(), 1.0, 1.0);
+          }
+#endif
         }
 
         if (stepAcceptable) {
@@ -541,19 +642,16 @@ void InteriorPoint(std::span<Variable> decisionVariables,
       // Reduce step size
       α *= α_red_factor;
 
-      // Safety factor for the minimal step size
-      constexpr double α_min_frac = 0.05;
-
       // If step size hit a minimum, check if the KKT error was reduced. If it
-      // wasn't, invoke feasibility restoration.
-      if (α < α_min_frac * Filter::γConstraint) {
+      // wasn't, report bad line search.
+      if (α < α_min) {
         double currentKKTError = KKTError(g, A_e, c_e, A_i, c_i, s, y, z, μ);
 
-        Eigen::VectorXd trial_x = x + α_max * p_x;
-        Eigen::VectorXd trial_s = s + α_max * p_s;
+        trial_x = x + α_max * p_x;
+        trial_s = s + α_max * p_s;
 
-        Eigen::VectorXd trial_y = y + α_z * p_y;
-        Eigen::VectorXd trial_z = z + α_z * p_z;
+        trial_y = y + α_z * p_y;
+        trial_z = z + α_z * p_z;
 
         // Upate autodiff
         xAD.SetValue(trial_x);
@@ -561,8 +659,8 @@ void InteriorPoint(std::span<Variable> decisionVariables,
         yAD.SetValue(trial_y);
         zAD.SetValue(trial_z);
 
-        Eigen::VectorXd trial_c_e = c_eAD.Value();
-        Eigen::VectorXd trial_c_i = c_iAD.Value();
+        trial_c_e = c_eAD.Value();
+        trial_c_i = c_iAD.Value();
 
         double nextKKTError = KKTError(gradientF.Value(), jacobianCe.Value(),
                                        trial_c_e, jacobianCi.Value(), trial_c_i,
@@ -576,130 +674,23 @@ void InteriorPoint(std::span<Variable> decisionVariables,
           break;
         }
 
-        // If the step direction was bad and feasibility restoration is
-        // already running, running it again won't help
-        if (feasibilityRestoration) {
-          status->exitCondition = SolverExitCondition::kLocallyInfeasible;
-          return;
-        }
-
-        auto initialEntry = filter.MakeEntry(s, c_e, c_i, μ);
-
-        // Feasibility restoration phase
-        Eigen::VectorXd fr_x = x;
-        Eigen::VectorXd fr_s = s;
-        SolverStatus fr_status;
-        FeasibilityRestoration(
-            decisionVariables, equalityConstraints, inequalityConstraints, μ,
-            [&](const SolverIterationInfo& info) {
-              Eigen::VectorXd trial_x =
-                  info.x.segment(0, decisionVariables.size());
-              xAD.SetValue(trial_x);
-
-              Eigen::VectorXd trial_s =
-                  info.s.segment(0, inequalityConstraints.size());
-              sAD.SetValue(trial_s);
-
-              Eigen::VectorXd trial_c_e = c_eAD.Value();
-              Eigen::VectorXd trial_c_i = c_iAD.Value();
-
-              // If current iterate is acceptable to normal filter and
-              // constraint violation has sufficiently reduced, stop
-              // feasibility restoration
-              auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ);
-              if (filter.IsAcceptable(entry) &&
-                  entry.constraintViolation <
-                      0.9 * initialEntry.constraintViolation) {
-                return true;
-              }
-
-              return false;
-            },
-            config, fr_x, fr_s, &fr_status);
-
-        if (fr_status.exitCondition ==
-            SolverExitCondition::kCallbackRequestedStop) {
-          p_x = fr_x - x;
-          p_s = fr_s - s;
-
-          // Lagrange mutliplier estimates
-          //
-          //   [y] = (ÂÂᵀ)⁻¹Â[ ∇f]
-          //   [z]           [−μe]
-          //
-          //   where  = [Aₑ   0]
-          //             [Aᵢ  −S]
-          //
-          // See equation (19.37) of [1].
-          {
-            xAD.SetValue(fr_x);
-            sAD.SetValue(c_iAD.Value());
-
-            A_e = jacobianCe.Value();
-            A_i = jacobianCi.Value();
-            g = gradientF.Value();
-
-            // Â = [Aₑ   0]
-            //     [Aᵢ  −S]
-            triplets.clear();
-            triplets.reserve(A_e.nonZeros() + A_i.nonZeros() + s.rows());
-            for (int col = 0; col < A_e.cols(); ++col) {
-              // Append column of Aₑ in top-left quadrant
-              for (Eigen::SparseMatrix<double>::InnerIterator it{A_e, col}; it;
-                   ++it) {
-                triplets.emplace_back(it.row(), it.col(), it.value());
-              }
-              // Append column of Aᵢ in bottom-left quadrant
-              for (Eigen::SparseMatrix<double>::InnerIterator it{A_i, col}; it;
-                   ++it) {
-                triplets.emplace_back(A_e.rows() + it.row(), it.col(),
-                                      it.value());
-              }
-            }
-            // Append −S in bottom-right quadrant
-            for (int i = 0; i < s.rows(); ++i) {
-              triplets.emplace_back(A_e.rows() + i, A_e.cols() + i, -s(i));
-            }
-            Eigen::SparseMatrix<double> Ahat{A_e.rows() + A_i.rows(),
-                                             A_e.cols() + s.rows()};
-            Ahat.setFromSortedTriplets(
-                triplets.begin(), triplets.end(),
-                [](const auto&, const auto& b) { return b; });
-
-            // lhs = ÂÂᵀ
-            Eigen::SparseMatrix<double> lhs = Ahat * Ahat.transpose();
-
-            // rhs = Â[ ∇f]
-            //        [−μe]
-            Eigen::VectorXd rhsTemp{g.rows() + e.rows()};
-            rhsTemp.block(0, 0, g.rows(), 1) = g;
-            rhsTemp.block(g.rows(), 0, s.rows(), 1) = -μ * e;
-            Eigen::VectorXd rhs = Ahat * rhsTemp;
-
-            Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> yzEstimator{lhs};
-            Eigen::VectorXd sol = yzEstimator.solve(rhs);
-
-            p_y = y - sol.block(0, 0, y.rows(), 1);
-            p_z = z - sol.block(y.rows(), 0, z.rows(), 1);
-          }
+        status->exitCondition = SolverExitCondition::kLineSearchFailed;
+        return;
+      }
+    }
 
-          α = 1.0;
-          α_z = 1.0;
+    lineSearchProfiler.Stop();
 
-          // Accept step
-          break;
-        } else if (fr_status.exitCondition == SolverExitCondition::kSuccess) {
-          status->exitCondition = SolverExitCondition::kLocallyInfeasible;
-          x = fr_x;
-          return;
-        } else {
-          status->exitCondition =
-              SolverExitCondition::kFeasibilityRestorationFailed;
-          x = fr_x;
-          return;
-        }
-      }
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    // Write out spy file contents if that's enabled
+    if (config.spy) {
+      ScopedProfiler spyWritesProfiler{spyWritesProf};
+      H_spy->Add(H);
+      A_e_spy->Add(A_e);
+      A_i_spy->Add(A_i);
+      lhs_spy->Add(lhs);
     }
+#endif
 
     // If full step was accepted, reset full-step rejected counter
     if (α == α_max) {
@@ -738,15 +729,12 @@ void InteriorPoint(std::span<Variable> decisionVariables,
     //   zₖ₊₁⁽ⁱ⁾ = max(min(zₖ₊₁⁽ⁱ⁾, κ_Σ μⱼ/sₖ₊₁⁽ⁱ⁾), μⱼ/(κ_Σ sₖ₊₁⁽ⁱ⁾))
     //
     // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2].
-    {
+    for (int row = 0; row < z.rows(); ++row) {
       // Barrier parameter scale factor for inequality constraint Lagrange
       // multiplier safeguard
       constexpr double κ_Σ = 1e10;
 
-      for (int row = 0; row < z.rows(); ++row) {
-        z(row) =
-            std::max(std::min(z(row), κ_Σ * μ / s(row)), μ / (κ_Σ * s(row)));
-      }
+      z(row) = std::max(std::min(z(row), κ_Σ * μ / s(row)), μ / (κ_Σ * s(row)));
     }
 
     // Update autodiff for Jacobians and Hessian
@@ -759,6 +747,8 @@ void InteriorPoint(std::span<Variable> decisionVariables,
     g = gradientF.Value();
     H = hessianL.Value();
 
+    ScopedProfiler nextIterPrepProfiler{nextIterPrepProf};
+
     c_e = c_eAD.Value();
     c_i = c_iAD.Value();
 
@@ -784,22 +774,18 @@ void InteriorPoint(std::span<Variable> decisionVariables,
       }
     }
 
-    const auto innerIterEndTime = std::chrono::system_clock::now();
+    nextIterPrepProfiler.Stop();
+    innerIterProfiler.Stop();
 
-    // Diagnostics for current iteration
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
     if (config.diagnostics) {
-      if (iterations % 20 == 0) {
-        sleipnir::println("{:^4}   {:^9}  {:^13}  {:^13}  {:^13}", "iter",
-                          "time (ms)", "error", "cost", "infeasibility");
-        sleipnir::println("{:=^61}", "");
-      }
-
-      sleipnir::println("{:4}{}  {:9.3f}  {:13e}  {:13e}  {:13e}", iterations,
-                        feasibilityRestoration ? "r" : " ",
-                        ToMilliseconds(innerIterEndTime - innerIterStartTime),
-                        E_0, f.Value(),
-                        c_e.lpNorm<1>() + (c_i - s).lpNorm<1>());
+      PrintIterationDiagnostics(iterations, IterationMode::kNormal,
+                                innerIterProfiler.CurrentDuration(), E_0,
+                                f.Value(),
+                                c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(),
+                                solver.HessianRegularization(), α, α_z);
     }
+#endif
 
     ++iterations;
 
@@ -810,7 +796,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
     }
 
     // Check for max wall clock time
-    if (innerIterEndTime - solveStartTime > config.timeout) {
+    if (std::chrono::steady_clock::now() - solveStartTime > config.timeout) {
       status->exitCondition = SolverExitCondition::kTimeout;
       return;
     }
@@ -832,6 +818,6 @@ void InteriorPoint(std::span<Variable> decisionVariables,
       continue;
     }
   }
-}  // NOLINT(readability/fn_size)
+}
 
 }  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/Newton.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/Newton.cpp
new file mode 100644
index 00000000000..185d3abb5b4
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/Newton.cpp
@@ -0,0 +1,348 @@
+// Copyright (c) Sleipnir contributors
+
+#include "sleipnir/optimization/solver/Newton.hpp"
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <functional>
+#include <limits>
+#include <memory>
+#include <ranges>
+
+#include <Eigen/SparseCholesky>
+
+#include "optimization/RegularizedLDLT.hpp"
+#include "optimization/solver/util/ErrorEstimate.hpp"
+#include "optimization/solver/util/Filter.hpp"
+#include "optimization/solver/util/KKTError.hpp"
+#include "sleipnir/autodiff/Gradient.hpp"
+#include "sleipnir/autodiff/Hessian.hpp"
+#include "sleipnir/optimization/SolverExitCondition.hpp"
+#include "sleipnir/util/ScopedProfiler.hpp"
+#include "sleipnir/util/SetupProfiler.hpp"
+#include "sleipnir/util/SolveProfiler.hpp"
+#include "util/ScopeExit.hpp"
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+#include "sleipnir/util/Print.hpp"
+#include "sleipnir/util/Spy.hpp"
+#include "util/PrintDiagnostics.hpp"
+#endif
+
+// See docs/algorithms.md#Works_cited for citation definitions.
+
+namespace sleipnir {
+
+void Newton(
+    std::span<Variable> decisionVariables, Variable& f,
+    std::span<std::function<bool(const SolverIterationInfo& info)>> callbacks,
+    const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
+  const auto solveStartTime = std::chrono::steady_clock::now();
+
+  wpi::SmallVector<SetupProfiler> setupProfilers;
+  setupProfilers.emplace_back("setup").Start();
+
+  // Map decision variables and constraints to VariableMatrices for Lagrangian
+  VariableMatrix xAD{decisionVariables};
+  xAD.SetValue(x);
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ L setup").Start();
+
+  // Lagrangian L
+  //
+  // L(xₖ, yₖ) = f(xₖ)
+  auto L = f;
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∇f(x) setup").Start();
+
+  // Gradient of f ∇f
+  Gradient gradientF{f, xAD};
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∇f(x) init solve").Start();
+
+  Eigen::SparseVector<double> g = gradientF.Value();
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∇²ₓₓL setup").Start();
+
+  // Hessian of the Lagrangian H
+  //
+  // Hₖ = ∇²ₓₓL(xₖ, yₖ)
+  Hessian hessianL{L, xAD};
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∇²ₓₓL init solve").Start();
+
+  Eigen::SparseMatrix<double> H = hessianL.Value();
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ precondition ✓").Start();
+
+  // Check whether initial guess has finite f(xₖ)
+  if (!std::isfinite(f.Value())) {
+    status->exitCondition =
+        SolverExitCondition::kNonfiniteInitialCostOrConstraints;
+    return;
+  }
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ spy setup").Start();
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+  // Sparsity pattern files written when spy flag is set in SolverConfig
+  std::unique_ptr<Spy> H_spy;
+  std::unique_ptr<Spy> lhs_spy;
+  if (config.spy) {
+    H_spy = std::make_unique<Spy>("H.spy", "Hessian", "Decision variables",
+                                  "Decision variables", H.rows(), H.cols());
+    lhs_spy =
+        std::make_unique<Spy>("lhs.spy", "Newton-KKT system left-hand side",
+                              "Rows", "Columns", H.rows(), H.cols());
+  }
+#endif
+
+  setupProfilers.back().Stop();
+
+  int iterations = 0;
+
+  Filter filter{f};
+
+  RegularizedLDLT solver;
+
+  // Variables for determining when a step is acceptable
+  constexpr double α_red_factor = 0.5;
+  constexpr double α_min = 1e-20;
+  int acceptableIterCounter = 0;
+
+  // Error estimate
+  double E_0 = std::numeric_limits<double>::infinity();
+
+  setupProfilers[0].Stop();
+
+  wpi::SmallVector<SolveProfiler> solveProfilers;
+  solveProfilers.emplace_back("solve");
+  solveProfilers.emplace_back("  ↳ feasibility ✓");
+  solveProfilers.emplace_back("  ↳ user callbacks");
+  solveProfilers.emplace_back("  ↳ iter matrix solve");
+  solveProfilers.emplace_back("  ↳ line search");
+  solveProfilers.emplace_back("  ↳ spy writes");
+  solveProfilers.emplace_back("  ↳ next iter prep");
+
+  auto& innerIterProf = solveProfilers[0];
+  auto& feasibilityCheckProf = solveProfilers[1];
+  auto& userCallbacksProf = solveProfilers[2];
+  auto& linearSystemSolveProf = solveProfilers[3];
+  auto& lineSearchProf = solveProfilers[4];
+  [[maybe_unused]]
+  auto& spyWritesProf = solveProfilers[5];
+  auto& nextIterPrepProf = solveProfilers[6];
+
+  // Prints final diagnostics when the solver exits
+  scope_exit exit{[&] {
+    status->cost = f.Value();
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    if (config.diagnostics) {
+      // Append gradient profilers
+      solveProfilers.push_back(gradientF.GetProfilers()[0]);
+      solveProfilers.back().name = "  ↳ ∇f(x)";
+      for (const auto& profiler :
+           gradientF.GetProfilers() | std::views::drop(1)) {
+        solveProfilers.push_back(profiler);
+      }
+
+      // Append Hessian profilers
+      solveProfilers.push_back(hessianL.GetProfilers()[0]);
+      solveProfilers.back().name = "  ↳ ∇²ₓₓL";
+      for (const auto& profiler :
+           hessianL.GetProfilers() | std::views::drop(1)) {
+        solveProfilers.push_back(profiler);
+      }
+
+      PrintFinalDiagnostics(iterations, setupProfilers, solveProfilers);
+    }
+#endif
+  }};
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+  if (config.diagnostics) {
+    sleipnir::println("Error tolerance: {}\n", config.tolerance);
+  }
+#endif
+
+  while (E_0 > config.tolerance &&
+         acceptableIterCounter < config.maxAcceptableIterations) {
+    ScopedProfiler innerIterProfiler{innerIterProf};
+    ScopedProfiler feasibilityCheckProfiler{feasibilityCheckProf};
+
+    // Check for diverging iterates
+    if (x.lpNorm<Eigen::Infinity>() > 1e20 || !x.allFinite()) {
+      status->exitCondition = SolverExitCondition::kDivergingIterates;
+      return;
+    }
+
+    feasibilityCheckProfiler.Stop();
+    ScopedProfiler userCallbacksProfiler{userCallbacksProf};
+
+    // Call user callbacks
+    for (const auto& callback : callbacks) {
+      if (callback({iterations, x, Eigen::VectorXd::Zero(0), g, H,
+                    Eigen::SparseMatrix<double>{},
+                    Eigen::SparseMatrix<double>{}})) {
+        status->exitCondition = SolverExitCondition::kCallbackRequestedStop;
+        return;
+      }
+    }
+
+    userCallbacksProfiler.Stop();
+    ScopedProfiler linearSystemSolveProfiler{linearSystemSolveProf};
+
+    // rhs = −[∇f]
+    Eigen::VectorXd rhs = -g;
+
+    // Solve the Newton-KKT system
+    //
+    // [H][ pₖˣ] = −[∇f]
+    solver.Compute(H, 0, config.tolerance / 10.0);
+    Eigen::VectorXd step = solver.Solve(rhs);
+
+    linearSystemSolveProfiler.Stop();
+    ScopedProfiler lineSearchProfiler{lineSearchProf};
+
+    // step = [ pₖˣ]
+    Eigen::VectorXd p_x = step.segment(0, x.rows());
+
+    constexpr double α_max = 1.0;
+    double α = α_max;
+
+    // Loop until a step is accepted. If a step becomes acceptable, the loop
+    // will exit early.
+    while (1) {
+      Eigen::VectorXd trial_x = x + α * p_x;
+
+      xAD.SetValue(trial_x);
+
+      // If f(xₖ + αpₖˣ) isn't finite, reduce step size immediately
+      if (!std::isfinite(f.Value())) {
+        // Reduce step size
+        α *= α_red_factor;
+        continue;
+      }
+
+      // Check whether filter accepts trial iterate
+      auto entry = filter.MakeEntry();
+      if (filter.TryAdd(entry, α)) {
+        // Accept step
+        break;
+      }
+
+      // Reduce step size
+      α *= α_red_factor;
+
+      // If step size hit a minimum, check if the KKT error was reduced. If it
+      // wasn't, report bad line search.
+      if (α < α_min) {
+        double currentKKTError = KKTError(g);
+
+        Eigen::VectorXd trial_x = x + α_max * p_x;
+
+        // Upate autodiff
+        xAD.SetValue(trial_x);
+
+        double nextKKTError = KKTError(gradientF.Value());
+
+        // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
+        if (nextKKTError <= 0.999 * currentKKTError) {
+          α = α_max;
+
+          // Accept step
+          break;
+        }
+
+        status->exitCondition = SolverExitCondition::kLineSearchFailed;
+        return;
+      }
+    }
+
+    // Handle very small search directions by letting αₖ = αₖᵐᵃˣ when
+    // max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach.
+    //
+    // See section 3.9 of [2].
+    double maxStepScaled = 0.0;
+    for (int row = 0; row < x.rows(); ++row) {
+      maxStepScaled = std::max(maxStepScaled,
+                               std::abs(p_x(row)) / (1.0 + std::abs(x(row))));
+    }
+    if (maxStepScaled < 10.0 * std::numeric_limits<double>::epsilon()) {
+      α = α_max;
+    }
+
+    lineSearchProfiler.Stop();
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    // Write out spy file contents if that's enabled
+    if (config.spy) {
+      ScopedProfiler spyWritesProfiler{spyWritesProf};
+      H_spy->Add(H);
+      lhs_spy->Add(H);
+    }
+#endif
+
+    // xₖ₊₁ = xₖ + αₖpₖˣ
+    x += α * p_x;
+
+    // Update autodiff for Hessian
+    xAD.SetValue(x);
+    g = gradientF.Value();
+    H = hessianL.Value();
+
+    ScopedProfiler nextIterPrepProfiler{nextIterPrepProf};
+
+    // Update the error estimate
+    E_0 = ErrorEstimate(g);
+    if (E_0 < config.acceptableTolerance) {
+      ++acceptableIterCounter;
+    } else {
+      acceptableIterCounter = 0;
+    }
+
+    nextIterPrepProfiler.Stop();
+    innerIterProfiler.Stop();
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    if (config.diagnostics) {
+      PrintIterationDiagnostics(iterations, IterationMode::kNormal,
+                                innerIterProfiler.CurrentDuration(), E_0,
+                                f.Value(), 0.0, solver.HessianRegularization(),
+                                α, 1.0);
+    }
+#endif
+
+    ++iterations;
+
+    // Check for max iterations
+    if (iterations >= config.maxIterations) {
+      status->exitCondition = SolverExitCondition::kMaxIterationsExceeded;
+      return;
+    }
+
+    // Check for max wall clock time
+    if (std::chrono::steady_clock::now() - solveStartTime > config.timeout) {
+      status->exitCondition = SolverExitCondition::kTimeout;
+      return;
+    }
+
+    // Check for solve to acceptable tolerance
+    if (E_0 > config.tolerance &&
+        acceptableIterCounter == config.maxAcceptableIterations) {
+      status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance;
+      return;
+    }
+  }
+}
+
+}  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp
index 662abc2fb6e..b40c0708754 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp
@@ -5,15 +5,16 @@
 #include <algorithm>
 #include <chrono>
 #include <cmath>
-#include <fstream>
+#include <functional>
 #include <limits>
+#include <memory>
+#include <ranges>
 
 #include <Eigen/SparseCholesky>
 #include <wpi/SmallVector.h>
 
 #include "optimization/RegularizedLDLT.hpp"
 #include "optimization/solver/util/ErrorEstimate.hpp"
-#include "optimization/solver/util/FeasibilityRestoration.hpp"
 #include "optimization/solver/util/Filter.hpp"
 #include "optimization/solver/util/IsLocallyInfeasible.hpp"
 #include "optimization/solver/util/KKTError.hpp"
@@ -21,20 +22,32 @@
 #include "sleipnir/autodiff/Hessian.hpp"
 #include "sleipnir/autodiff/Jacobian.hpp"
 #include "sleipnir/optimization/SolverExitCondition.hpp"
+#include "sleipnir/util/ScopedProfiler.hpp"
+#include "sleipnir/util/SetupProfiler.hpp"
+#include "sleipnir/util/SolveProfiler.hpp"
+#include "util/ScopeExit.hpp"
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
 #include "sleipnir/util/Print.hpp"
 #include "sleipnir/util/Spy.hpp"
-#include "util/ScopeExit.hpp"
-#include "util/ToMilliseconds.hpp"
+#include "util/PrintDiagnostics.hpp"
+#endif
 
 // See docs/algorithms.md#Works_cited for citation definitions.
 
 namespace sleipnir {
 
-void SQP(std::span<Variable> decisionVariables,
-         std::span<Variable> equalityConstraints, Variable& f,
-         function_ref<bool(const SolverIterationInfo& info)> callback,
-         const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
-  const auto solveStartTime = std::chrono::system_clock::now();
+void SQP(
+    std::span<Variable> decisionVariables,
+    std::span<Variable> equalityConstraints, Variable& f,
+    std::span<std::function<bool(const SolverIterationInfo& info)>> callbacks,
+    const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
+  const auto solveStartTime = std::chrono::steady_clock::now();
+
+  wpi::SmallVector<SetupProfiler> setupProfilers;
+  setupProfilers.emplace_back("setup").Start();
+
+  setupProfilers.emplace_back("  ↳ y setup").Start();
 
   // Map decision variables and constraints to VariableMatrices for Lagrangian
   VariableMatrix xAD{decisionVariables};
@@ -47,11 +60,17 @@ void SQP(std::span<Variable> decisionVariables,
     y.SetValue(0.0);
   }
 
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ L setup").Start();
+
   // Lagrangian L
   //
   // L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ)
   auto L = f - (yAD.T() * c_eAD)(0);
 
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∂cₑ/∂x setup").Start();
+
   // Equality constraint Jacobian Aₑ
   //
   //         [∇ᵀcₑ₁(xₖ)]
@@ -59,23 +78,45 @@ void SQP(std::span<Variable> decisionVariables,
   //         [    ⋮    ]
   //         [∇ᵀcₑₘ(xₖ)]
   Jacobian jacobianCe{c_eAD, xAD};
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∂cₑ/∂x init solve").Start();
+
   Eigen::SparseMatrix<double> A_e = jacobianCe.Value();
 
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∇f(x) setup").Start();
+
   // Gradient of f ∇f
   Gradient gradientF{f, xAD};
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∇f(x) init solve").Start();
+
   Eigen::SparseVector<double> g = gradientF.Value();
 
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∇²ₓₓL setup").Start();
+
   // Hessian of the Lagrangian H
   //
   // Hₖ = ∇²ₓₓL(xₖ, yₖ)
   Hessian hessianL{L, xAD};
+
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ ∇²ₓₓL init solve").Start();
+
   Eigen::SparseMatrix<double> H = hessianL.Value();
 
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ precondition ✓").Start();
+
   Eigen::VectorXd y = yAD.Value();
   Eigen::VectorXd c_e = c_eAD.Value();
 
   // Check for overconstrained problem
   if (equalityConstraints.size() > decisionVariables.size()) {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
     if (config.diagnostics) {
       sleipnir::println("The problem has too few degrees of freedom.");
       sleipnir::println(
@@ -86,6 +127,7 @@ void SQP(std::span<Variable> decisionVariables,
         }
       }
     }
+#endif
 
     status->exitCondition = SolverExitCondition::kTooFewDOFs;
     return;
@@ -98,60 +140,30 @@ void SQP(std::span<Variable> decisionVariables,
     return;
   }
 
+  setupProfilers.back().Stop();
+  setupProfilers.emplace_back("  ↳ spy setup").Start();
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
   // Sparsity pattern files written when spy flag is set in SolverConfig
-  std::ofstream H_spy;
-  std::ofstream A_e_spy;
+  std::unique_ptr<Spy> H_spy;
+  std::unique_ptr<Spy> A_e_spy;
+  std::unique_ptr<Spy> lhs_spy;
   if (config.spy) {
-    A_e_spy.open("A_e.spy");
-    H_spy.open("H.spy");
+    H_spy = std::make_unique<Spy>("H.spy", "Hessian", "Decision variables",
+                                  "Decision variables", H.rows(), H.cols());
+    A_e_spy = std::make_unique<Spy>("A_e.spy", "Equality constraint Jacobian",
+                                    "Constraints", "Decision variables",
+                                    A_e.rows(), A_e.cols());
+    lhs_spy = std::make_unique<Spy>(
+        "lhs.spy", "Newton-KKT system left-hand side", "Rows", "Columns",
+        H.rows() + A_e.rows(), H.cols() + A_e.rows());
   }
+#endif
 
-  if (config.diagnostics) {
-    sleipnir::println("Error tolerance: {}\n", config.tolerance);
-  }
-
-  std::chrono::system_clock::time_point iterationsStartTime;
+  setupProfilers.back().Stop();
 
   int iterations = 0;
 
-  // Prints final diagnostics when the solver exits
-  scope_exit exit{[&] {
-    status->cost = f.Value();
-
-    if (config.diagnostics) {
-      auto solveEndTime = std::chrono::system_clock::now();
-
-      sleipnir::println("\nSolve time: {:.3f} ms",
-                        ToMilliseconds(solveEndTime - solveStartTime));
-      sleipnir::println("  ↳ {:.3f} ms (solver setup)",
-                        ToMilliseconds(iterationsStartTime - solveStartTime));
-      if (iterations > 0) {
-        sleipnir::println(
-            "  ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)",
-            ToMilliseconds(solveEndTime - iterationsStartTime), iterations,
-            ToMilliseconds((solveEndTime - iterationsStartTime) / iterations));
-      }
-      sleipnir::println("");
-
-      sleipnir::println("{:^8}   {:^10}   {:^14}   {:^6}", "autodiff",
-                        "setup (ms)", "avg solve (ms)", "solves");
-      sleipnir::println("{:=^47}", "");
-      constexpr auto format = "{:^8}   {:10.3f}   {:14.3f}   {:6}";
-      sleipnir::println(format, "∇f(x)",
-                        gradientF.GetProfiler().SetupDuration(),
-                        gradientF.GetProfiler().AverageSolveDuration(),
-                        gradientF.GetProfiler().SolveMeasurements());
-      sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(),
-                        hessianL.GetProfiler().AverageSolveDuration(),
-                        hessianL.GetProfiler().SolveMeasurements());
-      sleipnir::println(format, "∂cₑ/∂x",
-                        jacobianCe.GetProfiler().SetupDuration(),
-                        jacobianCe.GetProfiler().AverageSolveDuration(),
-                        jacobianCe.GetProfiler().SolveMeasurements());
-      sleipnir::println("");
-    }
-  }};
-
   Filter filter{f};
 
   // Kept outside the loop so its storage can be reused
@@ -161,6 +173,7 @@ void SQP(std::span<Variable> decisionVariables,
 
   // Variables for determining when a step is acceptable
   constexpr double α_red_factor = 0.5;
+  constexpr double α_min = 1e-20;
   int acceptableIterCounter = 0;
 
   int fullStepRejectedCounter = 0;
@@ -168,19 +181,79 @@ void SQP(std::span<Variable> decisionVariables,
   // Error estimate
   double E_0 = std::numeric_limits<double>::infinity();
 
+  setupProfilers[0].Stop();
+
+  wpi::SmallVector<SolveProfiler> solveProfilers;
+  solveProfilers.emplace_back("solve");
+  solveProfilers.emplace_back("  ↳ feasibility ✓");
+  solveProfilers.emplace_back("  ↳ user callbacks");
+  solveProfilers.emplace_back("  ↳ iter matrix build");
+  solveProfilers.emplace_back("  ↳ iter matrix solve");
+  solveProfilers.emplace_back("  ↳ line search");
+  solveProfilers.emplace_back("    ↳ SOC");
+  solveProfilers.emplace_back("  ↳ spy writes");
+  solveProfilers.emplace_back("  ↳ next iter prep");
+
+  auto& innerIterProf = solveProfilers[0];
+  auto& feasibilityCheckProf = solveProfilers[1];
+  auto& userCallbacksProf = solveProfilers[2];
+  auto& linearSystemBuildProf = solveProfilers[3];
+  auto& linearSystemSolveProf = solveProfilers[4];
+  auto& lineSearchProf = solveProfilers[5];
+  auto& socProf = solveProfilers[6];
+  [[maybe_unused]]
+  auto& spyWritesProf = solveProfilers[7];
+  auto& nextIterPrepProf = solveProfilers[8];
+
+  // Prints final diagnostics when the solver exits
+  scope_exit exit{[&] {
+    status->cost = f.Value();
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    if (config.diagnostics) {
+      // Append gradient profilers
+      solveProfilers.push_back(gradientF.GetProfilers()[0]);
+      solveProfilers.back().name = "  ↳ ∇f(x)";
+      for (const auto& profiler :
+           gradientF.GetProfilers() | std::views::drop(1)) {
+        solveProfilers.push_back(profiler);
+      }
+
+      // Append Hessian profilers
+      solveProfilers.push_back(hessianL.GetProfilers()[0]);
+      solveProfilers.back().name = "  ↳ ∇²ₓₓL";
+      for (const auto& profiler :
+           hessianL.GetProfilers() | std::views::drop(1)) {
+        solveProfilers.push_back(profiler);
+      }
+
+      // Append equality constraint Jacobian profilers
+      solveProfilers.push_back(jacobianCe.GetProfilers()[0]);
+      solveProfilers.back().name = "  ↳ ∂cₑ/∂x";
+      for (const auto& profiler :
+           jacobianCe.GetProfilers() | std::views::drop(1)) {
+        solveProfilers.push_back(profiler);
+      }
+
+      PrintFinalDiagnostics(iterations, setupProfilers, solveProfilers);
+    }
+#endif
+  }};
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
   if (config.diagnostics) {
-    iterationsStartTime = std::chrono::system_clock::now();
+    sleipnir::println("Error tolerance: {}\n", config.tolerance);
   }
+#endif
 
   while (E_0 > config.tolerance &&
          acceptableIterCounter < config.maxAcceptableIterations) {
-    std::chrono::system_clock::time_point innerIterStartTime;
-    if (config.diagnostics) {
-      innerIterStartTime = std::chrono::system_clock::now();
-    }
+    ScopedProfiler innerIterProfiler{innerIterProf};
+    ScopedProfiler feasibilityCheckProfiler{feasibilityCheckProf};
 
     // Check for local equality constraint infeasibility
     if (IsEqualityLocallyInfeasible(A_e, c_e)) {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
       if (config.diagnostics) {
         sleipnir::println(
             "The problem is locally infeasible due to violated equality "
@@ -193,6 +266,7 @@ void SQP(std::span<Variable> decisionVariables,
           }
         }
       }
+#endif
 
       status->exitCondition = SolverExitCondition::kLocallyInfeasible;
       return;
@@ -204,24 +278,20 @@ void SQP(std::span<Variable> decisionVariables,
       return;
     }
 
-    // Write out spy file contents if that's enabled
-    if (config.spy) {
-      // Gap between sparsity patterns
-      if (iterations > 0) {
-        A_e_spy << "\n";
-        H_spy << "\n";
-      }
+    feasibilityCheckProfiler.Stop();
+    ScopedProfiler userCallbacksProfiler{userCallbacksProf};
 
-      Spy(H_spy, H);
-      Spy(A_e_spy, A_e);
+    // Call user callbacks
+    for (const auto& callback : callbacks) {
+      if (callback({iterations, x, Eigen::VectorXd::Zero(0), g, H, A_e,
+                    Eigen::SparseMatrix<double>{}})) {
+        status->exitCondition = SolverExitCondition::kCallbackRequestedStop;
+        return;
+      }
     }
 
-    // Call user callback
-    if (callback({iterations, x, Eigen::VectorXd::Zero(0), g, H, A_e,
-                  Eigen::SparseMatrix<double>{}})) {
-      status->exitCondition = SolverExitCondition::kCallbackRequestedStop;
-      return;
-    }
+    userCallbacksProfiler.Stop();
+    ScopedProfiler linearSystemBuildProfiler{linearSystemBuildProf};
 
     // lhs = [H   Aₑᵀ]
     //       [Aₑ   0 ]
@@ -232,7 +302,7 @@ void SQP(std::span<Variable> decisionVariables,
     triplets.clear();
     triplets.reserve(topLeft.nonZeros() + A_e.nonZeros());
     for (int col = 0; col < H.cols(); ++col) {
-      // Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant
+      // Append column of H lower triangle in top-left quadrant
       for (Eigen::SparseMatrix<double>::InnerIterator it{topLeft, col}; it;
            ++it) {
         triplets.emplace_back(it.row(), it.col(), it.value());
@@ -254,31 +324,37 @@ void SQP(std::span<Variable> decisionVariables,
     rhs.segment(0, x.rows()) = -(g - A_e.transpose() * y);
     rhs.segment(x.rows(), y.rows()) = -c_e;
 
+    linearSystemBuildProfiler.Stop();
+    ScopedProfiler linearSystemSolveProfiler{linearSystemSolveProf};
+
+    Eigen::VectorXd p_x;
+    Eigen::VectorXd p_y;
+    constexpr double α_max = 1.0;
+    double α = 1.0;
+
     // Solve the Newton-KKT system
     //
     // [H   Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy]
     // [Aₑ   0 ][−pₖʸ]    [   cₑ    ]
     solver.Compute(lhs, equalityConstraints.size(), config.tolerance / 10.0);
-    Eigen::VectorXd step{x.rows() + y.rows()};
-    if (solver.Info() == Eigen::Success) {
-      step = solver.Solve(rhs);
-    } else {
-      // The regularization procedure failed due to a rank-deficient equality
-      // constraint Jacobian with linearly dependent constraints
-      status->exitCondition = SolverExitCondition::kLocallyInfeasible;
+    if (solver.Info() != Eigen::Success) [[unlikely]] {
+      status->exitCondition = SolverExitCondition::kFactorizationFailed;
       return;
     }
 
+    Eigen::VectorXd step = solver.Solve(rhs);
+
+    linearSystemSolveProfiler.Stop();
+    ScopedProfiler lineSearchProfiler{lineSearchProf};
+
     // step = [ pₖˣ]
     //        [−pₖʸ]
-    Eigen::VectorXd p_x = step.segment(0, x.rows());
-    Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows());
+    p_x = step.segment(0, x.rows());
+    p_y = -step.segment(x.rows(), y.rows());
 
-    constexpr double α_max = 1.0;
-    double α = α_max;
+    α = α_max;
 
-    // Loop until a step is accepted. If a step becomes acceptable, the loop
-    // will exit early.
+    // Loop until a step is accepted
     while (1) {
       Eigen::VectorXd trial_x = x + α * p_x;
       Eigen::VectorXd trial_y = y + α * p_y;
@@ -297,7 +373,7 @@ void SQP(std::span<Variable> decisionVariables,
 
       // Check whether filter accepts trial iterate
       auto entry = filter.MakeEntry(trial_c_e);
-      if (filter.TryAdd(entry)) {
+      if (filter.TryAdd(entry, α)) {
         // Accept step
         break;
       }
@@ -320,6 +396,8 @@ void SQP(std::span<Variable> decisionVariables,
         bool stepAcceptable = false;
         for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable;
              ++soc_iteration) {
+          ScopedProfiler socProfiler{socProf};
+
           // Rebuild Newton-KKT rhs with updated constraint values.
           //
           // rhs = −[∇f − Aₑᵀy]
@@ -344,12 +422,25 @@ void SQP(std::span<Variable> decisionVariables,
 
           // Check whether filter accepts trial iterate
           entry = filter.MakeEntry(trial_c_e);
-          if (filter.TryAdd(entry)) {
+          if (filter.TryAdd(entry, α)) {
             p_x = p_x_cor;
             p_y = p_y_soc;
             α = α_soc;
             stepAcceptable = true;
           }
+
+          socProfiler.Stop();
+
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+          if (config.diagnostics) {
+            double E = ErrorEstimate(g, A_e, trial_c_e, trial_y);
+            PrintIterationDiagnostics(iterations,
+                                      IterationMode::kSecondOrderCorrection,
+                                      socProfiler.CurrentDuration(), E,
+                                      f.Value(), trial_c_e.lpNorm<1>(),
+                                      solver.HessianRegularization(), 1.0, 1.0);
+          }
+#endif
         }
 
         if (stepAcceptable) {
@@ -379,22 +470,19 @@ void SQP(std::span<Variable> decisionVariables,
       // Reduce step size
       α *= α_red_factor;
 
-      // Safety factor for the minimal step size
-      constexpr double α_min_frac = 0.05;
-
       // If step size hit a minimum, check if the KKT error was reduced. If it
-      // wasn't, report infeasible.
-      if (α < α_min_frac * Filter::γConstraint) {
+      // wasn't, report bad line search.
+      if (α < α_min) {
         double currentKKTError = KKTError(g, A_e, c_e, y);
 
-        Eigen::VectorXd trial_x = x + α_max * p_x;
-        Eigen::VectorXd trial_y = y + α_max * p_y;
+        trial_x = x + α_max * p_x;
+        trial_y = y + α_max * p_y;
 
         // Upate autodiff
         xAD.SetValue(trial_x);
         yAD.SetValue(trial_y);
 
-        Eigen::VectorXd trial_c_e = c_eAD.Value();
+        trial_c_e = c_eAD.Value();
 
         double nextKKTError =
             KKTError(gradientF.Value(), jacobianCe.Value(), trial_c_e, trial_y);
@@ -407,76 +495,22 @@ void SQP(std::span<Variable> decisionVariables,
           break;
         }
 
-        auto initialEntry = filter.MakeEntry(c_e);
-
-        // Feasibility restoration phase
-        Eigen::VectorXd fr_x = x;
-        SolverStatus fr_status;
-        FeasibilityRestoration(
-            decisionVariables, equalityConstraints,
-            [&](const SolverIterationInfo& info) {
-              trial_x = info.x.segment(0, decisionVariables.size());
-              xAD.SetValue(trial_x);
-
-              trial_c_e = c_eAD.Value();
-
-              // If current iterate is acceptable to normal filter and
-              // constraint violation has sufficiently reduced, stop
-              // feasibility restoration
-              entry = filter.MakeEntry(trial_c_e);
-              if (filter.IsAcceptable(entry) &&
-                  entry.constraintViolation <
-                      0.9 * initialEntry.constraintViolation) {
-                return true;
-              }
-
-              return false;
-            },
-            config, fr_x, &fr_status);
-
-        if (fr_status.exitCondition ==
-            SolverExitCondition::kCallbackRequestedStop) {
-          p_x = fr_x - x;
-
-          // Lagrange mutliplier estimates
-          //
-          //   y = (AₑAₑᵀ)⁻¹Aₑ∇f
-          //
-          // See equation (19.37) of [1].
-          {
-            xAD.SetValue(fr_x);
-
-            A_e = jacobianCe.Value();
-            g = gradientF.Value();
-
-            // lhs = AₑAₑᵀ
-            Eigen::SparseMatrix<double> lhs = A_e * A_e.transpose();
-
-            // rhs = Aₑ∇f
-            Eigen::VectorXd rhs = A_e * g;
-
-            Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> yEstimator{lhs};
-            Eigen::VectorXd sol = yEstimator.solve(rhs);
-
-            p_y = y - sol.block(0, 0, y.rows(), 1);
-          }
+        status->exitCondition = SolverExitCondition::kLineSearchFailed;
+        return;
+      }
+    }
 
-          α = 1.0;
+    lineSearchProfiler.Stop();
 
-          // Accept step
-          break;
-        } else if (fr_status.exitCondition == SolverExitCondition::kSuccess) {
-          status->exitCondition = SolverExitCondition::kLocallyInfeasible;
-          x = fr_x;
-          return;
-        } else {
-          status->exitCondition =
-              SolverExitCondition::kFeasibilityRestorationFailed;
-          x = fr_x;
-          return;
-        }
-      }
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    // Write out spy file contents if that's enabled
+    if (config.spy) {
+      ScopedProfiler spyWritesProfiler{spyWritesProf};
+      H_spy->Add(H);
+      A_e_spy->Add(A_e);
+      lhs_spy->Add(lhs);
     }
+#endif
 
     // If full step was accepted, reset full-step rejected counter
     if (α == α_max) {
@@ -508,6 +542,8 @@ void SQP(std::span<Variable> decisionVariables,
     g = gradientF.Value();
     H = hessianL.Value();
 
+    ScopedProfiler nextIterPrepProfiler{nextIterPrepProf};
+
     c_e = c_eAD.Value();
 
     // Update the error estimate
@@ -518,20 +554,17 @@ void SQP(std::span<Variable> decisionVariables,
       acceptableIterCounter = 0;
     }
 
-    const auto innerIterEndTime = std::chrono::system_clock::now();
+    nextIterPrepProfiler.Stop();
+    innerIterProfiler.Stop();
 
-    // Diagnostics for current iteration
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
     if (config.diagnostics) {
-      if (iterations % 20 == 0) {
-        sleipnir::println("{:^4}   {:^9}  {:^13}  {:^13}  {:^13}", "iter",
-                          "time (ms)", "error", "cost", "infeasibility");
-        sleipnir::println("{:=^61}", "");
-      }
-
-      sleipnir::println("{:4}   {:9.3f}  {:13e}  {:13e}  {:13e}", iterations,
-                        ToMilliseconds(innerIterEndTime - innerIterStartTime),
-                        E_0, f.Value(), c_e.lpNorm<1>());
+      PrintIterationDiagnostics(iterations, IterationMode::kNormal,
+                                innerIterProfiler.CurrentDuration(), E_0,
+                                f.Value(), c_e.lpNorm<1>(),
+                                solver.HessianRegularization(), α, α);
     }
+#endif
 
     ++iterations;
 
@@ -542,7 +575,7 @@ void SQP(std::span<Variable> decisionVariables,
     }
 
     // Check for max wall clock time
-    if (innerIterEndTime - solveStartTime > config.timeout) {
+    if (std::chrono::steady_clock::now() - solveStartTime > config.timeout) {
       status->exitCondition = SolverExitCondition::kTimeout;
       return;
     }
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp
index f1490028dd2..2d59cbe0da3 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp
@@ -11,6 +11,20 @@
 
 namespace sleipnir {
 
+/**
+ * Returns the error estimate using the KKT conditions for Newton's method.
+ *
+ * @param g Gradient of the cost function ∇f.
+ */
+inline double ErrorEstimate(const Eigen::VectorXd& g) {
+  // Update the error estimate using the KKT conditions from equations (19.5a)
+  // through (19.5d) of [1].
+  //
+  //   ∇f = 0
+
+  return g.lpNorm<Eigen::Infinity>();
+}
+
 /**
  * Returns the error estimate using the KKT conditions for SQP.
  *
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp
deleted file mode 100644
index 79b5d99ae27..00000000000
--- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp
+++ /dev/null
@@ -1,400 +0,0 @@
-// Copyright (c) Sleipnir contributors
-
-#pragma once
-
-#include <algorithm>
-#include <cmath>
-#include <iterator>
-#include <span>
-
-#include <Eigen/Core>
-#include <wpi/SmallVector.h>
-
-#include "sleipnir/autodiff/Variable.hpp"
-#include "sleipnir/autodiff/VariableMatrix.hpp"
-#include "sleipnir/optimization/SolverConfig.hpp"
-#include "sleipnir/optimization/SolverIterationInfo.hpp"
-#include "sleipnir/optimization/SolverStatus.hpp"
-#include "sleipnir/optimization/solver/InteriorPoint.hpp"
-#include "sleipnir/util/FunctionRef.hpp"
-
-namespace sleipnir {
-
-/**
- * Finds the iterate that minimizes the constraint violation while not deviating
- * too far from the starting point. This is a fallback procedure when the normal
- * Sequential Quadratic Programming method fails to converge to a feasible
- * point.
- *
- * @param[in] decisionVariables The list of decision variables.
- * @param[in] equalityConstraints The list of equality constraints.
- * @param[in] callback The user callback.
- * @param[in] config Configuration options for the solver.
- * @param[in,out] x The current iterate from the normal solve.
- * @param[out] status The solver status.
- */
-inline void FeasibilityRestoration(
-    std::span<Variable> decisionVariables,
-    std::span<Variable> equalityConstraints,
-    function_ref<bool(const SolverIterationInfo& info)> callback,
-    const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
-  // Feasibility restoration
-  //
-  //        min  ρ Σ (pₑ + nₑ) + ζ/2 (x - x_R)ᵀD_R(x - x_R)
-  //         x
-  //       pₑ,nₑ
-  //
-  //   s.t. cₑ(x) - pₑ + nₑ = 0
-  //        pₑ ≥ 0
-  //        nₑ ≥ 0
-  //
-  // where ρ = 1000, ζ = √μ where μ is the barrier parameter, x_R is original
-  // iterate before feasibility restoration, and D_R is a scaling matrix defined
-  // by
-  //
-  //   D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
-
-  constexpr double ρ = 1000.0;
-  double μ = config.tolerance / 10.0;
-
-  wpi::SmallVector<Variable> fr_decisionVariables;
-  fr_decisionVariables.reserve(decisionVariables.size() +
-                               2 * equalityConstraints.size());
-
-  // Assign x
-  fr_decisionVariables.assign(decisionVariables.begin(),
-                              decisionVariables.end());
-
-  // Allocate pₑ and nₑ
-  for (size_t row = 0; row < 2 * equalityConstraints.size(); ++row) {
-    fr_decisionVariables.emplace_back();
-  }
-
-  auto it = fr_decisionVariables.begin();
-
-  VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
-  it += decisionVariables.size();
-
-  VariableMatrix p_e{std::span{it, it + equalityConstraints.size()}};
-  it += equalityConstraints.size();
-
-  VariableMatrix n_e{std::span{it, it + equalityConstraints.size()}};
-  it += equalityConstraints.size();
-
-  // Set initial values for pₑ and nₑ.
-  //
-  //
-  // From equation (33) of [2]:
-  //                       ______________________
-  //       μ − ρ c(x)     /(μ − ρ c(x))²   μ c(x)
-  //   n = −−−−−−−−−− +  / (−−−−−−−−−−)  + −−−−−−     (1)
-  //           2ρ       √  (    2ρ    )      2ρ
-  //
-  // The quadratic formula:
-  //             ________
-  //       -b + √b² - 4ac
-  //   x = −−−−−−−−−−−−−−                             (2)
-  //             2a
-  //
-  // Rearrange (1) to fit the quadratic formula better:
-  //                     _________________________
-  //       μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x)
-  //   n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
-  //                         2ρ
-  //
-  // Solve for coefficients:
-  //
-  //   a = ρ                                         (3)
-  //   b = ρ c(x) - μ                                (4)
-  //
-  //   -4ac = μ c(x) 2ρ
-  //   -4(ρ)c = 2ρ μ c(x)
-  //   -4c = 2μ c(x)
-  //   c = -μ c(x)/2                                 (5)
-  //
-  //   p = c(x) + n                                  (6)
-  for (int row = 0; row < p_e.Rows(); ++row) {
-    double c_e = equalityConstraints[row].Value();
-
-    constexpr double a = 2 * ρ;
-    double b = ρ * c_e - μ;
-    double c = -μ * c_e / 2.0;
-
-    double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a);
-    double p = c_e + n;
-
-    p_e(row).SetValue(p);
-    n_e(row).SetValue(n);
-  }
-
-  // cₑ(x) - pₑ + nₑ = 0
-  wpi::SmallVector<Variable> fr_equalityConstraints;
-  fr_equalityConstraints.assign(equalityConstraints.begin(),
-                                equalityConstraints.end());
-  for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
-    auto& constraint = fr_equalityConstraints[row];
-    constraint = constraint - p_e(row) + n_e(row);
-  }
-
-  // cᵢ(x) - s - pᵢ + nᵢ = 0
-  wpi::SmallVector<Variable> fr_inequalityConstraints;
-
-  // pₑ ≥ 0
-  std::copy(p_e.begin(), p_e.end(),
-            std::back_inserter(fr_inequalityConstraints));
-
-  // nₑ ≥ 0
-  std::copy(n_e.begin(), n_e.end(),
-            std::back_inserter(fr_inequalityConstraints));
-
-  Variable J = 0.0;
-
-  // J += ρ Σ (pₑ + nₑ)
-  for (auto& elem : p_e) {
-    J += elem;
-  }
-  for (auto& elem : n_e) {
-    J += elem;
-  }
-  J *= ρ;
-
-  // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
-  Eigen::VectorXd D_R{x.rows()};
-  for (int row = 0; row < D_R.rows(); ++row) {
-    D_R(row) = std::min(1.0, 1.0 / std::abs(x(row)));
-  }
-
-  // J += ζ/2 (x - x_R)ᵀD_R(x - x_R)
-  for (int row = 0; row < x.rows(); ++row) {
-    J += std::sqrt(μ) / 2.0 * D_R(row) * sleipnir::pow(xAD(row) - x(row), 2);
-  }
-
-  Eigen::VectorXd fr_x = VariableMatrix{fr_decisionVariables}.Value();
-
-  // Set up initial value for inequality constraint slack variables
-  Eigen::VectorXd fr_s{fr_inequalityConstraints.size()};
-  fr_s.setOnes();
-
-  InteriorPoint(fr_decisionVariables, fr_equalityConstraints,
-                fr_inequalityConstraints, J, callback, config, true, fr_x, fr_s,
-                status);
-
-  x = fr_x.segment(0, decisionVariables.size());
-}
-
-/**
- * Finds the iterate that minimizes the constraint violation while not deviating
- * too far from the starting point. This is a fallback procedure when the normal
- * interior-point method fails to converge to a feasible point.
- *
- * @param[in] decisionVariables The list of decision variables.
- * @param[in] equalityConstraints The list of equality constraints.
- * @param[in] inequalityConstraints The list of inequality constraints.
- * @param[in] μ Barrier parameter.
- * @param[in] callback The user callback.
- * @param[in] config Configuration options for the solver.
- * @param[in,out] x The current iterate from the normal solve.
- * @param[in,out] s The current inequality constraint slack variables from the
- *   normal solve.
- * @param[out] status The solver status.
- */
-inline void FeasibilityRestoration(
-    std::span<Variable> decisionVariables,
-    std::span<Variable> equalityConstraints,
-    std::span<Variable> inequalityConstraints, double μ,
-    function_ref<bool(const SolverIterationInfo& info)> callback,
-    const SolverConfig& config, Eigen::VectorXd& x, Eigen::VectorXd& s,
-    SolverStatus* status) {
-  // Feasibility restoration
-  //
-  //        min  ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - x_R)ᵀD_R(x - x_R)
-  //         x
-  //       pₑ,nₑ
-  //       pᵢ,nᵢ
-  //
-  //   s.t. cₑ(x) - pₑ + nₑ = 0
-  //        cᵢ(x) - s - pᵢ + nᵢ = 0
-  //        pₑ ≥ 0
-  //        nₑ ≥ 0
-  //        pᵢ ≥ 0
-  //        nᵢ ≥ 0
-  //
-  // where ρ = 1000, ζ = √μ where μ is the barrier parameter, x_R is original
-  // iterate before feasibility restoration, and D_R is a scaling matrix defined
-  // by
-  //
-  //   D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
-
-  constexpr double ρ = 1000.0;
-
-  wpi::SmallVector<Variable> fr_decisionVariables;
-  fr_decisionVariables.reserve(decisionVariables.size() +
-                               2 * equalityConstraints.size() +
-                               2 * inequalityConstraints.size());
-
-  // Assign x
-  fr_decisionVariables.assign(decisionVariables.begin(),
-                              decisionVariables.end());
-
-  // Allocate pₑ, nₑ, pᵢ, and nᵢ
-  for (size_t row = 0;
-       row < 2 * equalityConstraints.size() + 2 * inequalityConstraints.size();
-       ++row) {
-    fr_decisionVariables.emplace_back();
-  }
-
-  auto it = fr_decisionVariables.begin();
-
-  VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
-  it += decisionVariables.size();
-
-  VariableMatrix p_e{std::span{it, it + equalityConstraints.size()}};
-  it += equalityConstraints.size();
-
-  VariableMatrix n_e{std::span{it, it + equalityConstraints.size()}};
-  it += equalityConstraints.size();
-
-  VariableMatrix p_i{std::span{it, it + inequalityConstraints.size()}};
-  it += inequalityConstraints.size();
-
-  VariableMatrix n_i{std::span{it, it + inequalityConstraints.size()}};
-
-  // Set initial values for pₑ, nₑ, pᵢ, and nᵢ.
-  //
-  //
-  // From equation (33) of [2]:
-  //                       ______________________
-  //       μ − ρ c(x)     /(μ − ρ c(x))²   μ c(x)
-  //   n = −−−−−−−−−− +  / (−−−−−−−−−−)  + −−−−−−     (1)
-  //           2ρ       √  (    2ρ    )      2ρ
-  //
-  // The quadratic formula:
-  //             ________
-  //       -b + √b² - 4ac
-  //   x = −−−−−−−−−−−−−−                             (2)
-  //             2a
-  //
-  // Rearrange (1) to fit the quadratic formula better:
-  //                     _________________________
-  //       μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x)
-  //   n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
-  //                         2ρ
-  //
-  // Solve for coefficients:
-  //
-  //   a = ρ                                         (3)
-  //   b = ρ c(x) - μ                                (4)
-  //
-  //   -4ac = μ c(x) 2ρ
-  //   -4(ρ)c = 2ρ μ c(x)
-  //   -4c = 2μ c(x)
-  //   c = -μ c(x)/2                                 (5)
-  //
-  //   p = c(x) + n                                  (6)
-  for (int row = 0; row < p_e.Rows(); ++row) {
-    double c_e = equalityConstraints[row].Value();
-
-    constexpr double a = 2 * ρ;
-    double b = ρ * c_e - μ;
-    double c = -μ * c_e / 2.0;
-
-    double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a);
-    double p = c_e + n;
-
-    p_e(row).SetValue(p);
-    n_e(row).SetValue(n);
-  }
-  for (int row = 0; row < p_i.Rows(); ++row) {
-    double c_i = inequalityConstraints[row].Value() - s(row);
-
-    constexpr double a = 2 * ρ;
-    double b = ρ * c_i - μ;
-    double c = -μ * c_i / 2.0;
-
-    double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a);
-    double p = c_i + n;
-
-    p_i(row).SetValue(p);
-    n_i(row).SetValue(n);
-  }
-
-  // cₑ(x) - pₑ + nₑ = 0
-  wpi::SmallVector<Variable> fr_equalityConstraints;
-  fr_equalityConstraints.assign(equalityConstraints.begin(),
-                                equalityConstraints.end());
-  for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
-    auto& constraint = fr_equalityConstraints[row];
-    constraint = constraint - p_e(row) + n_e(row);
-  }
-
-  // cᵢ(x) - s - pᵢ + nᵢ = 0
-  wpi::SmallVector<Variable> fr_inequalityConstraints;
-  fr_inequalityConstraints.assign(inequalityConstraints.begin(),
-                                  inequalityConstraints.end());
-  for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) {
-    auto& constraint = fr_inequalityConstraints[row];
-    constraint = constraint - s(row) - p_i(row) + n_i(row);
-  }
-
-  // pₑ ≥ 0
-  std::copy(p_e.begin(), p_e.end(),
-            std::back_inserter(fr_inequalityConstraints));
-
-  // pᵢ ≥ 0
-  std::copy(p_i.begin(), p_i.end(),
-            std::back_inserter(fr_inequalityConstraints));
-
-  // nₑ ≥ 0
-  std::copy(n_e.begin(), n_e.end(),
-            std::back_inserter(fr_inequalityConstraints));
-
-  // nᵢ ≥ 0
-  std::copy(n_i.begin(), n_i.end(),
-            std::back_inserter(fr_inequalityConstraints));
-
-  Variable J = 0.0;
-
-  // J += ρ Σ (pₑ + nₑ + pᵢ + nᵢ)
-  for (auto& elem : p_e) {
-    J += elem;
-  }
-  for (auto& elem : p_i) {
-    J += elem;
-  }
-  for (auto& elem : n_e) {
-    J += elem;
-  }
-  for (auto& elem : n_i) {
-    J += elem;
-  }
-  J *= ρ;
-
-  // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
-  Eigen::VectorXd D_R{x.rows()};
-  for (int row = 0; row < D_R.rows(); ++row) {
-    D_R(row) = std::min(1.0, 1.0 / std::abs(x(row)));
-  }
-
-  // J += ζ/2 (x - x_R)ᵀD_R(x - x_R)
-  for (int row = 0; row < x.rows(); ++row) {
-    J += std::sqrt(μ) / 2.0 * D_R(row) * sleipnir::pow(xAD(row) - x(row), 2);
-  }
-
-  Eigen::VectorXd fr_x = VariableMatrix{fr_decisionVariables}.Value();
-
-  // Set up initial value for inequality constraint slack variables
-  Eigen::VectorXd fr_s{fr_inequalityConstraints.size()};
-  fr_s.segment(0, inequalityConstraints.size()) = s;
-  fr_s.segment(inequalityConstraints.size(),
-               fr_s.size() - inequalityConstraints.size())
-      .setOnes();
-
-  InteriorPoint(fr_decisionVariables, fr_equalityConstraints,
-                fr_inequalityConstraints, J, callback, config, true, fr_x, fr_s,
-                status);
-
-  x = fr_x.segment(0, decisionVariables.size());
-  s = fr_s.segment(0, inequalityConstraints.size());
-}
-
-}  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp
index 0c14efd7b8a..ba625b5a85c 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp
@@ -45,9 +45,6 @@ struct FilterEntry {
  */
 class Filter {
  public:
-  static constexpr double γCost = 1e-8;
-  static constexpr double γConstraint = 1e-5;
-
   double maxConstraintViolation = 1e4;
 
   /**
@@ -74,6 +71,11 @@ class Filter {
                           maxConstraintViolation);
   }
 
+  /**
+   * Creates a new Newton's method filter entry.
+   */
+  FilterEntry MakeEntry() { return FilterEntry{m_f->Value(), 0.0}; }
+
   /**
    * Creates a new Sequential Quadratic Programming filter entry.
    *
@@ -131,9 +133,10 @@ class Filter {
    * Returns true if the given iterate is accepted by the filter.
    *
    * @param entry The entry to attempt adding to the filter.
+   * @param α The step size (0, 1].
    */
-  bool TryAdd(const FilterEntry& entry) {
-    if (IsAcceptable(entry)) {
+  bool TryAdd(const FilterEntry& entry, double α) {
+    if (IsAcceptable(entry, α)) {
       Add(entry);
       return true;
     } else {
@@ -145,9 +148,10 @@ class Filter {
    * Returns true if the given iterate is accepted by the filter.
    *
    * @param entry The entry to attempt adding to the filter.
+   * @param α The step size (0, 1].
    */
-  bool TryAdd(FilterEntry&& entry) {
-    if (IsAcceptable(entry)) {
+  bool TryAdd(FilterEntry&& entry, double α) {
+    if (IsAcceptable(entry, α)) {
       Add(std::move(entry));
       return true;
     } else {
@@ -159,23 +163,31 @@ class Filter {
    * Returns true if the given entry is acceptable to the filter.
    *
    * @param entry The entry to check.
+   * @param α The step size (0, 1].
    */
-  bool IsAcceptable(const FilterEntry& entry) {
+  bool IsAcceptable(const FilterEntry& entry, double α) {
     if (!std::isfinite(entry.cost) ||
         !std::isfinite(entry.constraintViolation)) {
       return false;
     }
 
+    double ϕ = std::pow(α, 1.5);
+
     // If current filter entry is better than all prior ones in some respect,
-    // accept it
-    return std::all_of(m_filter.begin(), m_filter.end(), [&](const auto& elem) {
-      return entry.cost <= elem.cost - γCost * elem.constraintViolation ||
+    // accept it.
+    //
+    // See equation (2.13) of [4].
+    return std::ranges::all_of(m_filter, [&](const auto& elem) {
+      return entry.cost <= elem.cost - ϕ * γCost * elem.constraintViolation ||
              entry.constraintViolation <=
-                 (1.0 - γConstraint) * elem.constraintViolation;
+                 (1.0 - ϕ * γConstraint) * elem.constraintViolation;
     });
   }
 
  private:
+  static constexpr double γCost = 1e-8;
+  static constexpr double γConstraint = 1e-5;
+
   Variable* m_f = nullptr;
   wpi::SmallVector<FilterEntry> m_filter;
 };
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp
index 3b603159d21..1bb15daed3b 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp
@@ -9,6 +9,20 @@
 
 namespace sleipnir {
 
+/**
+ * Returns the KKT error for Newton's method.
+ *
+ * @param g Gradient of the cost function ∇f.
+ */
+inline double KKTError(const Eigen::VectorXd& g) {
+  // Compute the KKT error as the 1-norm of the KKT conditions from equations
+  // (19.5a) through (19.5d) of [1].
+  //
+  //   ∇f = 0
+
+  return g.lpNorm<1>();
+}
+
 /**
  * Returns the KKT error for Sequential Quadratic Programming.
  *
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/PrintDiagnostics.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/PrintDiagnostics.hpp
new file mode 100644
index 00000000000..083d221da8f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/PrintDiagnostics.hpp
@@ -0,0 +1,210 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <stdint.h>
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <ranges>
+#include <string>
+
+#include <wpi/SmallVector.h>
+
+#include "sleipnir/util/Print.hpp"
+#include "sleipnir/util/SetupProfiler.hpp"
+#include "sleipnir/util/SolveProfiler.hpp"
+#include "util/ToMs.hpp"
+
+namespace sleipnir {
+
+/**
+ * Iteration mode.
+ */
+enum class IterationMode : uint8_t {
+  /// Normal iteration.
+  kNormal,
+  /// Second-order correction iteration.
+  kSecondOrderCorrection
+};
+
+/**
+ * Prints diagnostics for the current iteration.
+ *
+ * @param iterations Number of iterations.
+ * @param mode Which mode the iteration was in.
+ * @param time The iteration duration.
+ * @param error The error.
+ * @param cost The cost.
+ * @param infeasibility The infeasibility.
+ * @param δ The Hessian regularization factor.
+ * @param primal_α The primal step size.
+ * @param dual_α The dual step size.
+ */
+template <typename Rep, typename Period = std::ratio<1>>
+void PrintIterationDiagnostics(int iterations, IterationMode mode,
+                               const std::chrono::duration<Rep, Period>& time,
+                               double error, double cost, double infeasibility,
+                               double δ, double primal_α, double dual_α) {
+  if (iterations % 20 == 0) {
+    if (iterations == 0) {
+      sleipnir::println(
+          "┏{:━^6}┯{:━^6}┯{:━^11}┯{:━^14}┯{:━^14}┯{:━^15}┯{:━^7}┯{:━^10}┯"
+          "{:━^10}┯{:━^4}┓",
+          "", "", "", "", "", "", "", "", "", "");
+    } else {
+      sleipnir::println(
+          "┢{:━^6}┯{:━^6}┯{:━^11}┯{:━^14}┯{:━^14}┯{:━^15}┯{:━^7}┯{:━^10}┯"
+          "{:━^10}┯{:━^4}┪",
+          "", "", "", "", "", "", "", "", "", "");
+    }
+    sleipnir::println(
+        "┃ {:^4} │ {:^4} │ {:^9} │ {:^12} │ {:^12} │ {:^13} │ {:^5} │ {:^8} │ "
+        "{:^8} │ {:^2} ┃",
+        "iter", "mode", "time (ms)", "error", "cost", "infeasibility", "reg",
+        "primal α", "dual α", "↩");
+    sleipnir::println(
+        "┡{:━^6}┷{:━^6}┷{:━^11}┷{:━^14}┷{:━^14}┷{:━^15}┷{:━^7}┷{:━^10}┷{:━^10}┷"
+        "{:━^4}┩",
+        "", "", "", "", "", "", "", "", "", "");
+  }
+
+  constexpr const char* kIterationModes[] = {"norm", "SOC"};
+  sleipnir::print("│ {:4}   {:4}   {:9.3f}   {:12e}   {:12e}   {:13e}   ",
+                  iterations, kIterationModes[static_cast<uint8_t>(mode)],
+                  ToMs(time), error, cost, infeasibility);
+
+  // Print regularization
+  if (δ == 0.0) {
+    sleipnir::print(" 0   ");
+  } else {
+    int exponent = std::log10(δ);
+
+    if (exponent == 0) {
+      sleipnir::print(" 1   ");
+    } else if (exponent == 1) {
+      sleipnir::print("10   ");
+    } else {
+      // Gather regularization exponent digits
+      int n = std::abs(exponent);
+      wpi::SmallVector<int> digits;
+      do {
+        digits.emplace_back(n % 10);
+        n /= 10;
+      } while (n > 0);
+
+      std::string reg = "10";
+
+      // Append regularization exponent
+      if (exponent < 0) {
+        reg += "⁻";
+      }
+      constexpr const char* strs[] = {"⁰", "¹", "²", "³", "⁴",
+                                      "⁵", "⁶", "⁷", "⁸", "⁹"};
+      for (const auto& digit : digits | std::views::reverse) {
+        reg += strs[digit];
+      }
+
+      sleipnir::print("{:<5}", reg);
+    }
+  }
+
+  // Print step sizes and number of backtracks
+  sleipnir::println("   {:.2e}   {:.2e}   {:2d} │", primal_α, dual_α,
+                    static_cast<int>(-std::log2(primal_α)));
+}
+
+/**
+ * Renders histogram of the given normalized value.
+ *
+ * @tparam Width Width of the histogram in characters.
+ * @param value Normalized value from 0 to 1.
+ */
+template <int Width>
+  requires(Width > 0)
+inline std::string Histogram(double value) {
+  value = std::clamp(value, 0.0, 1.0);
+
+  double ipart;
+  int fpart = static_cast<int>(std::modf(value * Width, &ipart) * 8);
+
+  constexpr const char* strs[] = {" ", "▏", "▎", "▍", "▌", "▋", "▊", "▉", "█"};
+  std::string hist;
+
+  int index = 0;
+  while (index < ipart) {
+    hist += strs[8];
+    ++index;
+  }
+  if (fpart > 0) {
+    hist += strs[fpart];
+    ++index;
+  }
+  while (index < Width) {
+    hist += strs[0];
+    ++index;
+  }
+
+  return hist;
+}
+
+/**
+ * Prints final diagnostics.
+ *
+ * @param iterations Number of iterations.
+ * @param setupProfilers Setup profilers.
+ * @param solveProfilers Solve profilers.
+ */
+inline void PrintFinalDiagnostics(
+    int iterations, const wpi::SmallVector<SetupProfiler>& setupProfilers,
+    const wpi::SmallVector<SolveProfiler>& solveProfilers) {
+  // Print bottom of iteration diagnostics table
+  sleipnir::println("└{:─^106}┘", "");
+
+  // Print total time
+  auto setupDuration = ToMs(setupProfilers[0].Duration());
+  auto solveDuration = ToMs(solveProfilers[0].TotalDuration());
+  sleipnir::println("\nTime: {:.3f} ms", setupDuration + solveDuration);
+  sleipnir::println("  ↳ setup: {:.3f} ms", setupDuration);
+  sleipnir::println("  ↳ solve: {:.3f} ms ({} iterations)", solveDuration,
+                    iterations);
+
+  // Print setup diagnostics
+  sleipnir::println("\n┏{:━^23}┯{:━^20}┯{:━^12}┓", "", "", "");
+  sleipnir::println("┃ {:^21} │ {:^18} │ {:^10} ┃", "trace", "percent",
+                    "total (ms)");
+  sleipnir::println("┡{:━^23}┷{:━^20}┷{:━^12}┩", "", "", "");
+
+  for (auto& profiler : setupProfilers) {
+    double norm =
+        ToMs(profiler.Duration()) / ToMs(setupProfilers[0].Duration());
+    sleipnir::println("│ {:<21}   {:>6.2f}%▕{}▏   {:>10.3f} │", profiler.name,
+                      norm * 100.0, Histogram<9>(norm),
+                      ToMs(profiler.Duration()));
+  }
+
+  sleipnir::println("└{:─^57}┘", "");
+
+  // Print solve diagnostics
+  sleipnir::println("┏{:━^23}┯{:━^20}┯{:━^12}┯{:━^11}┯{:━^6}┓", "", "", "", "",
+                    "");
+  sleipnir::println("┃ {:^21} │ {:^18} │ {:^10} │ {:^9} │ {:^4} ┃", "trace",
+                    "percent", "total (ms)", "each (ms)", "runs");
+  sleipnir::println("┡{:━^23}┷{:━^20}┷{:━^12}┷{:━^11}┷{:━^6}┩", "", "", "", "",
+                    "");
+
+  for (auto& profiler : solveProfilers) {
+    double norm = ToMs(profiler.TotalDuration()) /
+                  ToMs(solveProfilers[0].TotalDuration());
+    sleipnir::println(
+        "│ {:<21}   {:>6.2f}%▕{}▏   {:>10.3f}   {:>9.3f}   {:>4} │",
+        profiler.name, norm * 100.0, Histogram<9>(norm),
+        ToMs(profiler.TotalDuration()), ToMs(profiler.AverageDuration()),
+        profiler.NumSolves());
+  }
+
+  sleipnir::println("└{:─^76}┘\n", "");
+}
+
+}  // namespace sleipnir
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMs.hpp
similarity index 81%
rename from wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp
rename to wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMs.hpp
index 0e004187255..bc063179a5d 100644
--- a/wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMs.hpp
@@ -11,8 +11,7 @@ namespace sleipnir {
  * decimals.
  */
 template <typename Rep, typename Period = std::ratio<1>>
-constexpr double ToMilliseconds(
-    const std::chrono::duration<Rep, Period>& duration) {
+constexpr double ToMs(const std::chrono::duration<Rep, Period>& duration) {
   using std::chrono::duration_cast;
   using std::chrono::microseconds;
   return duration_cast<microseconds>(duration).count() / 1e3;