Source code

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;
010import edu.wpi.first.math.numbers.N10;
011import edu.wpi.first.math.numbers.N11;
012import edu.wpi.first.math.numbers.N12;
013import edu.wpi.first.math.numbers.N13;
014import edu.wpi.first.math.numbers.N14;
015import edu.wpi.first.math.numbers.N15;
016import edu.wpi.first.math.numbers.N16;
017import edu.wpi.first.math.numbers.N17;
018import edu.wpi.first.math.numbers.N18;
019import edu.wpi.first.math.numbers.N19;
020import edu.wpi.first.math.numbers.N2;
021import edu.wpi.first.math.numbers.N20;
022import edu.wpi.first.math.numbers.N3;
023import edu.wpi.first.math.numbers.N4;
024import edu.wpi.first.math.numbers.N5;
025import edu.wpi.first.math.numbers.N6;
026import edu.wpi.first.math.numbers.N7;
027import edu.wpi.first.math.numbers.N8;
028import edu.wpi.first.math.numbers.N9;
029import java.util.Arrays;
030import java.util.Collections;
031import java.util.List;
032import java.util.stream.Collectors;
033import org.ejml.simple.SimpleMatrix;
034
035/**
036 * A plant defined using state-space notation.
037 *
038 * <p>A plant is a mathematical model of a system's dynamics.
039 *
040 * <p>For more on the underlying math, read
041 * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
042 *
043 * @param <States> Number of states.
044 * @param <Inputs> Number of inputs.
045 * @param <Outputs> Number of outputs.
046 */
047public class LinearSystem<States extends Num, Inputs extends Num, Outputs extends Num> {
048  /** Continuous system matrix. */
049  private final Matrix<States, States> m_A;
050
051  /** Continuous input matrix. */
052  private final Matrix<States, Inputs> m_B;
053
054  /** Output matrix. */
055  private final Matrix<Outputs, States> m_C;
056
057  /** Feedthrough matrix. */
058  private final Matrix<Outputs, Inputs> m_D;
059
060  /**
061   * Construct a new LinearSystem from the four system matrices.
062   *
063   * @param A The system matrix A.
064   * @param B The input matrix B.
065   * @param C The output matrix C.
066   * @param D The feedthrough matrix D.
067   * @throws IllegalArgumentException if any matrix element isn't finite.
068   */
069  public LinearSystem(
070      Matrix<States, States> A,
071      Matrix<States, Inputs> B,
072      Matrix<Outputs, States> C,
073      Matrix<Outputs, Inputs> D) {
074    for (int row = 0; row < A.getNumRows(); ++row) {
075      for (int col = 0; col < A.getNumCols(); ++col) {
076        if (!Double.isFinite(A.get(row, col))) {
077          throw new IllegalArgumentException(
078              "Elements of A aren't finite. This is usually due to model implementation errors.");
079        }
080      }
081    }
082    for (int row = 0; row < B.getNumRows(); ++row) {
083      for (int col = 0; col < B.getNumCols(); ++col) {
084        if (!Double.isFinite(B.get(row, col))) {
085          throw new IllegalArgumentException(
086              "Elements of B aren't finite. This is usually due to model implementation errors.");
087        }
088      }
089    }
090    for (int row = 0; row < C.getNumRows(); ++row) {
091      for (int col = 0; col < C.getNumCols(); ++col) {
092        if (!Double.isFinite(C.get(row, col))) {
093          throw new IllegalArgumentException(
094              "Elements of C aren't finite. This is usually due to model implementation errors.");
095        }
096      }
097    }
098    for (int row = 0; row < D.getNumRows(); ++row) {
099      for (int col = 0; col < D.getNumCols(); ++col) {
100        if (!Double.isFinite(D.get(row, col))) {
101          throw new IllegalArgumentException(
102              "Elements of D aren't finite. This is usually due to model implementation errors.");
103        }
104      }
105    }
106
107    this.m_A = A;
108    this.m_B = B;
109    this.m_C = C;
110    this.m_D = D;
111  }
112
113  /**
114   * Returns the system matrix A.
115   *
116   * @return the system matrix A.
117   */
118  public Matrix<States, States> getA() {
119    return m_A;
120  }
121
122  /**
123   * Returns an element of the system matrix A.
124   *
125   * @param row Row of A.
126   * @param col Column of A.
127   * @return the system matrix A at (i, j).
128   */
129  public double getA(int row, int col) {
130    return m_A.get(row, col);
131  }
132
133  /**
134   * Returns the input matrix B.
135   *
136   * @return the input matrix B.
137   */
138  public Matrix<States, Inputs> getB() {
139    return m_B;
140  }
141
142  /**
143   * Returns an element of the input matrix B.
144   *
145   * @param row Row of B.
146   * @param col Column of B.
147   * @return The value of the input matrix B at (i, j).
148   */
149  public double getB(int row, int col) {
150    return m_B.get(row, col);
151  }
152
153  /**
154   * Returns the output matrix C.
155   *
156   * @return Output matrix C.
157   */
158  public Matrix<Outputs, States> getC() {
159    return m_C;
160  }
161
162  /**
163   * Returns an element of the output matrix C.
164   *
165   * @param row Row of C.
166   * @param col Column of C.
167   * @return the double value of C at the given position.
168   */
169  public double getC(int row, int col) {
170    return m_C.get(row, col);
171  }
172
173  /**
174   * Returns the feedthrough matrix D.
175   *
176   * @return the feedthrough matrix D.
177   */
178  public Matrix<Outputs, Inputs> getD() {
179    return m_D;
180  }
181
182  /**
183   * Returns an element of the feedthrough matrix D.
184   *
185   * @param row Row of D.
186   * @param col Column of D.
187   * @return The feedthrough matrix D at (i, j).
188   */
189  public double getD(int row, int col) {
190    return m_D.get(row, col);
191  }
192
193  /**
194   * Computes the new x given the old x and the control input.
195   *
196   * <p>This is used by state observers directly to run updates based on state estimate.
197   *
198   * @param x The current state.
199   * @param clampedU The control input.
200   * @param dtSeconds Timestep for model update.
201   * @return the updated x.
202   */
203  public Matrix<States, N1> calculateX(
204      Matrix<States, N1> x, Matrix<Inputs, N1> clampedU, double dtSeconds) {
205    var discABpair = Discretization.discretizeAB(m_A, m_B, dtSeconds);
206
207    return discABpair.getFirst().times(x).plus(discABpair.getSecond().times(clampedU));
208  }
209
210  /**
211   * Computes the new y given the control input.
212   *
213   * <p>This is used by state observers directly to run updates based on state estimate.
214   *
215   * @param x The current state.
216   * @param clampedU The control input.
217   * @return the updated output matrix Y.
218   */
219  public Matrix<Outputs, N1> calculateY(Matrix<States, N1> x, Matrix<Inputs, N1> clampedU) {
220    return m_C.times(x).plus(m_D.times(clampedU));
221  }
222
223  /**
224   * Returns the LinearSystem with the outputs listed in outputIndices.
225   *
226   * <p>This is used by state observers such as the Kalman Filter.
227   *
228   * @param outputIndices the list of output indices to include in the sliced system.
229   * @return the sliced LinearSystem with outputs set to row vectors of LinearSystem.
230   * @throws IllegalArgumentException if any outputIndices are outside the range of system outputs.
231   * @throws IllegalArgumentException if number of outputIndices exceeds the number of system
232   *     outputs.
233   * @throws IllegalArgumentException if duplication exists in outputIndices.
234   */
235  public LinearSystem<States, Inputs, ? extends Num> slice(int... outputIndices) {
236    for (int index : outputIndices) {
237      if (index < 0 || index >= m_C.getNumRows()) {
238        throw new IllegalArgumentException(
239            "Output indices out of range. This is usually due to model implementation errors.");
240      }
241    }
242
243    if (outputIndices.length >= m_C.getNumRows()) {
244      throw new IllegalArgumentException(
245          "More outputs requested than available. This is usually due to model implementation "
246              + "errors.");
247    }
248
249    List<Integer> outputIndicesList =
250        Arrays.stream(outputIndices).distinct().boxed().collect(Collectors.toList());
251    Collections.sort(outputIndicesList);
252
253    if (outputIndices.length != outputIndicesList.size()) {
254      throw new IllegalArgumentException(
255          "Duplicate indices exist.  This is usually due to model implementation " + "errors.");
256    }
257
258    SimpleMatrix new_C_Storage = new SimpleMatrix(outputIndices.length, m_C.getNumCols());
259    int row = 0;
260    for (var index : outputIndicesList) {
261      var current_row_data = m_C.extractRowVector(index).getData();
262      new_C_Storage.setRow(row, 0, current_row_data);
263      row++;
264    }
265
266    SimpleMatrix new_D_Storage = new SimpleMatrix(outputIndices.length, m_D.getNumCols());
267    row = 0;
268    for (var index : outputIndicesList) {
269      var current_row_data = m_D.extractRowVector(index).getData();
270      new_D_Storage.setRow(row, 0, current_row_data);
271      row++;
272    }
273
274    switch (outputIndices.length) {
275      case 20:
276        Matrix<N20, States> new_C20 = new Matrix<>(new_C_Storage);
277        Matrix<N20, Inputs> new_D20 = new Matrix<>(new_D_Storage);
278        return new LinearSystem<>(m_A, m_B, new_C20, new_D20);
279      case 19:
280        Matrix<N19, States> new_C19 = new Matrix<>(new_C_Storage);
281        Matrix<N19, Inputs> new_D19 = new Matrix<>(new_D_Storage);
282        return new LinearSystem<>(m_A, m_B, new_C19, new_D19);
283      case 18:
284        Matrix<N18, States> new_C18 = new Matrix<>(new_C_Storage);
285        Matrix<N18, Inputs> new_D18 = new Matrix<>(new_D_Storage);
286        return new LinearSystem<>(m_A, m_B, new_C18, new_D18);
287      case 17:
288        Matrix<N17, States> new_C17 = new Matrix<>(new_C_Storage);
289        Matrix<N17, Inputs> new_D17 = new Matrix<>(new_D_Storage);
290        return new LinearSystem<>(m_A, m_B, new_C17, new_D17);
291      case 16:
292        Matrix<N16, States> new_C16 = new Matrix<>(new_C_Storage);
293        Matrix<N16, Inputs> new_D16 = new Matrix<>(new_D_Storage);
294        return new LinearSystem<>(m_A, m_B, new_C16, new_D16);
295      case 15:
296        Matrix<N15, States> new_C15 = new Matrix<>(new_C_Storage);
297        Matrix<N15, Inputs> new_D15 = new Matrix<>(new_D_Storage);
298        return new LinearSystem<>(m_A, m_B, new_C15, new_D15);
299      case 14:
300        Matrix<N14, States> new_C14 = new Matrix<>(new_C_Storage);
301        Matrix<N14, Inputs> new_D14 = new Matrix<>(new_D_Storage);
302        return new LinearSystem<>(m_A, m_B, new_C14, new_D14);
303      case 13:
304        Matrix<N13, States> new_C13 = new Matrix<>(new_C_Storage);
305        Matrix<N13, Inputs> new_D13 = new Matrix<>(new_D_Storage);
306        return new LinearSystem<>(m_A, m_B, new_C13, new_D13);
307      case 12:
308        Matrix<N12, States> new_C12 = new Matrix<>(new_C_Storage);
309        Matrix<N12, Inputs> new_D12 = new Matrix<>(new_D_Storage);
310        return new LinearSystem<>(m_A, m_B, new_C12, new_D12);
311      case 11:
312        Matrix<N11, States> new_C11 = new Matrix<>(new_C_Storage);
313        Matrix<N11, Inputs> new_D11 = new Matrix<>(new_D_Storage);
314        return new LinearSystem<>(m_A, m_B, new_C11, new_D11);
315      case 10:
316        Matrix<N10, States> new_C10 = new Matrix<>(new_C_Storage);
317        Matrix<N10, Inputs> new_D10 = new Matrix<>(new_D_Storage);
318        return new LinearSystem<>(m_A, m_B, new_C10, new_D10);
319      case 9:
320        Matrix<N9, States> new_C9 = new Matrix<>(new_C_Storage);
321        Matrix<N9, Inputs> new_D9 = new Matrix<>(new_D_Storage);
322        return new LinearSystem<>(m_A, m_B, new_C9, new_D9);
323      case 8:
324        Matrix<N8, States> new_C8 = new Matrix<>(new_C_Storage);
325        Matrix<N8, Inputs> new_D8 = new Matrix<>(new_D_Storage);
326        return new LinearSystem<>(m_A, m_B, new_C8, new_D8);
327      case 7:
328        Matrix<N7, States> new_C7 = new Matrix<>(new_C_Storage);
329        Matrix<N7, Inputs> new_D7 = new Matrix<>(new_D_Storage);
330        return new LinearSystem<>(m_A, m_B, new_C7, new_D7);
331      case 6:
332        Matrix<N6, States> new_C6 = new Matrix<>(new_C_Storage);
333        Matrix<N6, Inputs> new_D6 = new Matrix<>(new_D_Storage);
334        return new LinearSystem<>(m_A, m_B, new_C6, new_D6);
335      case 5:
336        Matrix<N5, States> new_C5 = new Matrix<>(new_C_Storage);
337        Matrix<N5, Inputs> new_D5 = new Matrix<>(new_D_Storage);
338        return new LinearSystem<>(m_A, m_B, new_C5, new_D5);
339      case 4:
340        Matrix<N4, States> new_C4 = new Matrix<>(new_C_Storage);
341        Matrix<N4, Inputs> new_D4 = new Matrix<>(new_D_Storage);
342        return new LinearSystem<>(m_A, m_B, new_C4, new_D4);
343      case 3:
344        Matrix<N3, States> new_C3 = new Matrix<>(new_C_Storage);
345        Matrix<N3, Inputs> new_D3 = new Matrix<>(new_D_Storage);
346        return new LinearSystem<>(m_A, m_B, new_C3, new_D3);
347      case 2:
348        Matrix<N2, States> new_C2 = new Matrix<>(new_C_Storage);
349        Matrix<N2, Inputs> new_D2 = new Matrix<>(new_D_Storage);
350        return new LinearSystem<>(m_A, m_B, new_C2, new_D2);
351      default:
352        Matrix<N1, States> new_C1 = new Matrix<>(new_C_Storage);
353        Matrix<N1, Inputs> new_D1 = new Matrix<>(new_D_Storage);
354        return new LinearSystem<>(m_A, m_B, new_C1, new_D1);
355    }
356  }
357
358  @Override
359  public String toString() {
360    return String.format(
361        "Linear System: A\n%s\n\nB:\n%s\n\nC:\n%s\n\nD:\n%s\n",
362        m_A.toString(), m_B.toString(), m_C.toString(), m_D.toString());
363  }
364}