Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Clean up NumericalIntegration and add Discretization tests (#3489)

Browse Source 
* Rename Butcher tableau sections in NumericalIntegration such that
  top-left is c, top-right is A, and bottom-right is b
* Move edu.wpi.first.math.Discretization to
  edu.wpi.first.math.system.Discretization
* Sort Java Discretization to match C++ function order
* Add tests for Java Discretization
  * Required adding Runge-Kutta time-varying impl to tests
* Move C++ Runge-Kutta time-varying impl to tests only
  * Users don't need it
This commit is contained in:
Tyler Veness
2021-07-25 07:42:59 -07:00
committed by GitHub
parent bfc209b120
commit 50af74c38f
20 changed files with 641 additions and 310 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
wpimath/src/main/java/edu/wpi/first/math/Matrix.java
View File

@@ -491,9 +491,27 @@ public class Matrix<R extends Num, C extends Num> {
return new Matrix<>(
this.m_storage.extractMatrix(
startingRow,
Objects.requireNonNull(height).getNum() + startingRow,
startingRow + Objects.requireNonNull(height).getNum(),
startingCol,
Objects.requireNonNull(width).getNum() + startingCol));
startingCol + Objects.requireNonNull(width).getNum()));
}
/**
* Extracts a matrix of a given size and start position with new underlying storage.
*
* @param <R2> Number of rows to extract.
* @param <C2> Number of columns to extract.
* @param height The number of rows of the extracted matrix.
* @param width The number of columns of the extracted matrix.
* @param startingRow The starting row of the extracted matrix.
* @param startingCol The starting column of the extracted matrix.
* @return The extracted matrix.
*/
public final <R2 extends Num, C2 extends Num> Matrix<R2, C2> block(
int height, int width, int startingRow, int startingCol) {
return new Matrix<R2, C2>(
this.m_storage.extractMatrix(
startingRow, startingRow + height, startingCol, startingCol + width));
}
/**

2
wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
View File

@@ -4,10 +4,10 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.Discretization;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.LinearSystem;
import org.ejml.simple.SimpleMatrix;

2
wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
View File

@@ -4,7 +4,6 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.Discretization;
import edu.wpi.first.math.Drake;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
@@ -12,6 +11,7 @@ import edu.wpi.first.math.Num;
import edu.wpi.first.math.StateSpaceUtil;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.LinearSystem;
import org.ejml.simple.SimpleMatrix;

2
wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
View File

@@ -4,13 +4,13 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.Discretization;
import edu.wpi.first.math.Drake;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.StateSpaceUtil;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.NumericalIntegration;
import edu.wpi.first.math.system.NumericalJacobian;
import java.util.function.BiFunction;

2
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
View File

@@ -4,7 +4,6 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.Discretization;
import edu.wpi.first.math.Drake;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
@@ -12,6 +11,7 @@ import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.StateSpaceUtil;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.LinearSystem;
/**

2
wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
View File

@@ -4,13 +4,13 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.Discretization;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.StateSpaceUtil;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.NumericalIntegration;
import edu.wpi.first.math.system.NumericalJacobian;
import java.util.function.BiFunction;

131
wpimath/src/main/java/edu/wpi/first/math/Discretization.java → wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
View File

@@ -2,8 +2,11 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math;
package edu.wpi.first.math.system;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.Pair;
import org.ejml.simple.SimpleMatrix;
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
@@ -28,46 +31,75 @@ public final class Discretization {
/**
* 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> Number of states.
* @param <Inputs> Number of inputs.
* @param states Nat representing the states of the system.
* @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 Pair containing discretized A and B matrices.
* @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>> discretizeABTaylor(
Nat<States> states,
Matrix<States, States> contA,
Matrix<States, Inputs> contB,
double dtseconds) {
Matrix<States, States> lastTerm = Matrix.eye(states);
double lastCoeff = dtseconds;
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 phi12 = lastTerm.times(lastCoeff);
int states = contA.getNumRows();
int inputs = contB.getNumCols();
var M = new Matrix<>(new SimpleMatrix(states + inputs, states + inputs));
M.assignBlock(0, 0, scaledA);
M.assignBlock(0, scaledA.getNumCols(), scaledB);
var phi = M.exp();
// 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);
var discA = new Matrix<States, States>(new SimpleMatrix(states, states));
var discB = new Matrix<States, Inputs>(new SimpleMatrix(states, inputs));
phi12 = phi12.plus(lastTerm.times(lastCoeff));
}
discA.extractFrom(0, 0, phi);
discB.extractFrom(0, contB.getNumRows(), phi);
var discB = phi12.times(contB);
return new Pair<>(discA, discB);
}
var discA = discretizeA(contA, dtseconds);
/**
* Discretizes the given continuous A and Q matrices.
*
* @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>> discretizeAQ(
Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
int states = contA.getNumRows();
return Pair.of(discA, discB);
// Make continuous Q symmetric if it isn't already
Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
// Set up the matrix M = [[-A, Q], [0, A.T]]
final var M = new Matrix<>(new SimpleMatrix(2 * states, 2 * states));
M.assignBlock(0, 0, contA.times(-1.0));
M.assignBlock(0, states, Q);
M.assignBlock(states, 0, new Matrix<>(new SimpleMatrix(states, states)));
M.assignBlock(states, states, contA.transpose());
final var phi = M.times(dtSeconds).exp();
// Phi12 = phi[0:States, States:2*States]
// Phi22 = phi[States:2*States, States:2*States]
final Matrix<States, States> phi12 = phi.block(states, states, 0, states);
final Matrix<States, States> phi22 = phi.block(states, states, states, states);
final var discA = phi22.transpose();
Q = discA.times(phi12);
// Make discrete Q symmetric if it isn't already
final var discQ = Q.plus(Q.transpose()).div(2.0);
return new Pair<>(discA, discQ);
}
/**
@@ -90,7 +122,8 @@ public final class Discretization {
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);
// Make continuous Q symmetric if it isn't already
Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
Matrix<States, States> lastTerm = Q.copy();
double lastCoeff = dtSeconds;
@@ -130,38 +163,4 @@ public final class Discretization {
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);
}
}

1
wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
View File

@@ -4,7 +4,6 @@
package edu.wpi.first.math.system;
import edu.wpi.first.math.Discretization;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;

211
wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
View File

@@ -26,12 +26,13 @@ public final class NumericalIntegration {
*/
@SuppressWarnings("ParameterName")
public static double rk4(DoubleFunction<Double> f, double x, double dtSeconds) {
final var halfDt = 0.5 * dtSeconds;
final var h = dtSeconds;
final var k1 = f.apply(x);
final var k2 = f.apply(x + k1 * halfDt);
final var k3 = f.apply(x + k2 * halfDt);
final var k4 = f.apply(x + k3 * dtSeconds);
return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
final var k2 = f.apply(x + h * k1 * 0.5);
final var k3 = f.apply(x + h * k2 * 0.5);
final var k4 = f.apply(x + h * k3);
return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
@@ -46,12 +47,14 @@ public final class NumericalIntegration {
@SuppressWarnings("ParameterName")
public static double rk4(
BiFunction<Double, Double, Double> f, double x, Double u, double dtSeconds) {
final var halfDt = 0.5 * dtSeconds;
final var h = dtSeconds;
final var k1 = f.apply(x, u);
final var k2 = f.apply(x + k1 * halfDt, u);
final var k3 = f.apply(x + k2 * halfDt, u);
final var k4 = f.apply(x + k3 * dtSeconds, u);
return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
final var k2 = f.apply(x + h * k1 * 0.5, u);
final var k3 = f.apply(x + h * k2 * 0.5, u);
final var k4 = f.apply(x + h * k3, u);
return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
@@ -71,12 +74,14 @@ public final class NumericalIntegration {
Matrix<States, N1> x,
Matrix<Inputs, N1> u,
double dtSeconds) {
final var halfDt = 0.5 * dtSeconds;
final var h = dtSeconds;
Matrix<States, N1> k1 = f.apply(x, u);
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)), u);
Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)), u);
Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)), u);
return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(h * 0.5)), u);
Matrix<States, N1> k3 = f.apply(x.plus(k2.times(h * 0.5)), u);
Matrix<States, N1> k4 = f.apply(x.plus(k3.times(h)), u);
return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
}
/**
@@ -91,12 +96,14 @@ public final class NumericalIntegration {
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public static <States extends Num> Matrix<States, N1> rk4(
Function<Matrix<States, N1>, Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
final var halfDt = 0.5 * dtSeconds;
final var h = dtSeconds;
Matrix<States, N1> k1 = f.apply(x);
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)));
Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)));
Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)));
return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(h * 0.5)));
Matrix<States, N1> k3 = f.apply(x.plus(k2.times(h * 0.5)));
Matrix<States, N1> k4 = f.apply(x.plus(k3.times(h)));
return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
}
/**
@@ -145,13 +152,8 @@ public final class NumericalIntegration {
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
// for the Butcher tableau the following arrays came from.
// This is used for time-varying integration
// // final double[5]
// final double[] A = {
// 1.0 / 4.0, 3.0 / 8.0, 12.0 / 13.0, 1.0, 1.0 / 2.0};
// final double[5][5]
final double[][] B = {
final double[][] A = {
{1.0 / 4.0},
{3.0 / 32.0, 9.0 / 32.0},
{1932.0 / 2197.0, -7200.0 / 2197.0, 7296.0 / 2197.0},
@@ -160,12 +162,12 @@ public final class NumericalIntegration {
};
// final double[6]
final double[] C1 = {
final double[] b1 = {
16.0 / 135.0, 0.0, 6656.0 / 12825.0, 28561.0 / 56430.0, -9.0 / 50.0, 2.0 / 55.0
};
// final double[6]
final double[] C2 = {25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0};
final double[] b2 = {25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0};
Matrix<States, N1> newX;
double truncationError;
@@ -181,47 +183,53 @@ public final class NumericalIntegration {
// Notice how the derivative in the Wikipedia notation is dy/dx.
// That means their y is our x and their x is our t
var k1 = f.apply(x, u).times(h);
var k2 = f.apply(x.plus(k1.times(B[0][0])), u).times(h);
var k3 = f.apply(x.plus(k1.times(B[1][0])).plus(k2.times(B[1][1])), u).times(h);
var k1 = f.apply(x, u);
var k2 = f.apply(x.plus(k1.times(A[0][0]).times(h)), u);
var k3 = f.apply(x.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h)), u);
var k4 =
f.apply(x.plus(k1.times(B[2][0])).plus(k2.times(B[2][1])).plus(k3.times(B[2][2])), u)
.times(h);
f.apply(
x.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h)),
u);
var k5 =
f.apply(
x.plus(k1.times(B[3][0]))
.plus(k2.times(B[3][1]))
.plus(k3.times(B[3][2]))
.plus(k4.times(B[3][3])),
u)
.times(h);
x.plus(
k1.times(A[3][0])
.plus(k2.times(A[3][1]))
.plus(k3.times(A[3][2]))
.plus(k4.times(A[3][3]))
.times(h)),
u);
var k6 =
f.apply(
x.plus(k1.times(B[4][0]))
.plus(k2.times(B[4][1]))
.plus(k3.times(B[4][2]))
.plus(k4.times(B[4][3]))
.plus(k5.times(B[4][4])),
u)
.times(h);
x.plus(
k1.times(A[4][0])
.plus(k2.times(A[4][1]))
.plus(k3.times(A[4][2]))
.plus(k4.times(A[4][3]))
.plus(k5.times(A[4][4]))
.times(h)),
u);
newX =
x.plus(k1.times(C1[0]))
.plus(k2.times(C1[1]))
.plus(k3.times(C1[2]))
.plus(k4.times(C1[3]))
.plus(k5.times(C1[4]))
.plus(k6.times(C1[5]));
x.plus(
k1.times(b1[0])
.plus(k2.times(b1[1]))
.plus(k3.times(b1[2]))
.plus(k4.times(b1[3]))
.plus(k5.times(b1[4]))
.plus(k6.times(b1[5]))
.times(h));
truncationError =
(k1.times(C1[0] - C2[0])
.plus(k2.times(C1[1] - C2[1]))
.plus(k3.times(C1[2] - C2[2]))
.plus(k4.times(C1[3] - C2[3]))
.plus(k5.times(C1[4] - C2[4]))
.plus(k6.times(C1[5] - C2[5])))
(k1.times(b1[0] - b2[0])
.plus(k2.times(b1[1] - b2[1]))
.plus(k3.times(b1[2] - b2[2]))
.plus(k4.times(b1[3] - b2[3]))
.plus(k5.times(b1[4] - b2[4]))
.plus(k6.times(b1[5] - b2[5]))
.times(h))
.normF();
h = 0.9 * h * Math.pow(maxError / truncationError, 1.0 / 5.0);
h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
} while (truncationError > maxError);
dtElapsed += h;
@@ -274,13 +282,8 @@ public final class NumericalIntegration {
// See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the
// Butcher tableau the following arrays came from.
// This is used for time-varying integration
// // final double[6]
// final double[] A = {
// 1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, 8.0 / 9.0, 1.0, 1.0};
// final double[6][6]
final double[][] B = {
final double[][] A = {
{1.0 / 5.0},
{3.0 / 40.0, 9.0 / 40.0},
{44.0 / 45.0, -56.0 / 15.0, 32.0 / 9.0},
@@ -290,12 +293,12 @@ public final class NumericalIntegration {
};
// final double[7]
final double[] C1 = {
final double[] b1 = {
35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0, 0.0
};
// final double[7]
final double[] C2 = {
final double[] b2 = {
5179.0 / 57600.0,
0.0,
7571.0 / 16695.0,
@@ -317,52 +320,58 @@ public final class NumericalIntegration {
// Only allow us to advance up to the dt remaining
h = Math.min(h, dtSeconds - dtElapsed);
var k1 = f.apply(x, u).times(h);
var k2 = f.apply(x.plus(k1.times(B[0][0])), u).times(h);
var k3 = f.apply(x.plus(k1.times(B[1][0])).plus(k2.times(B[1][1])), u).times(h);
var k1 = f.apply(x, u);
var k2 = f.apply(x.plus(k1.times(A[0][0]).times(h)), u);
var k3 = f.apply(x.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h)), u);
var k4 =
f.apply(x.plus(k1.times(B[2][0])).plus(k2.times(B[2][1])).plus(k3.times(B[2][2])), u)
.times(h);
f.apply(
x.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h)),
u);
var k5 =
f.apply(
x.plus(k1.times(B[3][0]))
.plus(k2.times(B[3][1]))
.plus(k3.times(B[3][2]))
.plus(k4.times(B[3][3])),
u)
.times(h);
x.plus(
k1.times(A[3][0])
.plus(k2.times(A[3][1]))
.plus(k3.times(A[3][2]))
.plus(k4.times(A[3][3]))
.times(h)),
u);
var k6 =
f.apply(
x.plus(k1.times(B[4][0]))
.plus(k2.times(B[4][1]))
.plus(k3.times(B[4][2]))
.plus(k4.times(B[4][3]))
.plus(k5.times(B[4][4])),
u)
.times(h);
x.plus(
k1.times(A[4][0])
.plus(k2.times(A[4][1]))
.plus(k3.times(A[4][2]))
.plus(k4.times(A[4][3]))
.plus(k5.times(A[4][4]))
.times(h)),
u);
// Since the final row of B and the array C1 have the same coefficients
// Since the final row of A and the array b1 have the same coefficients
// and k7 has no effect on newX, we can reuse the calculation.
newX =
x.plus(k1.times(B[5][0]))
.plus(k2.times(B[5][1]))
.plus(k3.times(B[5][2]))
.plus(k4.times(B[5][3]))
.plus(k5.times(B[5][4]))
.plus(k6.times(B[5][5]));
var k7 = f.apply(newX, u).times(h);
x.plus(
k1.times(A[5][0])
.plus(k2.times(A[5][1]))
.plus(k3.times(A[5][2]))
.plus(k4.times(A[5][3]))
.plus(k5.times(A[5][4]))
.plus(k6.times(A[5][5]))
.times(h));
var k7 = f.apply(newX, u);
truncationError =
(k1.times(C1[0] - C2[0])
.plus(k2.times(C1[1] - C2[1]))
.plus(k3.times(C1[2] - C2[2]))
.plus(k4.times(C1[3] - C2[3]))
.plus(k5.times(C1[4] - C2[4]))
.plus(k6.times(C1[5] - C2[5]))
.plus(k7.times(C1[6] - C2[6])))
(k1.times(b1[0] - b2[0])
.plus(k2.times(b1[1] - b2[1]))
.plus(k3.times(b1[2] - b2[2]))
.plus(k4.times(b1[3] - b2[3]))
.plus(k5.times(b1[4] - b2[4]))
.plus(k6.times(b1[5] - b2[5]))
.plus(k7.times(b1[6] - b2[6]))
.times(h))
.normF();
h = 0.9 * h * Math.pow(maxError / truncationError, 1.0 / 5.0);
h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
} while (truncationError > maxError);
dtElapsed += h;

119
wpimath/src/main/native/include/frc/system/NumericalIntegration.h
View File

@@ -23,12 +23,14 @@ namespace frc {
*/
template <typename F, typename T>
T RK4(F&& f, T x, units::second_t dt) {
const auto halfDt = 0.5 * dt;
const auto h = dt.to<double>();
T k1 = f(x);
T k2 = f(x + k1 * halfDt.to<double>());
T k3 = f(x + k2 * halfDt.to<double>());
T k4 = f(x + k3 * dt.to<double>());
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
T k2 = f(x + h * 0.5 * k1);
T k3 = f(x + h * 0.5 * k2);
T k4 = f(x + h * k3);
return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
@@ -41,31 +43,14 @@ T RK4(F&& f, T x, units::second_t dt) {
*/
template <typename F, typename T, typename U>
T RK4(F&& f, T x, U u, units::second_t dt) {
const auto halfDt = 0.5 * dt;
const auto h = dt.to<double>();
T k1 = f(x, u);
T k2 = f(x + k1 * halfDt.to<double>(), u);
T k3 = f(x + k2 * halfDt.to<double>(), u);
T k4 = f(x + k3 * dt.to<double>(), u);
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
T k2 = f(x + h * 0.5 * k1, u);
T k3 = f(x + h * 0.5 * k2, u);
T k4 = f(x + h * k3, u);
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x) for dt.
*
* @param f The function to integrate. It must take two arguments x and t.
* @param x The initial value of x.
* @param t The initial value of t.
* @param dt The time over which to integrate.
*/
template <typename F, typename T>
T RungeKuttaTimeVarying(F&& f, T x, units::second_t t, units::second_t dt) {
const auto halfDt = 0.5 * dt;
T k1 = f(t, x);
T k2 = f(t + halfDt, x + k1 / 2.0);
T k3 = f(t + halfDt, x + k2 / 2.0);
T k4 = f(t + dt, x + k3);
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
@@ -87,12 +72,8 @@ T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
// for the Butcher tableau the following arrays came from.
constexpr int kDim = 6;
// This is used for time-varying integration
// constexpr std::array<double, kDim - 1> A{
// 1.0 / 4.0, 3.0 / 8.0, 12.0 / 13.0, 1.0, 1.0 / 2.0};
// clang-format off
constexpr double B[kDim - 1][kDim - 1]{
constexpr double A[kDim - 1][kDim - 1]{
{ 1.0 / 4.0},
{ 3.0 / 32.0, 9.0 / 32.0},
{1932.0 / 2197.0, -7200.0 / 2197.0, 7296.0 / 2197.0},
@@ -100,10 +81,10 @@ T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
{ -8.0 / 27.0, 2.0, -3544.0 / 2565.0, 1859.0 / 4104.0, -11.0 / 40.0}};
// clang-format on
constexpr std::array<double, kDim> C1{16.0 / 135.0, 0.0,
constexpr std::array<double, kDim> b1{16.0 / 135.0, 0.0,
6656.0 / 12825.0, 28561.0 / 56430.0,
-9.0 / 50.0, 2.0 / 55.0};
constexpr std::array<double, kDim> C2{
constexpr std::array<double, kDim> b2{
25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0};
T newX;
@@ -121,22 +102,22 @@ T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
// Notice how the derivative in the Wikipedia notation is dy/dx.
// That means their y is our x and their x is our t
// clang-format off
T k1 = f(x, u) * h;
T k2 = f(x + k1 * B[0][0], u) * h;
T k3 = f(x + k1 * B[1][0] + k2 * B[1][1], u) * h;
T k4 = f(x + k1 * B[2][0] + k2 * B[2][1] + k3 * B[2][2], u) * h;
T k5 = f(x + k1 * B[3][0] + k2 * B[3][1] + k3 * B[3][2] + k4 * B[3][3], u) * h;
T k6 = f(x + k1 * B[4][0] + k2 * B[4][1] + k3 * B[4][2] + k4 * B[4][3] + k5 * B[4][4], u) * h;
T k1 = f(x, u);
T k2 = f(x + h * (A[0][0] * k1), u);
T k3 = f(x + h * (A[1][0] * k1 + A[1][1] * k2), u);
T k4 = f(x + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3), u);
T k5 = f(x + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4), u);
T k6 = f(x + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5), u);
// clang-format on
newX = x + k1 * C1[0] + k2 * C1[1] + k3 * C1[2] + k4 * C1[3] +
k5 * C1[4] + k6 * C1[5];
truncationError =
(k1 * (C1[0] - C2[0]) + k2 * (C1[1] - C2[1]) + k3 * (C1[2] - C2[2]) +
k4 * (C1[3] - C2[3]) + k5 * (C1[4] - C2[4]) + k6 * (C1[5] - C2[5]))
.norm();
newX = x + h * (b1[0] * k1 + b1[1] * k2 + b1[2] * k3 + b1[3] * k4 +
b1[4] * k5 + b1[5] * k6);
truncationError = (h * ((b1[0] - b2[0]) * k1 + (b1[1] - b2[1]) * k2 +
(b1[2] - b2[2]) * k3 + (b1[3] - b2[3]) * k4 +
(b1[4] - b2[4]) * k5 + (b1[5] - b2[5]) * k6))
.norm();
h = 0.9 * h * std::pow(maxError / truncationError, 1.0 / 5.0);
h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
} while (truncationError > maxError);
dtElapsed += h;
@@ -164,12 +145,8 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
constexpr int kDim = 7;
// This is used for time-varying integration
// constexpr std::array<double, kDim - 1> A{
// 1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, 8.0 / 9.0, 1.0, 1.0};
// clang-format off
constexpr double B[kDim - 1][kDim - 1]{
constexpr double A[kDim - 1][kDim - 1]{
{ 1.0 / 5.0},
{ 3.0 / 40.0, 9.0 / 40.0},
{ 44.0 / 45.0, -56.0 / 15.0, 32.0 / 9.0},
@@ -178,10 +155,10 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
{ 35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0}};
// clang-format on
constexpr std::array<double, kDim> C1{
constexpr std::array<double, kDim> b1{
35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0,
11.0 / 84.0, 0.0};
constexpr std::array<double, kDim> C2{5179.0 / 57600.0, 0.0,
constexpr std::array<double, kDim> b2{5179.0 / 57600.0, 0.0,
7571.0 / 16695.0, 393.0 / 640.0,
-92097.0 / 339200.0, 187.0 / 2100.0,
1.0 / 40.0};
@@ -199,27 +176,27 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
h = std::min(h, dt.to<double>() - dtElapsed);
// clang-format off
T k1 = f(x, u) * h;
T k2 = f(x + k1 * B[0][0], u) * h;
T k3 = f(x + k1 * B[1][0] + k2 * B[1][1], u) * h;
T k4 = f(x + k1 * B[2][0] + k2 * B[2][1] + k3 * B[2][2], u) * h;
T k5 = f(x + k1 * B[3][0] + k2 * B[3][1] + k3 * B[3][2] + k4 * B[3][3], u) * h;
T k6 = f(x + k1 * B[4][0] + k2 * B[4][1] + k3 * B[4][2] + k4 * B[4][3] + k5 * B[4][4], u) * h;
T k1 = f(x, u);
T k2 = f(x + h * (A[0][0] * k1), u);
T k3 = f(x + h * (A[1][0] * k1 + A[1][1] * k2), u);
T k4 = f(x + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3), u);
T k5 = f(x + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4), u);
T k6 = f(x + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5), u);
// clang-format on
// Since the final row of B and the array C1 have the same coefficients
// Since the final row of A and the array b1 have the same coefficients
// and k7 has no effect on newX, we can reuse the calculation.
newX = x + k1 * B[5][0] + k2 * B[5][1] + k3 * B[5][2] + k4 * B[5][3] +
k5 * B[5][4] + k6 * B[5][5];
T k7 = f(newX, u) * h;
newX = x + h * (A[5][0] * k1 + A[5][1] * k2 + A[5][2] * k3 +
A[5][3] * k4 + A[5][4] * k5 + A[5][5] * k6);
T k7 = f(newX, u);
truncationError =
(k1 * (C1[0] - C2[0]) + k2 * (C1[1] - C2[1]) + k3 * (C1[2] - C2[2]) +
k4 * (C1[3] - C2[3]) + k5 * (C1[4] - C2[4]) + k6 * (C1[5] - C2[5]) +
k7 * (C1[6] - C2[6]))
.norm();
truncationError = (h * ((b1[0] - b2[0]) * k1 + (b1[1] - b2[1]) * k2 +
(b1[2] - b2[2]) * k3 + (b1[3] - b2[3]) * k4 +
(b1[4] - b2[4]) * k5 + (b1[5] - b2[5]) * k6 +
(b1[6] - b2[6]) * k7))
.norm();
h = 0.9 * h * std::pow(maxError / truncationError, 1.0 / 5.0);
h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
} while (truncationError > maxError);
dtElapsed += h;

1
wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
View File

@@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.Discretization;
import java.util.ArrayList;
import java.util.List;
import org.ejml.dense.row.MatrixFeatures_DDRM;

2
wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
View File

@@ -8,7 +8,6 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.Discretization;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.StateSpaceUtil;
@@ -19,6 +18,7 @@ import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N4;
import edu.wpi.first.math.numbers.N6;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.NumericalIntegration;
import edu.wpi.first.math.system.NumericalJacobian;
import edu.wpi.first.math.system.plant.DCMotor;

215
wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java Normal file
View File

@@ -0,0 +1,215 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.system;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N2;
import org.junit.jupiter.api.Test;
public class DiscretizationTest {
// Check that for a simple second-order system that we can easily analyze
// analytically,
@Test
public void testDiscretizeA() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
final var x0 = VecBuilder.fill(1, 1);
final var discA = Discretization.discretizeA(contA, 1.0);
final var x1Discrete = discA.times(x0);
// We now have pos = vel = 1 and accel = 0, which should give us:
final var x1Truth =
VecBuilder.fill(
1.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0), 0.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0));
assertEquals(x1Truth, x1Discrete);
}
// Check that for a simple second-order system that we can easily analyze
// analytically,
@Test
public void testDiscretizeAB() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
final var contB = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0, 1);
final var x0 = VecBuilder.fill(1, 1);
final var u = VecBuilder.fill(1);
var discABPair = Discretization.discretizeAB(contA, contB, 1.0);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
var x1Discrete = discA.times(x0).plus(discB.times(u));
// We now have pos = vel = accel = 1, which should give us:
final var x1Truth =
VecBuilder.fill(
1.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0) + 0.5 * u.get(0, 0),
0.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0) + 1.0 * u.get(0, 0));
assertEquals(x1Truth, x1Discrete);
}
// Test that the discrete approximation of Q is roughly equal to
// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
@Test
public void testDiscretizeSlowModelAQ() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
final double dt = 1.0;
final var discQIntegrated =
RungeKuttaTimeVarying.rungeKuttaTimeVarying(
(Double t, Matrix<N2, N2> x) ->
contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
0.0,
new Matrix<>(Nat.N2(), Nat.N2()),
dt);
var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
var discQ = discAQPair.getSecond();
assertTrue(
discQIntegrated.minus(discQ).normF() < 1e-10,
"Expected these to be nearly equal:\ndiscQ:\n"
+ discQ
+ "\ndiscQIntegrated:\n"
+ discQIntegrated);
}
// Test that the discrete approximation of Q is roughly equal to
// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
@Test
public void testDiscretizeFastModelAQ() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1406.29);
final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
final var dt = 0.005;
final var discQIntegrated =
RungeKuttaTimeVarying.rungeKuttaTimeVarying(
(Double t, Matrix<N2, N2> x) ->
contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
0.0,
new Matrix<>(Nat.N2(), Nat.N2()),
dt);
var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
var discQ = discAQPair.getSecond();
assertTrue(
discQIntegrated.minus(discQ).normF() < 1e-3,
"Expected these to be nearly equal:\ndiscQ:\n"
+ discQ
+ "\ndiscQIntegrated:\n"
+ discQIntegrated);
}
// Test that the Taylor series discretization produces nearly identical results.
@Test
public void testDiscretizeSlowModelAQTaylor() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
final var dt = 1.0;
// Continuous Q should be positive semidefinite
final var esCont = contQ.getStorage().eig();
for (int i = 0; i < contQ.getNumRows(); ++i) {
assertTrue(esCont.getEigenvalue(i).real >= 0);
}
final var discQIntegrated =
RungeKuttaTimeVarying.rungeKuttaTimeVarying(
(Double t, Matrix<N2, N2> x) ->
contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
0.0,
new Matrix<>(Nat.N2(), Nat.N2()),
dt);
var discA = Discretization.discretizeA(contA, dt);
var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
var discATaylor = discAQPair.getFirst();
var discQTaylor = discAQPair.getSecond();
assertTrue(
discQIntegrated.minus(discQTaylor).normF() < 1e-10,
"Expected these to be nearly equal:\ndiscQTaylor:\n"
+ discQTaylor
+ "\ndiscQIntegrated:\n"
+ discQIntegrated);
assertTrue(discA.minus(discATaylor).normF() < 1e-10);
// Discrete Q should be positive semidefinite
final var esDisc = discQTaylor.getStorage().eig();
for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
assertTrue(esDisc.getEigenvalue(i).real >= 0);
}
}
// Test that the Taylor series discretization produces nearly identical results.
@Test
public void testDiscretizeFastModelAQTaylor() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1500);
final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
final var dt = 0.005;
// Continuous Q should be positive semidefinite
final var esCont = contQ.getStorage().eig();
for (int i = 0; i < contQ.getNumRows(); ++i) {
assertTrue(esCont.getEigenvalue(i).real >= 0);
}
final var discQIntegrated =
RungeKuttaTimeVarying.rungeKuttaTimeVarying(
(Double t, Matrix<N2, N2> x) ->
contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
0.0,
new Matrix<>(Nat.N2(), Nat.N2()),
dt);
var discA = Discretization.discretizeA(contA, dt);
var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
var discATaylor = discAQPair.getFirst();
var discQTaylor = discAQPair.getSecond();
assertTrue(
discQIntegrated.minus(discQTaylor).normF() < 1e-3,
"Expected these to be nearly equal:\ndiscQTaylor:\n"
+ discQTaylor
+ "\ndiscQIntegrated:\n"
+ discQIntegrated);
assertTrue(discA.minus(discATaylor).normF() < 1e-10);
// Discrete Q should be positive semidefinite
final var esDisc = discQTaylor.getStorage().eig();
for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
assertTrue(esDisc.getEigenvalue(i).real >= 0);
}
}
// Test that DiscretizeR() works
@Test
public void testDiscretizeR() {
var contR = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 0.0, 0.0, 1.0);
var discRTruth = Matrix.mat(Nat.N2(), Nat.N2()).fill(4.0, 0.0, 0.0, 2.0);
var discR = Discretization.discretizeR(contR, 0.5);
assertTrue(
discRTruth.minus(discR).normF() < 1e-10,
"Expected these to be nearly equal:\ndiscR:\n" + discR + "\ndiscRTruth:\n" + discRTruth);
}
}

