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.numbers.N1;
010
011public class LinearSystem<States extends Num, Inputs extends Num, Outputs extends Num> {
012  /** Continuous system matrix. */
013  private final Matrix<States, States> m_A;
014
015  /** Continuous input matrix. */
016  private final Matrix<States, Inputs> m_B;
017
018  /** Output matrix. */
019  private final Matrix<Outputs, States> m_C;
020
021  /** Feedthrough matrix. */
022  private final Matrix<Outputs, Inputs> m_D;
023
024  /**
025   * Construct a new LinearSystem from the four system matrices.
026   *
027   * @param A The system matrix A.
028   * @param B The input matrix B.
029   * @param C The output matrix C.
030   * @param D The feedthrough matrix D.
031   * @throws IllegalArgumentException if any matrix element isn't finite.
032   */
033  public LinearSystem(
034      Matrix<States, States> A,
035      Matrix<States, Inputs> B,
036      Matrix<Outputs, States> C,
037      Matrix<Outputs, Inputs> D) {
038    for (int row = 0; row < A.getNumRows(); ++row) {
039      for (int col = 0; col < A.getNumCols(); ++col) {
040        if (!Double.isFinite(A.get(row, col))) {
041          throw new IllegalArgumentException(
042              "Elements of A aren't finite. This is usually due to model implementation errors.");
043        }
044      }
045    }
046    for (int row = 0; row < B.getNumRows(); ++row) {
047      for (int col = 0; col < B.getNumCols(); ++col) {
048        if (!Double.isFinite(B.get(row, col))) {
049          throw new IllegalArgumentException(
050              "Elements of B aren't finite. This is usually due to model implementation errors.");
051        }
052      }
053    }
054    for (int row = 0; row < C.getNumRows(); ++row) {
055      for (int col = 0; col < C.getNumCols(); ++col) {
056        if (!Double.isFinite(C.get(row, col))) {
057          throw new IllegalArgumentException(
058              "Elements of C aren't finite. This is usually due to model implementation errors.");
059        }
060      }
061    }
062    for (int row = 0; row < D.getNumRows(); ++row) {
063      for (int col = 0; col < D.getNumCols(); ++col) {
064        if (!Double.isFinite(D.get(row, col))) {
065          throw new IllegalArgumentException(
066              "Elements of D aren't finite. This is usually due to model implementation errors.");
067        }
068      }
069    }
070
071    this.m_A = A;
072    this.m_B = B;
073    this.m_C = C;
074    this.m_D = D;
075  }
076
077  /**
078   * Returns the system matrix A.
079   *
080   * @return the system matrix A.
081   */
082  public Matrix<States, States> getA() {
083    return m_A;
084  }
085
086  /**
087   * Returns an element of the system matrix A.
088   *
089   * @param row Row of A.
090   * @param col Column of A.
091   * @return the system matrix A at (i, j).
092   */
093  public double getA(int row, int col) {
094    return m_A.get(row, col);
095  }
096
097  /**
098   * Returns the input matrix B.
099   *
100   * @return the input matrix B.
101   */
102  public Matrix<States, Inputs> getB() {
103    return m_B;
104  }
105
106  /**
107   * Returns an element of the input matrix B.
108   *
109   * @param row Row of B.
110   * @param col Column of B.
111   * @return The value of the input matrix B at (i, j).
112   */
113  public double getB(int row, int col) {
114    return m_B.get(row, col);
115  }
116
117  /**
118   * Returns the output matrix C.
119   *
120   * @return Output matrix C.
121   */
122  public Matrix<Outputs, States> getC() {
123    return m_C;
124  }
125
126  /**
127   * Returns an element of the output matrix C.
128   *
129   * @param row Row of C.
130   * @param col Column of C.
131   * @return the double value of C at the given position.
132   */
133  public double getC(int row, int col) {
134    return m_C.get(row, col);
135  }
136
137  /**
138   * Returns the feedthrough matrix D.
139   *
140   * @return the feedthrough matrix D.
141   */
142  public Matrix<Outputs, Inputs> getD() {
143    return m_D;
144  }
145
146  /**
147   * Returns an element of the feedthrough matrix D.
148   *
149   * @param row Row of D.
150   * @param col Column of D.
151   * @return The feedthrough matrix D at (i, j).
152   */
153  public double getD(int row, int col) {
154    return m_D.get(row, col);
155  }
156
157  /**
158   * Computes the new x given the old x and the control input.
159   *
160   * <p>This is used by state observers directly to run updates based on state estimate.
161   *
162   * @param x The current state.
163   * @param clampedU The control input.
164   * @param dtSeconds Timestep for model update.
165   * @return the updated x.
166   */
167  public Matrix<States, N1> calculateX(
168      Matrix<States, N1> x, Matrix<Inputs, N1> clampedU, double dtSeconds) {
169    var discABpair = Discretization.discretizeAB(m_A, m_B, dtSeconds);
170
171    return (discABpair.getFirst().times(x)).plus(discABpair.getSecond().times(clampedU));
172  }
173
174  /**
175   * Computes the new y given the control input.
176   *
177   * <p>This is used by state observers directly to run updates based on state estimate.
178   *
179   * @param x The current state.
180   * @param clampedU The control input.
181   * @return the updated output matrix Y.
182   */
183  public Matrix<Outputs, N1> calculateY(Matrix<States, N1> x, Matrix<Inputs, N1> clampedU) {
184    return m_C.times(x).plus(m_D.times(clampedU));
185  }
186
187  @Override
188  public String toString() {
189    return String.format(
190        "Linear System: A\n%s\n\nB:\n%s\n\nC:\n%s\n\nD:\n%s\n",
191        m_A.toString(), m_B.toString(), m_C.toString(), m_D.toString());
192  }
193}