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 org.wpilib.math.system;
006
007import org.wpilib.math.jni.LinearSystemUtilJNI;
008import org.wpilib.math.linalg.Matrix;
009import org.wpilib.math.util.Num;
010
011/** Linear system utilities. */
012public final class LinearSystemUtil {
013  private LinearSystemUtil() {
014    throw new UnsupportedOperationException("This is a utility class!");
015  }
016
017  /**
018   * Returns true if (A, B) is a stabilizable pair.
019   *
020   * <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
021   * absolute values less than one, where an eigenvalue is uncontrollable if rank([λI - A, B]) %3C n
022   * where n is the number of states.
023   *
024   * @param <States> Num representing the size of A.
025   * @param <Inputs> Num representing the columns of B.
026   * @param A System matrix.
027   * @param B Input matrix.
028   * @return If the system is stabilizable.
029   */
030  public static <States extends Num, Inputs extends Num> boolean isStabilizable(
031      Matrix<States, States> A, Matrix<States, Inputs> B) {
032    return LinearSystemUtilJNI.isStabilizable(
033        A.getNumRows(), B.getNumCols(), A.getData(), B.getData());
034  }
035
036  /**
037   * Returns true if (A, C) is a detectable pair.
038   *
039   * <p>(A, C) is detectable if and only if the unobservable eigenvalues of A, if any, have absolute
040   * values less than one, where an eigenvalue is unobservable if rank([λI - A; C]) %3C n where n is
041   * the number of states.
042   *
043   * @param <States> Num representing the size of A.
044   * @param <Outputs> Num representing the rows of C.
045   * @param A System matrix.
046   * @param C Output matrix.
047   * @return If the system is detectable.
048   */
049  public static <States extends Num, Outputs extends Num> boolean isDetectable(
050      Matrix<States, States> A, Matrix<Outputs, States> C) {
051    return LinearSystemUtilJNI.isStabilizable(
052        A.getNumRows(), C.getNumRows(), A.transpose().getData(), C.transpose().getData());
053  }
054}