6
wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
View File

@@ -14,11 +14,9 @@ import org.junit.jupiter.api.Test;
public class NumericalIntegrationTest {
@Test
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public void testExponential() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
//noinspection SuspiciousNameCombination
var y1 =
NumericalIntegration.rk4(
(Matrix<N1, N1> x) -> {
@@ -33,11 +31,9 @@ public class NumericalIntegrationTest {
}
@Test
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public void testExponentialRKF45() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
//noinspection SuspiciousNameCombination
var y1 =
NumericalIntegration.rkf45(
(x, u) -> {
@@ -53,11 +49,9 @@ public class NumericalIntegrationTest {
}
@Test
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public void testExponentialRKDP() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
//noinspection SuspiciousNameCombination
var y1 =
NumericalIntegration.rkdp(
(x, u) -> {

41
wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java Normal file
View File

@@ -0,0 +1,41 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.system;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import java.util.function.BiFunction;
public final class RungeKuttaTimeVarying {
private RungeKuttaTimeVarying() {
// Utility class
}
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, y) for dt.
*
* @param <Rows> Rows in y.
* @param <Cols> Columns in y.
* @param f The function to integrate. It must take two arguments t and y.
* @param t The initial value of t.
* @param y The initial value of y.
* @param dtSeconds The time over which to integrate.
*/
@SuppressWarnings("MethodTypeParameterName")
public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rungeKuttaTimeVarying(
BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
double t,
Matrix<Rows, Cols> y,
double dtSeconds) {
final var h = dtSeconds;
Matrix<Rows, Cols> k1 = f.apply(t, y);
Matrix<Rows, Cols> k2 = f.apply(t + dtSeconds * 0.5, y.plus(k1.times(h * 0.5)));
Matrix<Rows, Cols> k3 = f.apply(t + dtSeconds * 0.5, y.plus(k2.times(h * 0.5)));
Matrix<Rows, Cols> k4 = f.apply(t + dtSeconds, y.plus(k3.times(h)));
return y.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
}
}

43
wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java Normal file
View File

@@ -0,0 +1,43 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.system;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.numbers.N1;
import org.junit.jupiter.api.Test;
public class RungeKuttaTimeVaryingTest {
private static Matrix<N1, N1> rungeKuttaTimeVaryingSolution(double t) {
return new MatBuilder<>(Nat.N1(), Nat.N1())
.fill(12.0 * Math.exp(t) / (Math.pow(Math.exp(t) + 1.0, 2.0)));
}
// Tests RK4 with a time varying solution. From
// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
// x' = x (2 / (e^t + 1) - 1)
//
// The true (analytical) solution is:
//
// x(t) = 12 * e^t / ((e^t + 1)^2)
@Test
public void rungeKuttaTimeVaryingTest() {
final var y0 = rungeKuttaTimeVaryingSolution(5.0);
final var y1 =
RungeKuttaTimeVarying.rungeKuttaTimeVarying(
(Double t, Matrix<N1, N1> x) -> {
return new MatBuilder<>(Nat.N1(), Nat.N1())
.fill(x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0));
},
5.0,
y0,
1.0);
assertEquals(rungeKuttaTimeVaryingSolution(6.0).get(0, 0), y1.get(0, 0), 1e-3);
}
}

49
wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
View File

@@ -10,6 +10,7 @@
#include "Eigen/Eigenvalues"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/RungeKuttaTimeVarying.h"
// Check that for a simple second-order system that we can easily analyze
// analytically,
@@ -26,8 +27,8 @@ TEST(DiscretizationTest, DiscretizeA) {
// We now have pos = vel = 1 and accel = 0, which should give us:
Eigen::Matrix<double, 2, 1> x1Truth;
x1Truth(1) = x0(1);
x1Truth(0) = x0(0) + 1.0 * x0(1);
x1Truth(0) = 1.0 * x0(0) + 1.0 * x0(1);
x1Truth(1) = 0.0 * x0(0) + 1.0 * x0(1);
EXPECT_EQ(x1Truth, x1Discrete);
}
@@ -53,8 +54,8 @@ TEST(DiscretizationTest, DiscretizeAB) {
// We now have pos = vel = accel = 1, which should give us:
Eigen::Matrix<double, 2, 1> x1Truth;
x1Truth(1) = x0(1) + 1.0 * u(0);
x1Truth(0) = x0(0) + 1.0 * x0(1) + 0.5 * u(0);
x1Truth(0) = 1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0);
x1Truth(1) = 0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0);
EXPECT_EQ(x1Truth, x1Discrete);
}
@@ -79,7 +80,7 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
},
Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discQ;
@@ -100,7 +101,7 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
Eigen::Matrix<double, 2, 2> contQ;
contQ << 0.0025, 0, 0, 1;
constexpr auto dt = 5.05_ms;
constexpr auto dt = 5_ms;
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<Eigen::Matrix<double, 2, 2>(
@@ -111,7 +112,7 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
},
Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discQ;
@@ -128,9 +129,6 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
Eigen::Matrix<double, 2, 2> contA;
contA << 0, 1, 0, 0;
Eigen::Matrix<double, 2, 1> contB;
contB << 0, 1;
Eigen::Matrix<double, 2, 2> contQ;
contQ << 1, 0, 0, 1;
@@ -139,12 +137,11 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
Eigen::Matrix<double, 2, 2> discQTaylor;
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discATaylor;
Eigen::Matrix<double, 2, 1> discB;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
for (int i = 0; i < contQ.rows(); i++) {
EXPECT_GT(esCont.eigenvalues()[i], 0);
for (int i = 0; i < contQ.rows(); ++i) {
EXPECT_GE(esCont.eigenvalues()[i], 0);
}
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
@@ -156,9 +153,9 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
},
Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-10)
@@ -169,8 +166,8 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
// Discrete Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
for (int i = 0; i < discQTaylor.rows(); i++) {
EXPECT_GT(esDisc.eigenvalues()[i], 0);
for (int i = 0; i < discQTaylor.rows(); ++i) {
EXPECT_GE(esDisc.eigenvalues()[i], 0);
}
}
@@ -179,23 +176,19 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
Eigen::Matrix<double, 2, 2> contA;
contA << 0, 1, 0, -1500;
Eigen::Matrix<double, 2, 1> contB;
contB << 0, 1;
Eigen::Matrix<double, 2, 2> contQ;
contQ << 0.0025, 0, 0, 1;
constexpr auto dt = 5.05_ms;
constexpr auto dt = 5_ms;
Eigen::Matrix<double, 2, 2> discQTaylor;
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discATaylor;
Eigen::Matrix<double, 2, 1> discB;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
for (int i = 0; i < contQ.rows(); i++) {
EXPECT_GT(esCont.eigenvalues()[i], 0);
for (int i = 0; i < contQ.rows(); ++i) {
EXPECT_GE(esCont.eigenvalues()[i], 0);
}
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
@@ -207,9 +200,9 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
},
Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-3)
@@ -220,8 +213,8 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
// Discrete Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
for (int i = 0; i < discQTaylor.rows(); i++) {
EXPECT_GT(esDisc.eigenvalues()[i], 0);
for (int i = 0; i < discQTaylor.rows(); ++i) {
EXPECT_GE(esDisc.eigenvalues()[i], 0);
}
}

