drake::math Namespace Reference

Functions
WPILIB_DLLEXPORT Eigen::MatrixXd	DiscreteAlgebraicRiccatiEquation (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: More...
WPILIB_DLLEXPORT Eigen::MatrixXd	DiscreteAlgebraicRiccatiEquation (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)
	Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation: More...

Function Documentation

DiscreteAlgebraicRiccatiEquation() [1/2]

WPILIB_DLLEXPORT Eigen::MatrixXd drake::math::DiscreteAlgebraicRiccatiEquation	(	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(BᵀXB + R)⁻¹BᵀXA + Q = 0

Exceptions

std::exception	if Q is not positive semi-definite.
std::exception	if R is not positive definite.

Based on the Schur Vector approach outlined in this paper: "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation" by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell

DiscreteAlgebraicRiccatiEquation() [2/2]

WPILIB_DLLEXPORT Eigen::MatrixXd drake::math::DiscreteAlgebraicRiccatiEquation	(	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
	)

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 is equivalent to solving the original DARE:

A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0

where A₂ and Q₂ are a change of variables:

A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ

This overload of the DARE is useful for finding the control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.

    ∞ [xₖ]ᵀ[Q  N][xₖ]
J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
   k=0

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ₖ:

    ∞
J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
   k=0

This can be refactored as:

    ∞ [xₖ]ᵀ[Q 0][xₖ]
J = Σ [uₖ] [0 R][uₖ] ΔT
   k=0

Exceptions

std::runtime_error	if Q − NR⁻¹Nᵀ is not positive semi-definite.
std::runtime_error	if R is not positive definite.