001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.math; 006 007import org.ejml.simple.SimpleMatrix; 008 009public final class DARE { 010 private DARE() { 011 throw new UnsupportedOperationException("This is a utility class!"); 012 } 013 014 /** 015 * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation. 016 * 017 * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 018 * 019 * <p>This internal function skips expensive precondition checks for increased performance. The 020 * solver may hang if any of the following occur: 021 * 022 * <ul> 023 * <li>Q isn't symmetric positive semidefinite 024 * <li>R isn't symmetric positive definite 025 * <li>The (A, B) pair isn't stabilizable 026 * <li>The (A, C) pair where Q = CᵀC isn't detectable 027 * </ul> 028 * 029 * <p>Only use this function if you're sure the preconditions are met. 030 * 031 * @param <States> Number of states. 032 * @param <Inputs> Number of inputs. 033 * @param A System matrix. 034 * @param B Input matrix. 035 * @param Q State cost matrix. 036 * @param R Input cost matrix. 037 * @return Solution of DARE. 038 */ 039 public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail( 040 Matrix<States, States> A, 041 Matrix<States, Inputs> B, 042 Matrix<States, States> Q, 043 Matrix<Inputs, Inputs> R) { 044 var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols())); 045 WPIMathJNI.dareDetailABQR( 046 A.getStorage().getDDRM().getData(), 047 B.getStorage().getDDRM().getData(), 048 Q.getStorage().getDDRM().getData(), 049 R.getStorage().getDDRM().getData(), 050 A.getNumCols(), 051 B.getNumCols(), 052 S.getStorage().getDDRM().getData()); 053 return S; 054 } 055 056 /** 057 * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation. 058 * 059 * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0 060 * 061 * <p>This is equivalent to solving the original DARE: 062 * 063 * <p>A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0 064 * 065 * <p>where A₂ and Q₂ are a change of variables: 066 * 067 * <p>A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ 068 * 069 * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the 070 * following cost function subject to xₖ₊₁ = Axₖ + Buₖ. 071 * 072 * <pre> 073 * ∞ [xₖ]ᵀ[Q N][xₖ] 074 * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT 075 * k=0 076 * </pre> 077 * 078 * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback 079 * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ: 080 * 081 * <pre> 082 * ∞ 083 * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT 084 * k=0 085 * </pre> 086 * 087 * <p>This can be refactored as: 088 * 089 * <pre> 090 * ∞ [xₖ]ᵀ[Q 0][xₖ] 091 * J = Σ [uₖ] [0 R][uₖ] ΔT 092 * k=0 093 * </pre> 094 * 095 * <p>This internal function skips expensive precondition checks for increased performance. The 096 * solver may hang if any of the following occur: 097 * 098 * <ul> 099 * <li>Q₂ isn't symmetric positive semidefinite 100 * <li>R isn't symmetric positive definite 101 * <li>The (A₂, B) pair isn't stabilizable 102 * <li>The (A₂, C) pair where Q₂ = CᵀC isn't detectable 103 * </ul> 104 * 105 * <p>Only use this function if you're sure the preconditions are met. 106 * 107 * @param <States> Number of states. 108 * @param <Inputs> Number of inputs. 109 * @param A System matrix. 110 * @param B Input matrix. 111 * @param Q State cost matrix. 112 * @param R Input cost matrix. 113 * @param N State-input cross-term cost matrix. 114 * @return Solution of DARE. 115 */ 116 public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail( 117 Matrix<States, States> A, 118 Matrix<States, Inputs> B, 119 Matrix<States, States> Q, 120 Matrix<Inputs, Inputs> R, 121 Matrix<States, Inputs> N) { 122 var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols())); 123 WPIMathJNI.dareDetailABQRN( 124 A.getStorage().getDDRM().getData(), 125 B.getStorage().getDDRM().getData(), 126 Q.getStorage().getDDRM().getData(), 127 R.getStorage().getDDRM().getData(), 128 N.getStorage().getDDRM().getData(), 129 A.getNumCols(), 130 B.getNumCols(), 131 S.getStorage().getDDRM().getData()); 132 return S; 133 } 134 135 /** 136 * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation. 137 * 138 * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 139 * 140 * @param <States> Number of states. 141 * @param <Inputs> Number of inputs. 142 * @param A System matrix. 143 * @param B Input matrix. 144 * @param Q State cost matrix. 145 * @param R Input cost matrix. 146 * @return Solution of DARE. 147 * @throws IllegalArgumentException if Q isn't symmetric positive semidefinite. 148 * @throws IllegalArgumentException if R isn't symmetric positive definite. 149 * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable. 150 * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable. 151 */ 152 public static <States extends Num, Inputs extends Num> Matrix<States, States> dare( 153 Matrix<States, States> A, 154 Matrix<States, Inputs> B, 155 Matrix<States, States> Q, 156 Matrix<Inputs, Inputs> R) { 157 var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols())); 158 WPIMathJNI.dareABQR( 159 A.getStorage().getDDRM().getData(), 160 B.getStorage().getDDRM().getData(), 161 Q.getStorage().getDDRM().getData(), 162 R.getStorage().getDDRM().getData(), 163 A.getNumCols(), 164 B.getNumCols(), 165 S.getStorage().getDDRM().getData()); 166 return S; 167 } 168 169 /** 170 * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation. 171 * 172 * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0 173 * 174 * <p>This is equivalent to solving the original DARE: 175 * 176 * <p>A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0 177 * 178 * <p>where A₂ and Q₂ are a change of variables: 179 * 180 * <p>A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ 181 * 182 * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the 183 * following cost function subject to xₖ₊₁ = Axₖ + Buₖ. 184 * 185 * <pre> 186 * ∞ [xₖ]ᵀ[Q N][xₖ] 187 * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT 188 * k=0 189 * </pre> 190 * 191 * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback 192 * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ: 193 * 194 * <pre> 195 * ∞ 196 * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT 197 * k=0 198 * </pre> 199 * 200 * <p>This can be refactored as: 201 * 202 * <pre> 203 * ∞ [xₖ]ᵀ[Q 0][xₖ] 204 * J = Σ [uₖ] [0 R][uₖ] ΔT 205 * k=0 206 * </pre> 207 * 208 * @param <States> Number of states. 209 * @param <Inputs> Number of inputs. 210 * @param A System matrix. 211 * @param B Input matrix. 212 * @param Q State cost matrix. 213 * @param R Input cost matrix. 214 * @param N State-input cross-term cost matrix. 215 * @return Solution of DARE. 216 * @throws IllegalArgumentException if Q₂ isn't symmetric positive semidefinite. 217 * @throws IllegalArgumentException if R isn't symmetric positive definite. 218 * @throws IllegalArgumentException if the (A₂, B) pair isn't stabilizable. 219 * @throws IllegalArgumentException if the (A₂, C) pair where Q₂ = CᵀC isn't detectable. 220 */ 221 public static <States extends Num, Inputs extends Num> Matrix<States, States> dare( 222 Matrix<States, States> A, 223 Matrix<States, Inputs> B, 224 Matrix<States, States> Q, 225 Matrix<Inputs, Inputs> R, 226 Matrix<States, Inputs> N) { 227 var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols())); 228 WPIMathJNI.dareABQRN( 229 A.getStorage().getDDRM().getData(), 230 B.getStorage().getDDRM().getData(), 231 Q.getStorage().getDDRM().getData(), 232 R.getStorage().getDDRM().getData(), 233 N.getStorage().getDDRM().getData(), 234 A.getNumCols(), 235 B.getNumCols(), 236 S.getStorage().getDDRM().getData()); 237 return S; 238 } 239}