Faster DARE solver

[calcmogul]

I thought you may be interested that I contributed a new DARE solver to WPILib that's 2.8x faster than SLICOT, the solver you use now, based on roboRIO benchmarking on a 5 state, 2 input differential drive LQR problem.

The gist of the algorithm from DOI 10.1080/00207170410001714988 is:

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/LU>

template <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(
    const Eigen::Matrix<double, States, States>& A,
    const Eigen::Matrix<double, States, Inputs>& B,
    const Eigen::Matrix<double, States, States>& Q,
    const Eigen::Matrix<double, Inputs, Inputs>& R) {
  // [1] E. K.-W. Chu, H.-Y. Fan, W.-W. Lin & C.-S. Wang
  //     "Structure-Preserving Algorithms for Periodic Discrete-Time
  //     Algebraic Riccati Equations",
  //     International Journal of Control, 77:8, 767-788, 2004.
  //     DOI: 10.1080/00207170410001714988
  //
  // Implements SDA algorithm on p. 5 of [1] (initial A, G, H are from (4)).
  using StateMatrix = Eigen::Matrix<double, States, States>;

  StateMatrix A_k = A;
  StateMatrix G_k = B * R.llt().solve(B.transpose());
  StateMatrix H_k;
  StateMatrix H_k1 = Q;

  do {
    H_k = H_k1;

    StateMatrix W = StateMatrix::Identity() + G_k * H_k;
    auto W_solver = W.lu();

    StateMatrix V_1 = W_solver.solve(A_k);

    // Solve V₂Wᵀ = Gₖ for V₂
    //
    // V₂Wᵀ = Gₖ
    // (V₂Wᵀ)ᵀ = Gₖᵀ
    // WV₂ᵀ = Gₖᵀ
    // V₂ᵀ = W.solve(Gₖᵀ)
    // V₂ = W.solve(Gₖᵀ)ᵀ
    StateMatrix V_2 = W_solver.solve(G_k.transpose()).transpose();

    G_k += A_k * V_2 * A_k.transpose();
    H_k1 = H_k + V_1.transpose() * H_k * A_k;
    A_k *= V_1;
  } while ((H_k1 - H_k).norm() > 1e-10 * H_k1.norm());

  return H_k1;
}

The preconditions necessary for convergence are:

1. Q is symmetric positive semidefinite
2. R is symmetric positive definite
3. The (A, B) pair is stabilizable
4. The (A, C) pair where Q = CᵀC is detectable

The paper proves convergence under weaker conditions, but it seems to involve solving a generalized eigenvalue problem with the QZ algorithm. SLICOT and Drake use that to solve the whole problem, so it seemed too expensive to bother attempting.

The precondition checks turned out to be 50-60% of the total algorithm runtime, so WPILib exposed a function that skips them if the user knows they'll be satisfied. This would be a good candidate for your Kalman filter error covariance init code, since a comment in there mentioned it didn't use the DARE solver because it had unnecessary checks.

Here's WPILib's impl, which supports static sizing (for performance) and dynamic sizing (for JNI) and throws exceptions on precondition violations. https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/native/include/frc/DARE.h

I'd recommend std::expected instead of exceptions for your use case.

[jkuszmaul]

Thanks for the heads up! May swap this in if we end up leaning heavily on this again. I don't think that DARE solving has been the bottleneck on anything recently, although discretization definitely has been, so this'll be helpful.