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