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