29
wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
View File

@@ -67,32 +67,3 @@ TEST(NumericalIntegrationTest, ExponentialRKDP) {
y0, (Eigen::Matrix<double, 1, 1>() << 0.0).finished(), 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
namespace {
Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
return (Eigen::Matrix<double, 1, 1>()
<< 12.0 * std::exp(t) / (std::pow(std::exp(t) + 1.0, 2.0)))
.finished();
}
} // namespace
// Tests RungeKutta with a time varying solution.
// Now, lets test RK4 with a time varying solution. From
// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
// x' = x (2 / (e^t + 1) - 1)
//
// The true (analytical) solution is:
//
// x(t) = 12 * e^t / ((e^t + 1)^2)
TEST(NumericalIntegrationTest, RungeKuttaTimeVarying) {
Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
Eigen::Matrix<double, 1, 1> y1 = frc::RungeKuttaTimeVarying(
[](units::second_t t, Eigen::Matrix<double, 1, 1> x) {
return (Eigen::Matrix<double, 1, 1>()
<< x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0))
.finished();
},
y0, 5_s, 1_s);
EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
}

37
wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp Normal file
View File

@@ -0,0 +1,37 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <gtest/gtest.h>
#include <cmath>
#include "frc/system/RungeKuttaTimeVarying.h"
namespace {
Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
return (Eigen::Matrix<double, 1, 1>()
<< 12.0 * std::exp(t) / (std::pow(std::exp(t) + 1.0, 2.0)))
.finished();
}
} // namespace
// Tests RK4 with a time varying solution. From
// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
// x' = x (2 / (e^t + 1) - 1)
//
// The true (analytical) solution is:
//
// x(t) = 12 * e^t / ((e^t + 1)^2)
TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) {
Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
Eigen::Matrix<double, 1, 1> y1 = frc::RungeKuttaTimeVarying(
[](units::second_t t, Eigen::Matrix<double, 1, 1> x) {
return (Eigen::Matrix<double, 1, 1>()
<< x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0))
.finished();
},
5_s, y0, 1_s);
EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
}

34
wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h Normal file
View File

@@ -0,0 +1,34 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <array>
#include "Eigen/Core"
#include "units/time.h"
namespace frc {
/**
* Performs 4th order Runge-Kutta integration of dy/dt = f(t, y) for dt.
*
* @param f The function to integrate. It must take two arguments t and y.
* @param t The initial value of t.
* @param y The initial value of y.
* @param dt The time over which to integrate.
*/
template <typename F, typename T>
T RungeKuttaTimeVarying(F&& f, units::second_t t, T y, units::second_t dt) {
const auto h = dt.to<double>();
T k1 = f(t, y);
T k2 = f(t + dt * 0.5, y + h * k1 * 0.5);
T k3 = f(t + dt * 0.5, y + h * k2 * 0.5);
T k4 = f(t + dt, y + h * k3);
return y + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
} // namespace frc