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