mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-01 02:41:48 +00:00
[wpimath] Add core State-space classes (#2614)
Co-authored-by: Tyler Veness <calcmogul@gmail.com> Co-authored-by: Claudius Tewari <cttewari@gmail.com> Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
This commit is contained in:
Matt
@@ -0,0 +1,179 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.math;
|
||||
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.Num;
|
||||
import edu.wpi.first.wpiutil.math.Pair;
|
||||
|
||||
@SuppressWarnings({"PMD.TooManyMethods", "ParameterName", "MethodTypeParameterName"})
|
||||
public final class Discretization {
|
||||
private Discretization() {
|
||||
// Utility class
|
||||
}
|
||||
|
||||
/**
|
||||
* Discretizes the given continuous A matrix.
|
||||
*
|
||||
* @param <States> Num representing the number of states.
|
||||
* @param contA Continuous system matrix.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
* @return the discrete matrix system.
|
||||
*/
|
||||
public static <States extends Num> Matrix<States, States> discretizeA(
|
||||
Matrix<States, States> contA, double dtSeconds) {
|
||||
return contA.times(dtSeconds).exp();
|
||||
}
|
||||
|
||||
/**
|
||||
* Discretizes the given continuous A and B matrices.
|
||||
*
|
||||
* <p>Rather than solving a (States + Inputs) x (States + Inputs) matrix
|
||||
* exponential like in DiscretizeAB(), we take advantage of the structure of the
|
||||
* block matrix of A and B.
|
||||
*
|
||||
* <p>1) The exponential of A*t, which is only N x N, is relatively cheap.
|
||||
* 2) The upper-right quarter of the (States + Inputs) x (States + Inputs)
|
||||
* matrix, which we can approximate using a taylor series to several terms
|
||||
* and still be substantially cheaper than taking the big exponential.
|
||||
*
|
||||
* @param states Nat representing the states of the system.
|
||||
* @param contA Continuous system matrix.
|
||||
* @param contB Continuous input matrix.
|
||||
* @param dtseconds Discretization timestep.
|
||||
*/
|
||||
public static <States extends Num, Inputs extends Num> Pair<Matrix<States, States>,
|
||||
Matrix<States, Inputs>>
|
||||
discretizeABTaylor(Nat<States> states,
|
||||
Matrix<States, States> contA,
|
||||
Matrix<States, Inputs> contB,
|
||||
double dtseconds) {
|
||||
Matrix<States, States> lastTerm = Matrix.eye(states);
|
||||
double lastCoeff = dtseconds;
|
||||
|
||||
var phi12 = lastTerm.times(lastCoeff);
|
||||
|
||||
// i = 6 i.e. 5th order should be enough precision
|
||||
for (int i = 2; i < 6; ++i) {
|
||||
lastTerm = contA.times(lastTerm);
|
||||
lastCoeff *= dtseconds / ((double) i);
|
||||
|
||||
phi12 = phi12.plus(lastTerm.times(lastCoeff));
|
||||
}
|
||||
|
||||
var discB = phi12.times(contB);
|
||||
|
||||
var discA = discretizeA(contA, dtseconds);
|
||||
|
||||
return Pair.of(discA, discB);
|
||||
}
|
||||
|
||||
/**
|
||||
* Discretizes the given continuous A and Q matrices.
|
||||
*
|
||||
* <p>Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which
|
||||
* is expensive), we take advantage of the structure of the block matrix of A
|
||||
* and Q.
|
||||
*
|
||||
* <p>The exponential of A*t, which is only N x N, is relatively cheap.
|
||||
* 2) The upper-right quarter of the 2N x 2N matrix, which we can approximate
|
||||
* using a taylor series to several terms and still be substantially cheaper
|
||||
* than taking the big exponential.
|
||||
*
|
||||
* @param <States> Nat representing the number of states.
|
||||
* @param contA Continuous system matrix.
|
||||
* @param contQ Continuous process noise covariance matrix.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
* @return a pair representing the discrete system matrix and process noise covariance matrix.
|
||||
*/
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public static <States extends Num> Pair<Matrix<States, States>,
|
||||
Matrix<States, States>> discretizeAQTaylor(Matrix<States, States> contA,
|
||||
Matrix<States, States> contQ,
|
||||
double dtSeconds) {
|
||||
Matrix<States, States> Q = (contQ.plus(contQ.transpose())).div(2.0);
|
||||
|
||||
|
||||
Matrix<States, States> lastTerm = Q.copy();
|
||||
double lastCoeff = dtSeconds;
|
||||
|
||||
// A^T^n
|
||||
Matrix<States, States> Atn = contA.transpose();
|
||||
Matrix<States, States> phi12 = lastTerm.times(lastCoeff);
|
||||
|
||||
// i = 6 i.e. 6th order should be enough precision
|
||||
for (int i = 2; i < 6; ++i) {
|
||||
lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(Atn));
|
||||
lastCoeff *= dtSeconds / ((double) i);
|
||||
|
||||
phi12 = phi12.plus(lastTerm.times(lastCoeff));
|
||||
|
||||
Atn = Atn.times(contA.transpose());
|
||||
}
|
||||
|
||||
var discA = discretizeA(contA, dtSeconds);
|
||||
Q = discA.times(phi12);
|
||||
|
||||
// Make Q symmetric if it isn't already
|
||||
var discQ = Q.plus(Q.transpose()).div(2.0);
|
||||
|
||||
return new Pair<>(discA, discQ);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns a discretized version of the provided continuous measurement noise
|
||||
* covariance matrix. Note that dt=0.0 divides R by zero.
|
||||
*
|
||||
* @param <O> Nat representing the number of outputs.
|
||||
* @param R Continuous measurement noise covariance matrix.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
* @return Discretized version of the provided continuous measurement noise covariance matrix.
|
||||
*/
|
||||
public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> R, double dtSeconds) {
|
||||
return R.div(dtSeconds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Discretizes the given continuous A and B matrices.
|
||||
*
|
||||
* @param <States> Nat representing the states of the system.
|
||||
* @param <Inputs> Nat representing the inputs to the system.
|
||||
* @param contA Continuous system matrix.
|
||||
* @param contB Continuous input matrix.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
* @return a Pair representing discA and diskB.
|
||||
*/
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public static <States extends Num, Inputs extends Num> Pair<Matrix<States, States>,
|
||||
Matrix<States, Inputs>> discretizeAB(
|
||||
Matrix<States, States> contA,
|
||||
Matrix<States, Inputs> contB,
|
||||
double dtSeconds) {
|
||||
var scaledA = contA.times(dtSeconds);
|
||||
var scaledB = contB.times(dtSeconds);
|
||||
|
||||
var contSize = contB.getNumRows() + contB.getNumCols();
|
||||
var Mcont = new Matrix<>(new SimpleMatrix(contSize, contSize));
|
||||
Mcont.assignBlock(0, 0, scaledA);
|
||||
Mcont.assignBlock(0, scaledA.getNumCols(), scaledB);
|
||||
var Mdisc = Mcont.exp();
|
||||
|
||||
var discA = new Matrix<States, States>(new SimpleMatrix(contB.getNumRows(),
|
||||
contB.getNumRows()));
|
||||
var discB = new Matrix<States, Inputs>(new SimpleMatrix(contB.getNumRows(),
|
||||
contB.getNumCols()));
|
||||
|
||||
discA.extractFrom(0, 0, Mdisc);
|
||||
discB.extractFrom(0, contB.getNumRows(), Mdisc);
|
||||
|
||||
return new Pair<>(discA, discB);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,180 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.math;
|
||||
|
||||
import java.util.Random;
|
||||
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpiutil.math.MathUtil;
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.Num;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.WPIMathJNI;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N3;
|
||||
|
||||
@SuppressWarnings({"PMD.TooManyMethods", "ParameterName"})
|
||||
public final class StateSpaceUtil {
|
||||
private StateSpaceUtil() {
|
||||
// Utility class
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a covariance matrix from the given vector for use with Kalman
|
||||
* filters.
|
||||
*
|
||||
* <p>Each element is squared and placed on the covariance matrix diagonal.
|
||||
*
|
||||
* @param <States> Num representing the states of the system.
|
||||
* @param states A Nat representing the states of the system.
|
||||
* @param stdDevs For a Q matrix, its elements are the standard deviations of
|
||||
* each state from how the model behaves. For an R matrix, its
|
||||
* elements are the standard deviations for each output
|
||||
* measurement.
|
||||
* @return Process noise or measurement noise covariance matrix.
|
||||
*/
|
||||
@SuppressWarnings("MethodTypeParameterName")
|
||||
public static <States extends Num> Matrix<States, States> makeCovarianceMatrix(
|
||||
Nat<States> states, Matrix<States, N1> stdDevs
|
||||
) {
|
||||
var result = new Matrix<>(states, states);
|
||||
for (int i = 0; i < states.getNum(); i++) {
|
||||
result.set(i, i, Math.pow(stdDevs.get(i, 0), 2));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a vector of normally distributed white noise with the given noise
|
||||
* intensities for each element.
|
||||
*
|
||||
* @param <N> Num representing the dimensionality of the noise vector to create.
|
||||
* @param stdDevs A matrix whose elements are the standard deviations of each
|
||||
* element of the noise vector.
|
||||
* @return White noise vector.
|
||||
*/
|
||||
public static <N extends Num> Matrix<N, N1> makeWhiteNoiseVector(
|
||||
Matrix<N, N1> stdDevs
|
||||
) {
|
||||
var rand = new Random();
|
||||
|
||||
Matrix<N, N1> result = new Matrix<>(new SimpleMatrix(stdDevs.getNumRows(), 1));
|
||||
for (int i = 0; i < stdDevs.getNumRows(); i++) {
|
||||
result.set(i, 0, rand.nextGaussian() * stdDevs.get(i, 0));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a cost matrix from the given vector for use with LQR.
|
||||
*
|
||||
* <p>The cost matrix is constructed using Bryson's rule. The inverse square of
|
||||
* each element in the input is taken and placed on the cost matrix diagonal.
|
||||
*
|
||||
* @param <States> Nat representing the states of the system.
|
||||
* @param costs An array. For a Q matrix, its elements are the maximum allowed
|
||||
* excursions of the states from the reference. For an R matrix,
|
||||
* its elements are the maximum allowed excursions of the control
|
||||
* inputs from no actuation.
|
||||
* @return State excursion or control effort cost matrix.
|
||||
*/
|
||||
@SuppressWarnings("MethodTypeParameterName")
|
||||
public static <States extends Num> Matrix<States, States>
|
||||
makeCostMatrix(Matrix<States, N1> costs) {
|
||||
Matrix<States, States> result =
|
||||
new Matrix<>(new SimpleMatrix(costs.getNumRows(), costs.getNumRows()));
|
||||
result.fill(0.0);
|
||||
|
||||
for (int i = 0; i < costs.getNumRows(); i++) {
|
||||
result.set(i, i, 1.0 / (Math.pow(costs.get(i, 0), 2)));
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if (A, B) is a stabilizable pair.
|
||||
*
|
||||
* <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
|
||||
* any, have absolute values less than one, where an eigenvalue is
|
||||
* uncontrollable if rank(lambda * I - A, B) %3C n where n is number of states.
|
||||
*
|
||||
* @param <States> Num representing the size of A.
|
||||
* @param <Inputs> Num representing the columns of B.
|
||||
* @param A System matrix.
|
||||
* @param B Input matrix.
|
||||
* @return If the system is stabilizable.
|
||||
*/
|
||||
@SuppressWarnings("MethodTypeParameterName")
|
||||
public static <States extends Num, Inputs extends Num> boolean isStabilizable(
|
||||
Matrix<States, States> A, Matrix<States, Inputs> B) {
|
||||
return WPIMathJNI.isStabilizable(A.getNumRows(), B.getNumCols(),
|
||||
A.getData(), B.getData());
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians.
|
||||
*
|
||||
* @param pose A pose to convert to a vector.
|
||||
* @return The given pose in vector form, with the third element, theta, in radians.
|
||||
*/
|
||||
public static Matrix<N3, N1> poseToVector(Pose2d pose) {
|
||||
return VecBuilder.fill(
|
||||
pose.getX(),
|
||||
pose.getY(),
|
||||
pose.getRotation().getRadians()
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Clamp the input u to the min and max.
|
||||
*
|
||||
* @param u The input to clamp.
|
||||
* @param umin The minimum input magnitude.
|
||||
* @param umax The maximum input magnitude.
|
||||
* @param <I> The number of inputs.
|
||||
* @return The clamped input.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public static <I extends Num> Matrix<I, N1> clampInputMaxMagnitude(Matrix<I, N1> u,
|
||||
Matrix<I, N1> umin,
|
||||
Matrix<I, N1> umax) {
|
||||
var result = new Matrix<I, N1>(new SimpleMatrix(u.getNumRows(), 1));
|
||||
for (int i = 0; i < u.getNumRows(); i++) {
|
||||
result.set(i, 0, MathUtil.clamp(
|
||||
u.get(i, 0),
|
||||
umin.get(i, 0),
|
||||
umax.get(i, 0)));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Normalize all inputs if any excedes the maximum magnitude. Useful for systems such as
|
||||
* differential drivetrains.
|
||||
*
|
||||
* @param u The input vector.
|
||||
* @param maxMagnitude The maximum magnitude any input can have.
|
||||
* @param <I> The number of inputs.
|
||||
* @return The normalizedInput
|
||||
*/
|
||||
public static <I extends Num> Matrix<I, N1> normalizeInputVector(Matrix<I, N1> u,
|
||||
double maxMagnitude) {
|
||||
double maxValue = u.maxAbs();
|
||||
boolean isCapped = maxValue > maxMagnitude;
|
||||
|
||||
if (isCapped) {
|
||||
return u.times(maxMagnitude / maxValue);
|
||||
}
|
||||
return u;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user