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   * Returns a discretized version of the provided continuous measurement noise covariance matrix.
113   * Note that dt=0.0 divides R by zero.
114   *
115   * @param <O> Nat representing the number of outputs.
116   * @param contR Continuous measurement noise covariance matrix.
117   * @param dtSeconds Discretization timestep.
118   * @return Discretized version of the provided continuous measurement noise covariance matrix.
119   */
120  public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> contR, double dtSeconds) {
121    // R_d = 1/T R
122    return contR.div(dtSeconds);
123  }
124}