mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-01 02:41:48 +00:00
[wpimath] Clean up NumericalIntegration and add Discretization tests (#3489)
* 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
@@ -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));
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
/**
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user