mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
[wpimath] Rewrite DARE solver (#5328)
I timed the DARE unit tests, and the new solver is 0 to 100% faster in all cases (that is, it's at least as fast as Drake's and up to 2x faster in some cases). The new solver is also much simpler, takes less time to compile, and drops the libwpimath.so size from 325 MB to 301 MB. I think most of the compilation time is coming from the eigenvalue decompositions used to enforce argument preconditions.
This commit is contained in:
Tyler Veness
91
wpimath/src/main/native/include/frc/DARE.h
Normal file
91
wpimath/src/main/native/include/frc/DARE.h
Normal file
@@ -0,0 +1,91 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <wpi/SymbolExports.h>
|
||||
|
||||
#include "Eigen/Core"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
*
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic
|
||||
* Riccati equation:
|
||||
*
|
||||
* AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
*
|
||||
* @param A The system matrix.
|
||||
* @param B The input matrix.
|
||||
* @param Q The state cost matrix.
|
||||
* @param R The input cost matrix.
|
||||
* @throws std::invalid_argument if number of inputs is greater than number of
|
||||
* states.
|
||||
* @throws std::invalid_argument if Q isn't symmetric positive semidefinite.
|
||||
* @throws std::invalid_argument if R isn't symmetric positive definite.
|
||||
* @throws std::invalid_argument if the (A, B) pair isn't stabilizable.
|
||||
* @throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't
|
||||
* detectable.
|
||||
*/
|
||||
WPILIB_DLLEXPORT
|
||||
Eigen::MatrixXd DARE(const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& B,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& Q,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& R);
|
||||
|
||||
/**
|
||||
Computes the unique stabilizing solution X to the discrete-time algebraic
|
||||
Riccati equation:
|
||||
|
||||
AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
|
||||
This overload of the DARE is useful for finding the control law uₖ that
|
||||
minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
|
||||
|
||||
@verbatim
|
||||
∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
k=0
|
||||
@endverbatim
|
||||
|
||||
This is a more general form of the following. The linear-quadratic regulator
|
||||
is the feedback control law uₖ that minimizes the following cost function
|
||||
subject to xₖ₊₁ = Axₖ + Buₖ:
|
||||
|
||||
@verbatim
|
||||
∞
|
||||
J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
k=0
|
||||
@endverbatim
|
||||
|
||||
This can be refactored as:
|
||||
|
||||
@verbatim
|
||||
∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
k=0
|
||||
@endverbatim
|
||||
|
||||
@param A The system matrix.
|
||||
@param B The input matrix.
|
||||
@param Q The state cost matrix.
|
||||
@param R The input cost matrix.
|
||||
@param N The state-input cross cost matrix.
|
||||
@throws std::invalid_argument if number of inputs is greater than number of
|
||||
states.
|
||||
@throws std::invalid_argument if Q − NR⁻¹Nᵀ isn't symmetric positive
|
||||
semidefinite.
|
||||
@throws std::invalid_argument if R isn't symmetric positive definite.
|
||||
@throws std::invalid_argument if the (A, B) pair isn't stabilizable.
|
||||
@throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't detectable.
|
||||
*/
|
||||
WPILIB_DLLEXPORT
|
||||
Eigen::MatrixXd DARE(const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& B,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& Q,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& R,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& N);
|
||||
|
||||
} // namespace frc
|
||||
@@ -8,8 +8,7 @@
|
||||
#include <string>
|
||||
|
||||
#include "Eigen/Cholesky"
|
||||
#include "Eigen/Eigenvalues"
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
#include "frc/DARE.h"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/controller/LinearQuadraticRegulator.h"
|
||||
#include "frc/fmt/Eigen.h"
|
||||
@@ -52,8 +51,7 @@ LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
|
||||
throw std::invalid_argument(msg);
|
||||
}
|
||||
|
||||
Matrixd<States, States> S =
|
||||
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
|
||||
Matrixd<States, States> S = DARE(discA, discB, Q, R);
|
||||
|
||||
// K = (BᵀSB + R)⁻¹BᵀSA
|
||||
m_K = (discB.transpose() * S * discB + R)
|
||||
@@ -72,8 +70,7 @@ LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
|
||||
Matrixd<States, Inputs> discB;
|
||||
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
|
||||
|
||||
Matrixd<States, States> S =
|
||||
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
|
||||
Matrixd<States, States> S = DARE(discA, discB, Q, R, N);
|
||||
|
||||
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
|
||||
m_K = (discB.transpose() * S * discB + R)
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
#pragma once
|
||||
|
||||
#include "Eigen/Cholesky"
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
#include "frc/DARE.h"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/estimator/ExtendedKalmanFilter.h"
|
||||
#include "frc/system/Discretization.h"
|
||||
@@ -41,8 +41,7 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
|
||||
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
|
||||
|
||||
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
|
||||
m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
|
||||
discA.transpose(), C.transpose(), discQ, discR);
|
||||
m_initP = DARE(discA.transpose(), C.transpose(), discQ, discR);
|
||||
} else {
|
||||
m_initP = StateMatrix::Zero();
|
||||
}
|
||||
@@ -77,8 +76,7 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
|
||||
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
|
||||
|
||||
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
|
||||
m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
|
||||
discA.transpose(), C.transpose(), discQ, discR);
|
||||
m_initP = DARE(discA.transpose(), C.transpose(), discQ, discR);
|
||||
} else {
|
||||
m_initP = StateMatrix::Zero();
|
||||
}
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
#include <string>
|
||||
|
||||
#include "Eigen/Cholesky"
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
#include "frc/DARE.h"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/estimator/KalmanFilter.h"
|
||||
#include "frc/system/Discretization.h"
|
||||
@@ -47,8 +47,8 @@ KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
|
||||
throw std::invalid_argument(msg);
|
||||
}
|
||||
|
||||
Matrixd<States, States> P = drake::math::DiscreteAlgebraicRiccatiEquation(
|
||||
discA.transpose(), C.transpose(), discQ, discR);
|
||||
Matrixd<States, States> P =
|
||||
DARE(discA.transpose(), C.transpose(), discQ, discR);
|
||||
|
||||
// S = CPCᵀ + R
|
||||
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
|
||||
|
||||
Reference in New Issue
Block a user