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.util;
006
007import org.ejml.simple.SimpleMatrix;
008import org.wpilib.math.linalg.Matrix;
009import org.wpilib.math.numbers.N1;
010
011/** State-space utilities. */
012public final class StateSpaceUtil {
013  private StateSpaceUtil() {
014    throw new UnsupportedOperationException("This is a utility class!");
015  }
016
017  /**
018   * Creates a cost matrix from the given vector for use with LQR.
019   *
020   * <p>The cost matrix is constructed using Bryson's rule. The inverse square of each tolerance is
021   * placed on the cost matrix diagonal. If a tolerance is infinity, its cost matrix entry is set to
022   * zero.
023   *
024   * @param <Elements> Nat representing the number of system states or inputs.
025   * @param tolerances An array. For a Q matrix, its elements are the maximum allowed excursions of
026   *     the states from the reference. For an R matrix, its elements are the maximum allowed
027   *     excursions of the control inputs from no actuation.
028   * @return State excursion or control effort cost matrix.
029   */
030  public static <Elements extends Num> Matrix<Elements, Elements> costMatrix(
031      Matrix<Elements, N1> tolerances) {
032    Matrix<Elements, Elements> result =
033        new Matrix<>(new SimpleMatrix(tolerances.getNumRows(), tolerances.getNumRows()));
034    result.fill(0.0);
035
036    for (int i = 0; i < tolerances.getNumRows(); i++) {
037      if (tolerances.get(i, 0) == Double.POSITIVE_INFINITY) {
038        result.set(i, i, 0.0);
039      } else {
040        result.set(i, i, 1.0 / Math.pow(tolerances.get(i, 0), 2));
041      }
042    }
043
044    return result;
045  }
046
047  /**
048   * Creates a covariance matrix from the given vector for use with Kalman filters.
049   *
050   * <p>Each element is squared and placed on the covariance matrix diagonal.
051   *
052   * @param <States> Num representing the states of the system.
053   * @param states A Nat representing the states of the system.
054   * @param stdDevs For a Q matrix, its elements are the standard deviations of each state from how
055   *     the model behaves. For an R matrix, its elements are the standard deviations for each
056   *     output measurement.
057   * @return Process noise or measurement noise covariance matrix.
058   */
059  public static <States extends Num> Matrix<States, States> covarianceMatrix(
060      Nat<States> states, Matrix<States, N1> stdDevs) {
061    var result = new Matrix<>(states, states);
062    for (int i = 0; i < states.getNum(); i++) {
063      result.set(i, i, Math.pow(stdDevs.get(i, 0), 2));
064    }
065    return result;
066  }
067
068  /**
069   * Renormalize all inputs if any exceeds the maximum magnitude. Useful for systems such as
070   * differential drivetrains.
071   *
072   * @param u The input vector.
073   * @param maxMagnitude The maximum magnitude any input can have.
074   * @param <I> Number of inputs.
075   * @return The normalizedInput
076   */
077  public static <I extends Num> Matrix<I, N1> desaturateInputVector(
078      Matrix<I, N1> u, double maxMagnitude) {
079    return u.times(Math.min(1.0, maxMagnitude / u.maxAbs()));
080  }
081}