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.jni;
006
007/** DARE JNI. */
008public final class DAREJNI extends WPIMathJNI {
009  /**
010   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
011   *
012   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
013   *
014   * <p>This internal function skips expensive precondition checks for increased performance. The
015   * solver may hang if any of the following occur:
016   *
017   * <ul>
018   *   <li>Q isn't symmetric positive semidefinite
019   *   <li>R isn't symmetric positive definite
020   *   <li>The (A, B) pair isn't stabilizable
021   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
022   * </ul>
023   *
024   * <p>Only use this function if you're sure the preconditions are met. Solves the discrete
025   * algebraic Riccati equation.
026   *
027   * @param A Array containing elements of A in row-major order.
028   * @param B Array containing elements of B in row-major order.
029   * @param Q Array containing elements of Q in row-major order.
030   * @param R Array containing elements of R in row-major order.
031   * @param states Number of states in A matrix.
032   * @param inputs Number of inputs in B matrix.
033   * @param S Array storage for DARE solution.
034   */
035  public static native void dareNoPrecondABQR(
036      double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
037
038  /**
039   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
040   *
041   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
042   *
043   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
044   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
045   *
046   * <pre>
047   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
048   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
049   *    k=0
050   * </pre>
051   *
052   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
053   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
054   *
055   * <pre>
056   *     ∞
057   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
058   *    k=0
059   * </pre>
060   *
061   * <p>This can be refactored as:
062   *
063   * <pre>
064   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
065   * J = Σ [uₖ] [0 R][uₖ] ΔT
066   *    k=0
067   * </pre>
068   *
069   * <p>This internal function skips expensive precondition checks for increased performance. The
070   * solver may hang if any of the following occur:
071   *
072   * <ul>
073   *   <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
074   *   <li>R isn't symmetric positive definite
075   *   <li>The (A − BR⁻¹Nᵀ, B) pair isn't stabilizable
076   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
077   * </ul>
078   *
079   * <p>Only use this function if you're sure the preconditions are met.
080   *
081   * @param A Array containing elements of A in row-major order.
082   * @param B Array containing elements of B in row-major order.
083   * @param Q Array containing elements of Q in row-major order.
084   * @param R Array containing elements of R in row-major order.
085   * @param N Array containing elements of N in row-major order.
086   * @param states Number of states in A matrix.
087   * @param inputs Number of inputs in B matrix.
088   * @param S Array storage for DARE solution.
089   */
090  public static native void dareNoPrecondABQRN(
091      double[] A,
092      double[] B,
093      double[] Q,
094      double[] R,
095      double[] N,
096      int states,
097      int inputs,
098      double[] S);
099
100  /**
101   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
102   *
103   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
104   *
105   * @param A Array containing elements of A in row-major order.
106   * @param B Array containing elements of B in row-major order.
107   * @param Q Array containing elements of Q in row-major order.
108   * @param R Array containing elements of R in row-major order.
109   * @param states Number of states in A matrix.
110   * @param inputs Number of inputs in B matrix.
111   * @param S Array storage for DARE solution.
112   * @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
113   * @throws IllegalArgumentException if R isn't symmetric positive definite.
114   * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
115   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
116   */
117  public static native void dareABQR(
118      double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
119
120  /**
121   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
122   *
123   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
124   *
125   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
126   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
127   *
128   * <pre>
129   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
130   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
131   *    k=0
132   * </pre>
133   *
134   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
135   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
136   *
137   * <pre>
138   *     ∞
139   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
140   *    k=0
141   * </pre>
142   *
143   * <p>This can be refactored as:
144   *
145   * <pre>
146   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
147   * J = Σ [uₖ] [0 R][uₖ] ΔT
148   *    k=0
149   * </pre>
150   *
151   * @param A Array containing elements of A in row-major order.
152   * @param B Array containing elements of B in row-major order.
153   * @param Q Array containing elements of Q in row-major order.
154   * @param R Array containing elements of R in row-major order.
155   * @param N Array containing elements of N in row-major order.
156   * @param states Number of states in A matrix.
157   * @param inputs Number of inputs in B matrix.
158   * @param S Array storage for DARE solution.
159   * @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
160   * @throws IllegalArgumentException if R isn't symmetric positive definite.
161   * @throws IllegalArgumentException if the (A − BR⁻¹Nᵀ, B) pair isn't stabilizable.
162   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
163   */
164  public static native void dareABQRN(
165      double[] A,
166      double[] B,
167      double[] Q,
168      double[] R,
169      double[] N,
170      int states,
171      int inputs,
172      double[] S);
173
174  /** Utility class. */
175  private DAREJNI() {}
176}