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}