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.system;
006
007import edu.wpi.first.math.Matrix;
008import edu.wpi.first.math.Num;
009import edu.wpi.first.math.Pair;
010import org.ejml.simple.SimpleMatrix;
011
012public final class Discretization {
013  private Discretization() {
014    // Utility class
015  }
016
017  /**
018   * Discretizes the given continuous A matrix.
019   *
020   * @param <States> Num representing the number of states.
021   * @param contA Continuous system matrix.
022   * @param dtSeconds Discretization timestep.
023   * @return the discrete matrix system.
024   */
025  public static <States extends Num> Matrix<States, States> discretizeA(
026      Matrix<States, States> contA, double dtSeconds) {
027    // A_d = eᴬᵀ
028    return contA.times(dtSeconds).exp();
029  }
030
031  /**
032   * Discretizes the given continuous A and B matrices.
033   *
034   * @param <States> Nat representing the states of the system.
035   * @param <Inputs> Nat representing the inputs to the system.
036   * @param contA Continuous system matrix.
037   * @param contB Continuous input matrix.
038   * @param dtSeconds Discretization timestep.
039   * @return a Pair representing discA and diskB.
040   */
041  public static <States extends Num, Inputs extends Num>
042      Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeAB(
043          Matrix<States, States> contA, Matrix<States, Inputs> contB, double dtSeconds) {
044    int states = contA.getNumRows();
045    int inputs = contB.getNumCols();
046
047    // M = [A  B]
048    //     [0  0]
049    var M = new Matrix<>(new SimpleMatrix(states + inputs, states + inputs));
050    M.assignBlock(0, 0, contA);
051    M.assignBlock(0, contA.getNumCols(), contB);
052
053    //  ϕ = eᴹᵀ = [A_d  B_d]
054    //            [ 0    I ]
055    var phi = M.times(dtSeconds).exp();
056
057    var discA = new Matrix<States, States>(new SimpleMatrix(states, states));
058    discA.extractFrom(0, 0, phi);
059
060    var discB = new Matrix<States, Inputs>(new SimpleMatrix(states, inputs));
061    discB.extractFrom(0, contB.getNumRows(), phi);
062
063    return new Pair<>(discA, discB);
064  }
065
066  /**
067   * Discretizes the given continuous A and Q matrices.
068   *
069   * @param <States> Nat representing the number of states.
070   * @param contA Continuous system matrix.
071   * @param contQ Continuous process noise covariance matrix.
072   * @param dtSeconds Discretization timestep.
073   * @return a pair representing the discrete system matrix and process noise covariance matrix.
074   */
075  public static <States extends Num>
076      Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQ(
077          Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
078    int states = contA.getNumRows();
079
080    // Make continuous Q symmetric if it isn't already
081    Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
082
083    // M = [−A  Q ]
084    //     [ 0  Aᵀ]
085    final var M = new Matrix<>(new SimpleMatrix(2 * states, 2 * states));
086    M.assignBlock(0, 0, contA.times(-1.0));
087    M.assignBlock(0, states, Q);
088    M.assignBlock(states, 0, new Matrix<>(new SimpleMatrix(states, states)));
089    M.assignBlock(states, states, contA.transpose());
090
091    // ϕ = eᴹᵀ = [−A_d  A_d⁻¹Q_d]
092    //           [ 0      A_dᵀ  ]
093    final var phi = M.times(dtSeconds).exp();
094
095    // ϕ₁₂ = A_d⁻¹Q_d
096    final Matrix<States, States> phi12 = phi.block(states, states, 0, states);
097
098    // ϕ₂₂ = A_dᵀ
099    final Matrix<States, States> phi22 = phi.block(states, states, states, states);
100
101    final var discA = phi22.transpose();
102
103    Q = discA.times(phi12);
104
105    // Make discrete Q symmetric if it isn't already
106    final var discQ = Q.plus(Q.transpose()).div(2.0);
107
108    return new Pair<>(discA, discQ);
109  }
110
111  /**
112   * Discretizes the given continuous A and Q matrices.
113   *
114   * <p>Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which is expensive),
115   * we take advantage of the structure of the block matrix of A and Q.
116   *
117   * <ul>
118   *   <li>eᴬᵀ, which is only N x N, is relatively cheap.
119   *   <li>The upper-right quarter of the 2N x 2N matrix, which we can approximate using a taylor
120   *       series to several terms and still be substantially cheaper than taking the big
121   *       exponential.
122   * </ul>
123   *
124   * @param <States> Nat representing the number of states.
125   * @param contA Continuous system matrix.
126   * @param contQ Continuous process noise covariance matrix.
127   * @param dtSeconds Discretization timestep.
128   * @return a pair representing the discrete system matrix and process noise covariance matrix.
129   */
130  public static <States extends Num>
131      Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQTaylor(
132          Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
133    //       T
134    // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
135    //       0
136    //
137    // M = [−A  Q ]
138    //     [ 0  Aᵀ]
139    // ϕ = eᴹᵀ
140    // ϕ₁₂ = A_d⁻¹Q_d
141    //
142    // Taylor series of ϕ:
143    //
144    //   ϕ = eᴹᵀ = I + MT + 1/2 M²T² + 1/6 M³T³ + …
145    //   ϕ = eᴹᵀ = I + MT + 1/2 T²M² + 1/6 T³M³ + …
146    //
147    // Taylor series of ϕ expanded for ϕ₁₂:
148    //
149    //   ϕ₁₂ = 0 + QT + 1/2 T² (−AQ + QAᵀ) + 1/6 T³ (−A lastTerm + Q Aᵀ²) + …
150    //
151    // ```
152    // lastTerm = Q
153    // lastCoeff = T
154    // ATn = Aᵀ
155    // ϕ₁₂ = lastTerm lastCoeff = QT
156    //
157    // for i in range(2, 6):
158    //   // i = 2
159    //   lastTerm = −A lastTerm + Q ATn = −AQ + QAᵀ
160    //   lastCoeff *= T/i → lastCoeff *= T/2 = 1/2 T²
161    //   ATn *= Aᵀ = Aᵀ²
162    //
163    //   // i = 3
164    //   lastTerm = −A lastTerm + Q ATn = −A (−AQ + QAᵀ) + QAᵀ² = …
165    //   …
166    // ```
167
168    // Make continuous Q symmetric if it isn't already
169    Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
170
171    Matrix<States, States> lastTerm = Q.copy();
172    double lastCoeff = dtSeconds;
173
174    // Aᵀⁿ
175    Matrix<States, States> ATn = contA.transpose();
176
177    Matrix<States, States> phi12 = lastTerm.times(lastCoeff);
178
179    // i = 6 i.e. 5th order should be enough precision
180    for (int i = 2; i < 6; ++i) {
181      lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(ATn));
182      lastCoeff *= dtSeconds / ((double) i);
183
184      phi12 = phi12.plus(lastTerm.times(lastCoeff));
185
186      ATn = ATn.times(contA.transpose());
187    }
188
189    var discA = discretizeA(contA, dtSeconds);
190    Q = discA.times(phi12);
191
192    // Make Q symmetric if it isn't already
193    var discQ = Q.plus(Q.transpose()).div(2.0);
194
195    return new Pair<>(discA, discQ);
196  }
197
198  /**
199   * Returns a discretized version of the provided continuous measurement noise covariance matrix.
200   * Note that dt=0.0 divides R by zero.
201   *
202   * @param <O> Nat representing the number of outputs.
203   * @param contR Continuous measurement noise covariance matrix.
204   * @param dtSeconds Discretization timestep.
205   * @return Discretized version of the provided continuous measurement noise covariance matrix.
206   */
207  public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> contR, double dtSeconds) {
208    // R_d = 1/T R
209    return contR.div(dtSeconds);
210  }
211}