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