mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +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,74 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-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.controller;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N2;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
class ControlAffinePlantInversionFeedforwardTest {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
@Test
|
||||
void testCalculate() {
|
||||
ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
|
||||
new ControlAffinePlantInversionFeedforward<N2, N1>(
|
||||
Nat.N2(),
|
||||
Nat.N1(),
|
||||
this::getDynamics,
|
||||
0.02);
|
||||
|
||||
assertEquals(48.0, feedforward.calculate(
|
||||
VecBuilder.fill(2, 2),
|
||||
VecBuilder.fill(3, 3)).get(0, 0),
|
||||
1e-6);
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
@Test
|
||||
void testCalculateState() {
|
||||
Matrix<N2, N1> B = VecBuilder.fill(0, 1);
|
||||
|
||||
ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
|
||||
new ControlAffinePlantInversionFeedforward<N2, N1>(
|
||||
Nat.N2(),
|
||||
Nat.N1(),
|
||||
this::getStateDynamics,
|
||||
B,
|
||||
0.02);
|
||||
|
||||
assertEquals(48.0, feedforward.calculate(
|
||||
VecBuilder.fill(2, 2),
|
||||
VecBuilder.fill(3, 3)).get(0, 0),
|
||||
1e-6);
|
||||
}
|
||||
|
||||
@SuppressWarnings("ParameterName")
|
||||
protected Matrix<N2, N1> getDynamics(Matrix<N2, N1> x, Matrix<N1, N1> u) {
|
||||
var result = new Matrix<>(Nat.N2(), Nat.N1());
|
||||
|
||||
result = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x)
|
||||
.plus(VecBuilder.fill(0, 1).times(u));
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
@SuppressWarnings("ParameterName")
|
||||
protected Matrix<N2, N1> getStateDynamics(Matrix<N2, N1> x) {
|
||||
var result = new Matrix<>(Nat.N2(), Nat.N1());
|
||||
|
||||
result = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x);
|
||||
|
||||
return result;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.controller;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N2;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
class LinearPlantInversionFeedforwardTest {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
@Test
|
||||
void testCalculate() {
|
||||
Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
|
||||
Matrix<N2, N1> B = VecBuilder.fill(0, 1);
|
||||
|
||||
LinearPlantInversionFeedforward<N2, N1, N1> feedforward =
|
||||
new LinearPlantInversionFeedforward<N2, N1, N1>(A, B, 0.02);
|
||||
|
||||
assertEquals(47.502599, feedforward.calculate(
|
||||
VecBuilder.fill(2, 2),
|
||||
VecBuilder.fill(3, 3)).get(0, 0),
|
||||
0.002);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,116 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.controller;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import edu.wpi.first.wpilibj.system.LinearSystem;
|
||||
import edu.wpi.first.wpilibj.system.plant.DCMotor;
|
||||
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N2;
|
||||
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
public class LinearQuadraticRegulatorTest {
|
||||
public static LinearSystem<N2, N1, N1> elevatorPlant;
|
||||
static LinearSystem<N2, N1, N1> armPlant;
|
||||
|
||||
static {
|
||||
createArm();
|
||||
createElevator();
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public static void createArm() {
|
||||
var motors = DCMotor.getVex775Pro(2);
|
||||
|
||||
var m = 4.0;
|
||||
var r = 0.4;
|
||||
var J = 1d / 3d * m * r * r;
|
||||
var G = 100.0;
|
||||
|
||||
armPlant = LinearSystemId.createSingleJointedArmSystem(motors, J, G);
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public static void createElevator() {
|
||||
var motors = DCMotor.getVex775Pro(2);
|
||||
|
||||
var m = 5.0;
|
||||
var r = 0.0181864;
|
||||
var G = 1.0;
|
||||
elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
|
||||
}
|
||||
|
||||
@Test
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public void testLQROnElevator() {
|
||||
|
||||
var qElms = VecBuilder.fill(0.02, 0.4);
|
||||
var rElms = VecBuilder.fill(12.0);
|
||||
var dt = 0.00505;
|
||||
|
||||
var controller = new LinearQuadraticRegulator<>(
|
||||
elevatorPlant, qElms, rElms, dt);
|
||||
|
||||
var k = controller.getK();
|
||||
|
||||
assertEquals(522.153, k.get(0, 0), 0.1);
|
||||
assertEquals(38.2, k.get(0, 1), 0.1);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testFourMotorElevator() {
|
||||
|
||||
var dt = 0.020;
|
||||
|
||||
var plant = LinearSystemId.createElevatorSystem(
|
||||
DCMotor.getVex775Pro(4),
|
||||
8.0,
|
||||
0.75 * 25.4 / 1000.0,
|
||||
14.67);
|
||||
|
||||
var regulator = new LinearQuadraticRegulator<>(
|
||||
plant,
|
||||
VecBuilder.fill(0.1, 0.2),
|
||||
VecBuilder.fill(12.0),
|
||||
dt);
|
||||
|
||||
assertEquals(10.381, regulator.getK().get(0, 0), 1e-2);
|
||||
assertEquals(0.6929, regulator.getK().get(0, 1), 1e-2);
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public void testLQROnArm() {
|
||||
|
||||
var motors = DCMotor.getVex775Pro(2);
|
||||
|
||||
var m = 4.0;
|
||||
var r = 0.4;
|
||||
var G = 100.0;
|
||||
|
||||
var plant = LinearSystemId.createSingleJointedArmSystem(motors, 1d / 3d * m * r * r, G);
|
||||
|
||||
var qElms = VecBuilder.fill(0.01745, 0.08726);
|
||||
var rElms = VecBuilder.fill(12.0);
|
||||
var dt = 0.00505;
|
||||
|
||||
var controller = new LinearQuadraticRegulator<>(
|
||||
plant, qElms, rElms, dt);
|
||||
|
||||
var k = controller.getK();
|
||||
|
||||
assertEquals(19.16, k.get(0, 0), 0.1);
|
||||
assertEquals(3.32, k.get(0, 1), 0.1);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,169 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.controller;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Random;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import edu.wpi.first.wpilibj.estimator.KalmanFilter;
|
||||
import edu.wpi.first.wpilibj.system.LinearSystem;
|
||||
import edu.wpi.first.wpilibj.system.LinearSystemLoop;
|
||||
import edu.wpi.first.wpilibj.system.plant.DCMotor;
|
||||
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N2;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
public class LinearSystemLoopTest {
|
||||
public static final double kDt = 0.00505;
|
||||
private static final double kPositionStddev = 0.0001;
|
||||
private static final Random random = new Random();
|
||||
private final LinearSystemLoop<N2, N1, N1> m_loop;
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public LinearSystemLoopTest() {
|
||||
LinearSystem<N2, N1, N1> plant = LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5,
|
||||
0.0181864, 1.0);
|
||||
KalmanFilter<N2, N1, N1> observer = new KalmanFilter<>(Nat.N2(), Nat.N1(), plant,
|
||||
VecBuilder.fill(0.05, 1.0),
|
||||
VecBuilder.fill(0.0001), kDt);
|
||||
|
||||
var qElms = VecBuilder.fill(0.02, 0.4);
|
||||
var rElms = VecBuilder.fill(12.0);
|
||||
var dt = 0.00505;
|
||||
|
||||
var controller = new LinearQuadraticRegulator<>(
|
||||
plant, qElms, rElms, dt);
|
||||
|
||||
m_loop = new LinearSystemLoop<>(plant, controller, observer, 12, dt);
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
private static void updateTwoState(LinearSystemLoop<N2, N1, N1> loop, double noise) {
|
||||
Matrix<N1, N1> y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus(
|
||||
VecBuilder.fill(noise)
|
||||
);
|
||||
|
||||
loop.correct(y);
|
||||
loop.predict(kDt);
|
||||
}
|
||||
|
||||
@Test
|
||||
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
|
||||
public void testStateSpaceEnabled() {
|
||||
|
||||
m_loop.reset(VecBuilder.fill(0, 0));
|
||||
Matrix<N2, N1> references = VecBuilder.fill(2.0, 0.0);
|
||||
var constraints = new TrapezoidProfile.Constraints(4, 3);
|
||||
m_loop.setNextR(references);
|
||||
|
||||
TrapezoidProfile profile;
|
||||
TrapezoidProfile.State state;
|
||||
for (int i = 0; i < 1000; i++) {
|
||||
profile = new TrapezoidProfile(
|
||||
constraints, new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)),
|
||||
new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0))
|
||||
);
|
||||
state = profile.calculate(kDt);
|
||||
m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
|
||||
|
||||
updateTwoState(m_loop, (random.nextGaussian()) * kPositionStddev);
|
||||
var u = m_loop.getU(0);
|
||||
|
||||
assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
|
||||
}
|
||||
|
||||
assertEquals(2.0, m_loop.getXHat(0), 0.05);
|
||||
assertEquals(0.0, m_loop.getXHat(1), 0.5);
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
|
||||
public void testFlywheelEnabled() {
|
||||
|
||||
LinearSystem<N1, N1, N1> plant = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2),
|
||||
0.00289, 1.0);
|
||||
KalmanFilter<N1, N1, N1> observer = new KalmanFilter<>(Nat.N1(), Nat.N1(), plant,
|
||||
VecBuilder.fill(1.0),
|
||||
VecBuilder.fill(kPositionStddev), kDt);
|
||||
|
||||
var qElms = VecBuilder.fill(9.0);
|
||||
var rElms = VecBuilder.fill(12.0);
|
||||
|
||||
var controller = new LinearQuadraticRegulator<>(
|
||||
plant, qElms, rElms, kDt);
|
||||
|
||||
var feedforward = new LinearPlantInversionFeedforward<>(plant, kDt);
|
||||
|
||||
var loop = new LinearSystemLoop<>(plant, controller, feedforward, observer, 12);
|
||||
|
||||
loop.reset(VecBuilder.fill(0.0));
|
||||
var references = VecBuilder.fill(3000 / 60d * 2 * Math.PI);
|
||||
|
||||
loop.setNextR(references);
|
||||
|
||||
List<Double> timeData = new ArrayList<>();
|
||||
List<Double> xHat = new ArrayList<>();
|
||||
List<Double> volt = new ArrayList<>();
|
||||
List<Double> reference = new ArrayList<>();
|
||||
List<Double> error = new ArrayList<>();
|
||||
|
||||
var time = 0.0;
|
||||
while (time < 10) {
|
||||
|
||||
if (time > 10) {
|
||||
break;
|
||||
}
|
||||
|
||||
loop.setNextR(references);
|
||||
|
||||
Matrix<N1, N1> y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus(
|
||||
VecBuilder.fill(random.nextGaussian() * kPositionStddev)
|
||||
);
|
||||
|
||||
loop.correct(y);
|
||||
loop.predict(kDt);
|
||||
var u = loop.getU(0);
|
||||
|
||||
assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
|
||||
|
||||
xHat.add(loop.getXHat(0) / 2d / Math.PI * 60);
|
||||
volt.add(u);
|
||||
timeData.add(time);
|
||||
reference.add(references.get(0, 0) / 2d / Math.PI * 60);
|
||||
error.add(loop.getError(0) / 2d / Math.PI * 60);
|
||||
time += kDt;
|
||||
}
|
||||
|
||||
// XYChart bigChart = new XYChartBuilder().build();
|
||||
// bigChart.addSeries("Xhat, RPM", timeData, xHat);
|
||||
// bigChart.addSeries("Reference, RPM", timeData, reference);
|
||||
// bigChart.addSeries("Error, RPM", timeData, error);
|
||||
|
||||
// XYChart smolChart = new XYChartBuilder().build();
|
||||
// smolChart.addSeries("Volts, V", timeData, volt);
|
||||
|
||||
// try {
|
||||
// new SwingWrapper<>(List.of(bigChart, smolChart)).displayChartMatrix();
|
||||
// Thread.sleep(10000000);
|
||||
// } catch (InterruptedException e) { e.printStackTrace(); }
|
||||
|
||||
assertEquals(0.0, loop.getError(0), 0.1);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,219 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.estimator;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
|
||||
import edu.wpi.first.wpilibj.system.NumericalJacobian;
|
||||
import edu.wpi.first.wpilibj.system.RungeKutta;
|
||||
import edu.wpi.first.wpilibj.system.plant.DCMotor;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N2;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N3;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N5;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
@SuppressWarnings("ParameterName")
|
||||
public class ExtendedKalmanFilterTest {
|
||||
public static Matrix<N5, N1> getDynamics(final Matrix<N5, N1> x, final Matrix<N2, N1> u) {
|
||||
final var motors = DCMotor.getCIM(2);
|
||||
|
||||
final var gr = 7.08; // Gear ratio
|
||||
final var rb = 0.8382 / 2.0; // Wheelbase radius (track width)
|
||||
final var r = 0.0746125; // Wheel radius
|
||||
final var m = 63.503; // Robot mass
|
||||
final var J = 5.6; // Robot moment of inertia
|
||||
|
||||
final var C1 =
|
||||
-Math.pow(gr, 2) * motors.m_KtNMPerAmp / (
|
||||
motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r);
|
||||
final var C2 = gr * motors.m_KtNMPerAmp / (motors.m_rOhms * r);
|
||||
final var k1 = 1.0 / m + rb * rb / J;
|
||||
final var k2 = 1.0 / m - rb * rb / J;
|
||||
|
||||
final var vl = x.get(3, 0);
|
||||
final var vr = x.get(4, 0);
|
||||
final var Vl = u.get(0, 0);
|
||||
final var Vr = u.get(1, 0);
|
||||
|
||||
final Matrix<N5, N1> result = new Matrix<>(Nat.N5(), Nat.N1());
|
||||
final var v = 0.5 * (vl + vr);
|
||||
result.set(0, 0, v * Math.cos(x.get(2, 0)));
|
||||
result.set(1, 0, v * Math.sin(x.get(2, 0)));
|
||||
result.set(2, 0, (vr - vl) / (2.0 * rb));
|
||||
result.set(3, 0, k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)));
|
||||
result.set(4, 0, k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)));
|
||||
return result;
|
||||
}
|
||||
|
||||
public static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
|
||||
return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
|
||||
}
|
||||
|
||||
public static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
|
||||
return VecBuilder.fill(
|
||||
x.get(0, 0),
|
||||
x.get(1, 0),
|
||||
x.get(2, 0),
|
||||
x.get(3, 0),
|
||||
x.get(4, 0)
|
||||
);
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
@Test
|
||||
public void testInit() {
|
||||
double dtSeconds = 0.00505;
|
||||
|
||||
assertDoesNotThrow(() -> {
|
||||
ExtendedKalmanFilter<N5, N2, N3> observer =
|
||||
new ExtendedKalmanFilter<>(Nat.N5(), Nat.N2(), Nat.N3(),
|
||||
ExtendedKalmanFilterTest::getDynamics,
|
||||
ExtendedKalmanFilterTest::getLocalMeasurementModel,
|
||||
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
|
||||
VecBuilder.fill(0.0001, 0.01, 0.01), dtSeconds);
|
||||
|
||||
Matrix<N2, N1> u = VecBuilder.fill(12.0, 12.0);
|
||||
observer.predict(u, dtSeconds);
|
||||
|
||||
var localY = getLocalMeasurementModel(observer.getXhat(), u);
|
||||
observer.correct(u, localY);
|
||||
|
||||
var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
|
||||
var R = StateSpaceUtil.makeCostMatrix(
|
||||
VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
|
||||
observer.correct(Nat.N5(),
|
||||
u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
|
||||
});
|
||||
|
||||
}
|
||||
|
||||
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
|
||||
"PMD.ExcessiveMethodLength"})
|
||||
@Test
|
||||
public void testConvergence() {
|
||||
double dtSeconds = 0.00505;
|
||||
double rbMeters = 0.8382 / 2.0; // Robot radius
|
||||
|
||||
ExtendedKalmanFilter<N5, N2, N3> observer =
|
||||
new ExtendedKalmanFilter<>(Nat.N5(), Nat.N2(), Nat.N3(),
|
||||
ExtendedKalmanFilterTest::getDynamics,
|
||||
ExtendedKalmanFilterTest::getLocalMeasurementModel,
|
||||
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
|
||||
VecBuilder.fill(0.001, 0.01, 0.01), dtSeconds);
|
||||
|
||||
List<Pose2d> waypoints = Arrays.asList(new Pose2d(2.75, 22.521, new Rotation2d()),
|
||||
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
|
||||
var trajectory = TrajectoryGenerator.generateTrajectory(
|
||||
waypoints,
|
||||
new TrajectoryConfig(8.8, 0.1)
|
||||
);
|
||||
|
||||
Matrix<N5, N1> r = new Matrix<>(Nat.N5(), Nat.N1());
|
||||
|
||||
Matrix<N5, N1> nextR = new Matrix<>(Nat.N5(), Nat.N1());
|
||||
Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
|
||||
|
||||
List<Double> trajXs = new ArrayList<>();
|
||||
List<Double> trajYs = new ArrayList<>();
|
||||
|
||||
List<Double> observerXs = new ArrayList<>();
|
||||
List<Double> observerYs = new ArrayList<>();
|
||||
|
||||
var B = NumericalJacobian.numericalJacobianU(Nat.N5(), Nat.N2(),
|
||||
ExtendedKalmanFilterTest::getDynamics, new Matrix<>(Nat.N5(), Nat.N1()), u);
|
||||
|
||||
observer.setXhat(VecBuilder.fill(
|
||||
trajectory.getInitialPose().getTranslation().getX(),
|
||||
trajectory.getInitialPose().getTranslation().getY(),
|
||||
trajectory.getInitialPose().getRotation().getRadians(),
|
||||
0.0, 0.0));
|
||||
|
||||
var groundTruthX = observer.getXhat();
|
||||
|
||||
double totalTime = trajectory.getTotalTimeSeconds();
|
||||
for (int i = 0; i < (totalTime / dtSeconds); i++) {
|
||||
var ref = trajectory.sample(dtSeconds * i);
|
||||
double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
|
||||
double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
|
||||
|
||||
nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
|
||||
nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
|
||||
nextR.set(2, 0, ref.poseMeters.getRotation().getRadians());
|
||||
nextR.set(3, 0, vl);
|
||||
nextR.set(4, 0, vr);
|
||||
|
||||
var localY =
|
||||
getLocalMeasurementModel(groundTruthX, u);
|
||||
var whiteNoiseStdDevs = VecBuilder.fill(0.0001, 0.5, 0.5);
|
||||
observer.correct(u,
|
||||
localY.plus(StateSpaceUtil.makeWhiteNoiseVector(whiteNoiseStdDevs)));
|
||||
|
||||
Matrix<N5, N1> rdot = nextR.minus(r).div(dtSeconds);
|
||||
u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
|
||||
|
||||
observer.predict(u, dtSeconds);
|
||||
|
||||
groundTruthX = RungeKutta.rungeKutta(ExtendedKalmanFilterTest::getDynamics,
|
||||
groundTruthX, u, dtSeconds);
|
||||
|
||||
r = nextR;
|
||||
|
||||
trajXs.add(ref.poseMeters.getTranslation().getX());
|
||||
trajYs.add(ref.poseMeters.getTranslation().getY());
|
||||
|
||||
observerXs.add(observer.getXhat().get(0, 0));
|
||||
observerYs.add(observer.getXhat().get(1, 0));
|
||||
}
|
||||
|
||||
var localY = getLocalMeasurementModel(observer.getXhat(), u);
|
||||
observer.correct(u, localY);
|
||||
|
||||
var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
|
||||
var R = StateSpaceUtil.makeCostMatrix(
|
||||
VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
|
||||
observer.correct(Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
|
||||
|
||||
// var chartBuilder = new XYChartBuilder();
|
||||
// chartBuilder.title = "The Magic of Sensor Fusion, now with a "
|
||||
// + observer.getClass().getSimpleName();
|
||||
// var xyPosChart = chartBuilder.build();
|
||||
//
|
||||
// xyPosChart.setXAxisTitle("X pos, meters");
|
||||
// xyPosChart.setYAxisTitle("Y pos, meters");
|
||||
// xyPosChart.addSeries("Trajectory", trajXs, trajYs);
|
||||
// xyPosChart.addSeries("xHat", observerXs, observerYs);
|
||||
// new SwingWrapper<>(xyPosChart).displayChart();
|
||||
// try {
|
||||
// Thread.sleep(1000000000);
|
||||
// } catch (InterruptedException ex) {
|
||||
// ex.printStackTrace();
|
||||
// }
|
||||
|
||||
var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
|
||||
assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 1.0);
|
||||
assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 1.0);
|
||||
assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
|
||||
assertEquals(0.0, observer.getXhat(3), 1.0);
|
||||
assertEquals(0.0, observer.getXhat(4), 1.0);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,258 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.estimator;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Random;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.system.LinearSystem;
|
||||
import edu.wpi.first.wpilibj.system.plant.DCMotor;
|
||||
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N2;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N3;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N6;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
public class KalmanFilterTest {
|
||||
private static LinearSystem<N2, N1, N1> elevatorPlant;
|
||||
|
||||
private static final double kDt = 0.00505;
|
||||
|
||||
static {
|
||||
createElevator();
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public static void createElevator() {
|
||||
var motors = DCMotor.getVex775Pro(2);
|
||||
|
||||
var m = 5.0;
|
||||
var r = 0.0181864;
|
||||
var G = 1.0;
|
||||
elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
|
||||
}
|
||||
|
||||
// A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]^T,
|
||||
// Y is [x, y, theta]^T and u is [ax, ay, alpha}^T
|
||||
LinearSystem<N6, N3, N3> m_swerveObserverSystem = new LinearSystem<>(
|
||||
Matrix.mat(Nat.N6(), Nat.N6()).fill( // A
|
||||
0, 0, 0, 1, 0, 0,
|
||||
0, 0, 0, 0, 1, 0,
|
||||
0, 0, 0, 0, 0, 1,
|
||||
0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0),
|
||||
Matrix.mat(Nat.N6(), Nat.N3()).fill( // B
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
1, 0, 0,
|
||||
0, 1, 0,
|
||||
0, 0, 1
|
||||
),
|
||||
Matrix.mat(Nat.N3(), Nat.N6()).fill( // C
|
||||
1, 0, 0, 0, 0, 0,
|
||||
0, 1, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 0, 0
|
||||
),
|
||||
new Matrix<>(Nat.N3(), Nat.N3())); // D
|
||||
|
||||
@Test
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public void testElevatorKalmanFilter() {
|
||||
|
||||
var Q = VecBuilder.fill(0.05, 1.0);
|
||||
var R = VecBuilder.fill(0.0001);
|
||||
|
||||
assertDoesNotThrow(() -> new KalmanFilter<>(Nat.N2(), Nat.N1(), elevatorPlant, Q, R, kDt));
|
||||
}
|
||||
|
||||
@Test
|
||||
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
|
||||
public void testSwerveKFStationary() {
|
||||
|
||||
var random = new Random();
|
||||
|
||||
var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(),
|
||||
m_swerveObserverSystem,
|
||||
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
|
||||
// weights
|
||||
VecBuilder.fill(2, 2, 2), // measurement weights
|
||||
0.020
|
||||
);
|
||||
|
||||
List<Double> xhatsX = new ArrayList<>();
|
||||
List<Double> measurementsX = new ArrayList<>();
|
||||
List<Double> xhatsY = new ArrayList<>();
|
||||
List<Double> measurementsY = new ArrayList<>();
|
||||
|
||||
Matrix<N3, N1> measurement;
|
||||
for (int i = 0; i < 100; i++) {
|
||||
// the robot is at [0, 0, 0] so we just park here
|
||||
measurement = VecBuilder.fill(
|
||||
random.nextGaussian(), random.nextGaussian(), random.nextGaussian());
|
||||
filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
|
||||
|
||||
// we continue to not accelerate
|
||||
filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
|
||||
|
||||
measurementsX.add(measurement.get(0, 0));
|
||||
measurementsY.add(measurement.get(1, 0));
|
||||
xhatsX.add(filter.getXhat(0));
|
||||
xhatsY.add(filter.getXhat(1));
|
||||
}
|
||||
|
||||
//var chart = new XYChartBuilder().build();
|
||||
//chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
|
||||
//chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
|
||||
//try {
|
||||
// new SwingWrapper<>(chart).displayChart();
|
||||
// Thread.sleep(10000000);
|
||||
//} catch (Exception ign) {
|
||||
//}
|
||||
assertEquals(0.0, filter.getXhat(0), 0.3);
|
||||
assertEquals(0.0, filter.getXhat(0), 0.3);
|
||||
}
|
||||
|
||||
@Test
|
||||
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
|
||||
public void testSwerveKFMovingWithoutAccelerating() {
|
||||
|
||||
var random = new Random();
|
||||
|
||||
var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(),
|
||||
m_swerveObserverSystem,
|
||||
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
|
||||
// weights
|
||||
VecBuilder.fill(4, 4, 4), // measurement weights
|
||||
0.020
|
||||
);
|
||||
|
||||
List<Double> xhatsX = new ArrayList<>();
|
||||
List<Double> measurementsX = new ArrayList<>();
|
||||
List<Double> xhatsY = new ArrayList<>();
|
||||
List<Double> measurementsY = new ArrayList<>();
|
||||
|
||||
// we set the velocity of the robot so that it's moving forward slowly
|
||||
filter.setXhat(0, 0.5);
|
||||
filter.setXhat(1, 0.5);
|
||||
|
||||
for (int i = 0; i < 300; i++) {
|
||||
// the robot is at [0, 0, 0] so we just park here
|
||||
var measurement = VecBuilder.fill(
|
||||
random.nextGaussian() / 10d,
|
||||
random.nextGaussian() / 10d,
|
||||
random.nextGaussian() / 4d // std dev of [1, 1, 1]
|
||||
);
|
||||
|
||||
filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
|
||||
|
||||
// we continue to not accelerate
|
||||
filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
|
||||
|
||||
measurementsX.add(measurement.get(0, 0));
|
||||
measurementsY.add(measurement.get(1, 0));
|
||||
xhatsX.add(filter.getXhat(0));
|
||||
xhatsY.add(filter.getXhat(1));
|
||||
}
|
||||
|
||||
//var chart = new XYChartBuilder().build();
|
||||
//chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
|
||||
//chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
|
||||
//try {
|
||||
// new SwingWrapper<>(chart).displayChart();
|
||||
// Thread.sleep(10000000);
|
||||
//} catch (Exception ign) {}
|
||||
|
||||
assertEquals(0.0, filter.getXhat(0), 0.2);
|
||||
assertEquals(0.0, filter.getXhat(1), 0.2);
|
||||
}
|
||||
|
||||
@Test
|
||||
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
|
||||
public void testSwerveKFMovingOverTrajectory() {
|
||||
|
||||
var random = new Random();
|
||||
|
||||
var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(),
|
||||
m_swerveObserverSystem,
|
||||
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
|
||||
// weights
|
||||
VecBuilder.fill(4, 4, 4), // measurement weights
|
||||
0.020
|
||||
);
|
||||
|
||||
List<Double> xhatsX = new ArrayList<>();
|
||||
List<Double> measurementsX = new ArrayList<>();
|
||||
List<Double> xhatsY = new ArrayList<>();
|
||||
List<Double> measurementsY = new ArrayList<>();
|
||||
|
||||
var trajectory = TrajectoryGenerator.generateTrajectory(
|
||||
List.of(
|
||||
new Pose2d(0, 0, new Rotation2d()),
|
||||
new Pose2d(5, 5, new Rotation2d())
|
||||
), new TrajectoryConfig(2, 2)
|
||||
);
|
||||
var time = 0.0;
|
||||
var lastVelocity = VecBuilder.fill(0.0, 0.0, 0.0);
|
||||
|
||||
while (time <= trajectory.getTotalTimeSeconds()) {
|
||||
var sample = trajectory.sample(time);
|
||||
var measurement = VecBuilder.fill(
|
||||
sample.poseMeters.getTranslation().getX() + random.nextGaussian() / 5d,
|
||||
sample.poseMeters.getTranslation().getY() + random.nextGaussian() / 5d,
|
||||
sample.poseMeters.getRotation().getRadians() + random.nextGaussian() / 3d
|
||||
);
|
||||
|
||||
var velocity = VecBuilder.fill(
|
||||
sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getCos(),
|
||||
sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getSin(),
|
||||
sample.curvatureRadPerMeter * sample.velocityMetersPerSecond
|
||||
);
|
||||
var u = (velocity.minus(lastVelocity)).div(0.020);
|
||||
lastVelocity = velocity;
|
||||
|
||||
filter.correct(u, measurement);
|
||||
filter.predict(u, 0.020);
|
||||
|
||||
measurementsX.add(measurement.get(0, 0));
|
||||
measurementsY.add(measurement.get(1, 0));
|
||||
xhatsX.add(filter.getXhat(0));
|
||||
xhatsY.add(filter.getXhat(1));
|
||||
|
||||
time += 0.020;
|
||||
}
|
||||
|
||||
//var chart = new XYChartBuilder().build();
|
||||
//chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
|
||||
//chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
|
||||
//try {
|
||||
// new SwingWrapper<>(chart).displayChart();
|
||||
// Thread.sleep(10000000);
|
||||
//} catch (Exception ign) {
|
||||
//}
|
||||
|
||||
assertEquals(trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters
|
||||
.getTranslation().getX(), filter.getXhat(0), 0.2);
|
||||
assertEquals(trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters
|
||||
.getTranslation().getY(), filter.getXhat(1), 0.2);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.estimator;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
public class MerweScaledSigmaPointsTest {
|
||||
@Test
|
||||
public void testZeroMeanPoints() {
|
||||
var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
|
||||
var points = merweScaledSigmaPoints.sigmaPoints(VecBuilder.fill(0, 0),
|
||||
Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1));
|
||||
|
||||
assertTrue(points.isEqual(Matrix.mat(Nat.N2(), Nat.N5()).fill(
|
||||
0.0, 0.00173205, 0.0, -0.00173205, 0.0,
|
||||
0.0, 0.0, 0.00173205, 0.0, -0.00173205
|
||||
), 1E-6));
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testNonzeroMeanPoints() {
|
||||
var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
|
||||
var points = merweScaledSigmaPoints.sigmaPoints(VecBuilder.fill(1, 2),
|
||||
Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 10));
|
||||
|
||||
assertTrue(points.isEqual(Matrix.mat(Nat.N2(), Nat.N5()).fill(
|
||||
1.0, 1.00173205, 1.0, 0.99826795, 1.0,
|
||||
2.0, 2.0, 2.00547723, 2.0, 1.99452277
|
||||
), 1E-6));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,396 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.estimator;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.math.Discretization;
|
||||
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
|
||||
import edu.wpi.first.wpilibj.system.NumericalJacobian;
|
||||
import edu.wpi.first.wpilibj.system.RungeKutta;
|
||||
import edu.wpi.first.wpilibj.system.plant.DCMotor;
|
||||
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N2;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N4;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N6;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
|
||||
public class UnscentedKalmanFilterTest {
|
||||
@SuppressWarnings({"LocalVariableName", "ParameterName"})
|
||||
public static Matrix<N6, N1> getDynamics(Matrix<N6, N1> x, Matrix<N2, N1> u) {
|
||||
var motors = DCMotor.getCIM(2);
|
||||
|
||||
var gHigh = 7.08;
|
||||
var rb = 0.8382 / 2.0;
|
||||
var r = 0.0746125;
|
||||
var m = 63.503;
|
||||
var J = 5.6;
|
||||
|
||||
var C1 = -Math.pow(gHigh, 2) * motors.m_KtNMPerAmp
|
||||
/ (motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r);
|
||||
var C2 = gHigh * motors.m_KtNMPerAmp / (motors.m_rOhms * r);
|
||||
|
||||
var c = x.get(2, 0);
|
||||
var s = x.get(3, 0);
|
||||
var vl = x.get(4, 0);
|
||||
var vr = x.get(5, 0);
|
||||
|
||||
var Vl = u.get(0, 0);
|
||||
var Vr = u.get(1, 0);
|
||||
|
||||
var k1 = 1.0 / m + rb * rb / J;
|
||||
var k2 = 1.0 / m - rb * rb / J;
|
||||
|
||||
var xvel = (vl + vr) / 2;
|
||||
var w = (vr - vl) / (2.0 * rb);
|
||||
|
||||
return VecBuilder.fill(
|
||||
xvel * c,
|
||||
xvel * s,
|
||||
-s * w,
|
||||
c * w,
|
||||
k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)),
|
||||
k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr))
|
||||
);
|
||||
}
|
||||
|
||||
@SuppressWarnings("ParameterName")
|
||||
public static Matrix<N4, N1> getLocalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
|
||||
return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0), x.get(5, 0));
|
||||
}
|
||||
|
||||
@SuppressWarnings("ParameterName")
|
||||
public static Matrix<N6, N1> getGlobalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
|
||||
return x.copy();
|
||||
}
|
||||
|
||||
@Test
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public void testInit() {
|
||||
assertDoesNotThrow(() -> {
|
||||
UnscentedKalmanFilter<N6, N2, N4> observer = new UnscentedKalmanFilter<>(
|
||||
Nat.N6(), Nat.N4(),
|
||||
UnscentedKalmanFilterTest::getDynamics,
|
||||
UnscentedKalmanFilterTest::getLocalMeasurementModel,
|
||||
VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
|
||||
VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
|
||||
0.00505);
|
||||
|
||||
var u = VecBuilder.fill(12.0, 12.0);
|
||||
observer.predict(u, 0.00505);
|
||||
|
||||
var localY = getLocalMeasurementModel(observer.getXhat(), u);
|
||||
observer.correct(u, localY);
|
||||
});
|
||||
}
|
||||
|
||||
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
|
||||
"PMD.ExcessiveMethodLength"})
|
||||
@Test
|
||||
public void testConvergence() {
|
||||
double dtSeconds = 0.00505;
|
||||
double rbMeters = 0.8382 / 2.0; // Robot radius
|
||||
|
||||
List<Double> trajXs = new ArrayList<>();
|
||||
List<Double> trajYs = new ArrayList<>();
|
||||
|
||||
List<Double> observerXs = new ArrayList<>();
|
||||
List<Double> observerYs = new ArrayList<>();
|
||||
List<Double> observerC = new ArrayList<>();
|
||||
List<Double> observerS = new ArrayList<>();
|
||||
List<Double> observervl = new ArrayList<>();
|
||||
List<Double> observervr = new ArrayList<>();
|
||||
|
||||
List<Double> inputVl = new ArrayList<>();
|
||||
List<Double> inputVr = new ArrayList<>();
|
||||
|
||||
List<Double> timeData = new ArrayList<>();
|
||||
List<Matrix<?, ?>> rdots = new ArrayList<>();
|
||||
|
||||
UnscentedKalmanFilter<N6, N2, N4> observer = new UnscentedKalmanFilter<>(
|
||||
Nat.N6(), Nat.N4(),
|
||||
UnscentedKalmanFilterTest::getDynamics,
|
||||
UnscentedKalmanFilterTest::getLocalMeasurementModel,
|
||||
VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
|
||||
VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
|
||||
dtSeconds);
|
||||
|
||||
List<Pose2d> waypoints = Arrays.asList(new Pose2d(2.75, 22.521, new Rotation2d()),
|
||||
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
|
||||
var trajectory = TrajectoryGenerator.generateTrajectory(
|
||||
waypoints,
|
||||
new TrajectoryConfig(8.8, 0.1)
|
||||
);
|
||||
|
||||
Matrix<N6, N1> nextR;
|
||||
Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
|
||||
|
||||
var B = NumericalJacobian.numericalJacobianU(Nat.N6(), Nat.N2(),
|
||||
UnscentedKalmanFilterTest::getDynamics, new Matrix<>(Nat.N6(), Nat.N1()), u);
|
||||
|
||||
observer.setXhat(VecBuilder.fill(2.75, 22.521, 1.0, 0.0, 0.0, 0.0)); // TODO not hard code this
|
||||
|
||||
var ref = trajectory.sample(0.0);
|
||||
|
||||
Matrix<N6, N1> r = VecBuilder.fill(
|
||||
ref.poseMeters.getTranslation().getX(),
|
||||
ref.poseMeters.getTranslation().getY(),
|
||||
ref.poseMeters.getRotation().getCos(),
|
||||
ref.poseMeters.getRotation().getSin(),
|
||||
ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)),
|
||||
ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters))
|
||||
);
|
||||
nextR = r.copy();
|
||||
|
||||
var trueXhat = observer.getXhat();
|
||||
|
||||
double totalTime = trajectory.getTotalTimeSeconds();
|
||||
for (int i = 0; i < (totalTime / dtSeconds); i++) {
|
||||
|
||||
ref = trajectory.sample(dtSeconds * i);
|
||||
double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
|
||||
double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
|
||||
|
||||
nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
|
||||
nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
|
||||
nextR.set(2, 0, ref.poseMeters.getRotation().getCos());
|
||||
nextR.set(3, 0, ref.poseMeters.getRotation().getSin());
|
||||
nextR.set(4, 0, vl);
|
||||
nextR.set(5, 0, vr);
|
||||
|
||||
Matrix<N4, N1> localY =
|
||||
getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
|
||||
var noiseStdDev = VecBuilder.fill(0.001, 0.001, 0.5, 0.5);
|
||||
|
||||
observer.correct(u,
|
||||
localY.plus(StateSpaceUtil.makeWhiteNoiseVector(
|
||||
noiseStdDev)));
|
||||
|
||||
var rdot = nextR.minus(r).div(dtSeconds);
|
||||
u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
|
||||
|
||||
rdots.add(rdot);
|
||||
|
||||
trajXs.add(ref.poseMeters.getTranslation().getX());
|
||||
trajYs.add(ref.poseMeters.getTranslation().getY());
|
||||
|
||||
observerXs.add(observer.getXhat().get(0, 0));
|
||||
observerYs.add(observer.getXhat().get(1, 0));
|
||||
|
||||
observerC.add(observer.getXhat(2));
|
||||
observerS.add(observer.getXhat(3));
|
||||
|
||||
observervl.add(observer.getXhat(4));
|
||||
observervr.add(observer.getXhat(5));
|
||||
|
||||
inputVl.add(u.get(0, 0));
|
||||
inputVr.add(u.get(1, 0));
|
||||
|
||||
timeData.add(i * dtSeconds);
|
||||
|
||||
r = nextR;
|
||||
observer.predict(u, dtSeconds);
|
||||
trueXhat = RungeKutta.rungeKutta(UnscentedKalmanFilterTest::getDynamics,
|
||||
trueXhat, u, dtSeconds);
|
||||
}
|
||||
|
||||
var localY = getLocalMeasurementModel(trueXhat, u);
|
||||
observer.correct(u, localY);
|
||||
|
||||
var globalY = getGlobalMeasurementModel(trueXhat, u);
|
||||
var R = StateSpaceUtil.makeCostMatrix(
|
||||
VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5));
|
||||
observer.correct(Nat.N6(), u, globalY,
|
||||
UnscentedKalmanFilterTest::getGlobalMeasurementModel, R);
|
||||
|
||||
final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
|
||||
|
||||
// var chartBuilder = new XYChartBuilder();
|
||||
// chartBuilder.title = "The Magic of Sensor Fusion, now with a "
|
||||
// + observer.getClass().getSimpleName();
|
||||
// var xyPosChart = chartBuilder.build();
|
||||
|
||||
// xyPosChart.setXAxisTitle("X pos, meters");
|
||||
// xyPosChart.setYAxisTitle("Y pos, meters");
|
||||
// xyPosChart.addSeries("Trajectory", trajXs, trajYs);
|
||||
// xyPosChart.addSeries("xHat", observerXs, observerYs);
|
||||
|
||||
// var stateChart = new XYChartBuilder()
|
||||
// .title("States (x-hat)").build();
|
||||
// stateChart.addSeries("Cos", timeData, observerC);
|
||||
// stateChart.addSeries("Sin", timeData, observerS);
|
||||
// stateChart.addSeries("vl, m/s", timeData, observervl);
|
||||
// stateChart.addSeries("vr, m/s", timeData, observervr);
|
||||
|
||||
// var inputChart = new XYChartBuilder().title("Inputs").build();
|
||||
// inputChart.addSeries("Left voltage", timeData, inputVl);
|
||||
// inputChart.addSeries("Right voltage", timeData, inputVr);
|
||||
|
||||
// var rdotChart = new XYChartBuilder().title("Rdot").build();
|
||||
// rdotChart.addSeries("xdot, or vx", timeData, rdots.stream().map(it -> it.get(0, 0))
|
||||
// .collect(Collectors.toList()));
|
||||
// rdotChart.addSeries("ydot, or vy", timeData, rdots.stream().map(it -> it.get(1, 0))
|
||||
// .collect(Collectors.toList()));
|
||||
// rdotChart.addSeries("cos dot", timeData, rdots.stream().map(it -> it.get(2, 0))
|
||||
// .collect(Collectors.toList()));
|
||||
// rdotChart.addSeries("sin dot", timeData, rdots.stream().map(it -> it.get(3, 0))
|
||||
// .collect(Collectors.toList()));
|
||||
// rdotChart.addSeries("vl dot, or al", timeData, rdots.stream().map(it -> it.get(4, 0))
|
||||
// .collect(Collectors.toList()));
|
||||
// rdotChart.addSeries("vr dot, or ar", timeData, rdots.stream().map(it -> it.get(5, 0))
|
||||
// .collect(Collectors.toList()));
|
||||
|
||||
// List<XYChart> charts = new ArrayList<>();
|
||||
// charts.add(xyPosChart);
|
||||
// charts.add(stateChart);
|
||||
// charts.add(inputChart);
|
||||
// charts.add(rdotChart);
|
||||
// new SwingWrapper<>(charts).displayChartMatrix();
|
||||
// try {
|
||||
// Thread.sleep(1000000000);
|
||||
// } catch (InterruptedException ex) {
|
||||
// ex.printStackTrace();
|
||||
// }
|
||||
|
||||
assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.25);
|
||||
assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.25);
|
||||
assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
|
||||
assertEquals(0.0, observer.getXhat(3), 1.0);
|
||||
assertEquals(0.0, observer.getXhat(4), 1.0);
|
||||
}
|
||||
|
||||
@Test
|
||||
@SuppressWarnings({"LocalVariableName", "ParameterName", "PMD.AvoidInstantiatingObjectsInLoops"})
|
||||
public void testLinearUKF() {
|
||||
var dt = 0.020;
|
||||
var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
|
||||
var observer = new UnscentedKalmanFilter<>(Nat.N1(), Nat.N1(),
|
||||
(x, u) -> plant.getA().times(x).plus(plant.getB().times(u)),
|
||||
plant::calculateY,
|
||||
VecBuilder.fill(0.05),
|
||||
VecBuilder.fill(1.0),
|
||||
dt);
|
||||
|
||||
var time = new ArrayList<Double>();
|
||||
var refData = new ArrayList<Double>();
|
||||
var xhat = new ArrayList<Double>();
|
||||
var udata = new ArrayList<Double>();
|
||||
var xdotData = new ArrayList<Double>();
|
||||
|
||||
var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dt);
|
||||
var discA = discABPair.getFirst();
|
||||
var discB = discABPair.getSecond();
|
||||
|
||||
Matrix<N1, N1> ref = VecBuilder.fill(100);
|
||||
Matrix<N1, N1> u = VecBuilder.fill(0);
|
||||
|
||||
Matrix<N1, N1> xdot;
|
||||
for (int i = 0; i < (2.0 / dt); i++) {
|
||||
observer.predict(u, dt);
|
||||
|
||||
u = discB.solve(ref.minus(discA.times(ref)));
|
||||
|
||||
xdot = plant.getA().times(observer.getXhat()).plus(plant.getB().times(u));
|
||||
|
||||
time.add(i * dt);
|
||||
refData.add(ref.get(0, 0));
|
||||
xhat.add(observer.getXhat(0));
|
||||
udata.add(u.get(0, 0));
|
||||
xdotData.add(xdot.get(0, 0));
|
||||
}
|
||||
|
||||
// var chartBuilder = new XYChartBuilder();
|
||||
// chartBuilder.title = "The Magic of Sensor Fusion";
|
||||
// var chart = chartBuilder.build();
|
||||
|
||||
// chart.addSeries("Ref", time, refData);
|
||||
// chart.addSeries("xHat", time, xhat);
|
||||
// chart.addSeries("input", time, udata);
|
||||
//// chart.addSeries("xdot", time, xdotData);
|
||||
|
||||
// new SwingWrapper<>(chart).displayChart();
|
||||
// try {
|
||||
// Thread.sleep(1000000000);
|
||||
// } catch (InterruptedException e) {
|
||||
// }
|
||||
|
||||
assertEquals(ref.get(0, 0), observer.getXhat(0), 5);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testUnscentedTransform() {
|
||||
// From FilterPy
|
||||
var ret = UnscentedKalmanFilter.unscentedTransform(Nat.N4(), Nat.N4(),
|
||||
Matrix.mat(Nat.N4(), Nat.N9()).fill(
|
||||
-0.9, -0.822540333075852, -0.8922540333075852, -0.9,
|
||||
-0.9, -0.9774596669241481, -0.9077459666924148, -0.9, -0.9,
|
||||
1.0, 1.0, 1.077459666924148, 1.0, 1.0, 1.0, 0.9225403330758519, 1.0, 1.0,
|
||||
-0.9, -0.9, -0.9, -0.822540333075852, -0.8922540333075852, -0.9,
|
||||
-0.9, -0.9774596669241481, -0.9077459666924148,
|
||||
1.0, 1.0, 1.0, 1.0, 1.077459666924148, 1.0, 1.0, 1.0, 0.9225403330758519
|
||||
),
|
||||
VecBuilder.fill(
|
||||
-132.33333333,
|
||||
16.66666667,
|
||||
16.66666667,
|
||||
16.66666667,
|
||||
16.66666667,
|
||||
16.66666667,
|
||||
16.66666667,
|
||||
16.66666667,
|
||||
16.66666667
|
||||
),
|
||||
VecBuilder.fill(
|
||||
-129.34333333,
|
||||
16.66666667,
|
||||
16.66666667,
|
||||
16.66666667,
|
||||
16.66666667,
|
||||
16.66666667,
|
||||
16.66666667,
|
||||
16.66666667,
|
||||
16.66666667
|
||||
)
|
||||
);
|
||||
|
||||
assertTrue(
|
||||
VecBuilder.fill(-0.9, 1, -0.9, 1).isEqual(
|
||||
ret.getFirst(), 1E-5
|
||||
));
|
||||
|
||||
assertTrue(
|
||||
Matrix.mat(Nat.N4(), Nat.N4()).fill(
|
||||
2.02000002e-01, 2.00000500e-02, -2.69044710e-29,
|
||||
-4.59511477e-29,
|
||||
2.00000500e-02, 2.00001000e-01, -2.98781068e-29,
|
||||
-5.12759588e-29,
|
||||
-2.73372625e-29, -3.09882635e-29, 2.02000002e-01,
|
||||
2.00000500e-02,
|
||||
-4.67065917e-29, -5.10705197e-29, 2.00000500e-02,
|
||||
2.00001000e-01
|
||||
).isEqual(
|
||||
ret.getSecond(), 1E-5
|
||||
));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,200 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import org.ejml.dense.row.MatrixFeatures_DDRM;
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.SimpleMatrixUtils;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N2;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertFalse;
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
@SuppressWarnings("PMD.TooManyMethods")
|
||||
public class StateSpaceUtilTest {
|
||||
@Test
|
||||
public void testCostArray() {
|
||||
var mat = StateSpaceUtil.makeCostMatrix(
|
||||
VecBuilder.fill(1.0, 2.0, 3.0));
|
||||
|
||||
assertEquals(1.0, mat.get(0, 0), 1e-3);
|
||||
assertEquals(0.0, mat.get(0, 1), 1e-3);
|
||||
assertEquals(0.0, mat.get(0, 2), 1e-3);
|
||||
assertEquals(0.0, mat.get(1, 0), 1e-3);
|
||||
assertEquals(1.0 / 4.0, mat.get(1, 1), 1e-3);
|
||||
assertEquals(0.0, mat.get(1, 2), 1e-3);
|
||||
assertEquals(0.0, mat.get(0, 2), 1e-3);
|
||||
assertEquals(0.0, mat.get(1, 2), 1e-3);
|
||||
assertEquals(1.0 / 9.0, mat.get(2, 2), 1e-3);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testCovArray() {
|
||||
var mat = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(),
|
||||
VecBuilder.fill(1.0, 2.0, 3.0));
|
||||
|
||||
assertEquals(1.0, mat.get(0, 0), 1e-3);
|
||||
assertEquals(0.0, mat.get(0, 1), 1e-3);
|
||||
assertEquals(0.0, mat.get(0, 2), 1e-3);
|
||||
assertEquals(0.0, mat.get(1, 0), 1e-3);
|
||||
assertEquals(4.0, mat.get(1, 1), 1e-3);
|
||||
assertEquals(0.0, mat.get(1, 2), 1e-3);
|
||||
assertEquals(0.0, mat.get(0, 2), 1e-3);
|
||||
assertEquals(0.0, mat.get(1, 2), 1e-3);
|
||||
assertEquals(9.0, mat.get(2, 2), 1e-3);
|
||||
}
|
||||
|
||||
@Test
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public void testIsStabilizable() {
|
||||
Matrix<N2, N2> A;
|
||||
Matrix<N2, N1> B = VecBuilder.fill(0, 1);
|
||||
|
||||
// First eigenvalue is uncontrollable and unstable.
|
||||
// Second eigenvalue is controllable and stable.
|
||||
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5);
|
||||
assertFalse(StateSpaceUtil.isStabilizable(A, B));
|
||||
|
||||
// First eigenvalue is uncontrollable and marginally stable.
|
||||
// Second eigenvalue is controllable and stable.
|
||||
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5);
|
||||
assertFalse(StateSpaceUtil.isStabilizable(A, B));
|
||||
|
||||
// First eigenvalue is uncontrollable and stable.
|
||||
// Second eigenvalue is controllable and stable.
|
||||
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5);
|
||||
assertTrue(StateSpaceUtil.isStabilizable(A, B));
|
||||
|
||||
// First eigenvalue is uncontrollable and stable.
|
||||
// Second eigenvalue is controllable and unstable.
|
||||
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2);
|
||||
assertTrue(StateSpaceUtil.isStabilizable(A, B));
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testMakeWhiteNoiseVector() {
|
||||
var firstData = new ArrayList<Double>();
|
||||
var secondData = new ArrayList<Double>();
|
||||
for (int i = 0; i < 1000; i++) {
|
||||
var noiseVec = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(1.0, 2.0));
|
||||
firstData.add(noiseVec.get(0, 0));
|
||||
secondData.add(noiseVec.get(1, 0));
|
||||
}
|
||||
assertEquals(1.0, calculateStandardDeviation(firstData), 0.2);
|
||||
assertEquals(2.0, calculateStandardDeviation(secondData), 0.2);
|
||||
}
|
||||
|
||||
private double calculateStandardDeviation(List<Double> numArray) {
|
||||
double sum = 0.0;
|
||||
double standardDeviation = 0.0;
|
||||
int length = numArray.size();
|
||||
|
||||
for (double num : numArray) {
|
||||
sum += num;
|
||||
}
|
||||
|
||||
double mean = sum / length;
|
||||
|
||||
for (double num : numArray) {
|
||||
standardDeviation += Math.pow(num - mean, 2);
|
||||
}
|
||||
|
||||
return Math.sqrt(standardDeviation / length);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testDiscretizeA() {
|
||||
var contA = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
|
||||
var x0 = VecBuilder.fill(1, 1);
|
||||
var discA = Discretization.discretizeA(contA, 1.0);
|
||||
var x1Discrete = discA.times(x0);
|
||||
|
||||
// We now have pos = vel = 1 and accel = 0, which should give us:
|
||||
var x1Truth = VecBuilder.fill(x0.get(0, 0) + 1.0 * x0.get(1, 0),
|
||||
x0.get(1, 0));
|
||||
assertTrue(x1Truth.isEqual(x1Discrete, 1E-4));
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
@Test
|
||||
public void testDiscretizeAB() {
|
||||
var contA = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
|
||||
var contB = VecBuilder.fill(0, 1);
|
||||
var x0 = VecBuilder.fill(1, 1);
|
||||
var u = VecBuilder.fill(1);
|
||||
|
||||
var abPair = Discretization.discretizeAB(contA, contB, 1.0);
|
||||
|
||||
var x1Discrete = abPair.getFirst().times(x0).plus(abPair.getSecond().times(u));
|
||||
|
||||
// We now have pos = vel = accel = 1, which should give us:
|
||||
var x1Truth = VecBuilder.fill(x0.get(0, 0) + x0.get(1, 0) + 0.5 * u.get(0, 0), x0.get(0, 0)
|
||||
+ u.get(0, 0));
|
||||
|
||||
assertTrue(x1Truth.isEqual(x1Discrete, 1E-4));
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testMatrixExp() {
|
||||
Matrix<N2, N2> wrappedMatrix = Matrix.eye(Nat.N2());
|
||||
var wrappedResult = wrappedMatrix.exp();
|
||||
|
||||
assertTrue(wrappedResult.isEqual(
|
||||
Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
|
||||
|
||||
var matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4);
|
||||
wrappedResult = matrix.times(0.01).exp();
|
||||
|
||||
assertTrue(wrappedResult.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912,
|
||||
0.03076368, 1.04111993), 1E-8));
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testSimpleMatrixExp() {
|
||||
SimpleMatrix matrix = SimpleMatrixUtils.eye(2);
|
||||
var result = SimpleMatrixUtils.exp(matrix);
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isIdentical(
|
||||
result.getDDRM(),
|
||||
new SimpleMatrix(2, 2, true, new double[]{Math.E, 0, 0, Math.E}).getDDRM(),
|
||||
1E-9
|
||||
));
|
||||
|
||||
matrix = new SimpleMatrix(2, 2, true, new double[]{1, 2, 3, 4});
|
||||
result = SimpleMatrixUtils.exp(matrix.scale(0.01));
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isIdentical(
|
||||
result.getDDRM(),
|
||||
new SimpleMatrix(2, 2, true, new double[]{1.01035625, 0.02050912,
|
||||
0.03076368, 1.04111993}).getDDRM(),
|
||||
1E-8
|
||||
));
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testPoseToVector() {
|
||||
Pose2d pose = new Pose2d(1, 2, new Rotation2d(3));
|
||||
var vector = StateSpaceUtil.poseToVector(pose);
|
||||
assertEquals(pose.getTranslation().getX(), vector.get(0, 0), 1e-6);
|
||||
assertEquals(pose.getTranslation().getY(), vector.get(1, 0), 1e-6);
|
||||
assertEquals(pose.getRotation().getRadians(), vector.get(2, 0), 1e-6);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,91 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.system;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import edu.wpi.first.wpilibj.system.plant.DCMotor;
|
||||
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
class LinearSystemIDTest {
|
||||
@Test
|
||||
public void testDrivetrainVelocitySystem() {
|
||||
var model = LinearSystemId.createDrivetrainVelocitySystem(
|
||||
DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6
|
||||
);
|
||||
assertTrue(model.getA().isEqual(Matrix.mat(Nat.N2(),
|
||||
Nat.N2()).fill(-10.14132, 3.06598, 3.06598, -10.14132), 0.001));
|
||||
|
||||
assertTrue(model.getB().isEqual(Matrix.mat(Nat.N2(),
|
||||
Nat.N2()).fill(4.2590, -1.28762, -1.2876, 4.2590), 0.001));
|
||||
|
||||
assertTrue(model.getC().isEqual(Matrix.mat(Nat.N2(),
|
||||
Nat.N2()).fill(1.0, 0.0, 0.0, 1.0), 0.001));
|
||||
|
||||
assertTrue(model.getD().isEqual(Matrix.mat(Nat.N2(),
|
||||
Nat.N2()).fill(0.0, 0.0, 0.0, 0.0), 0.001));
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testElevatorSystem() {
|
||||
|
||||
var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12);
|
||||
assertTrue(model.getA().isEqual(Matrix.mat(Nat.N2(),
|
||||
Nat.N2()).fill(0, 1, 0, -99.05473), 0.001));
|
||||
|
||||
assertTrue(model.getB().isEqual(VecBuilder.fill(0, 20.8), 0.001));
|
||||
|
||||
assertTrue(model.getC().isEqual(Matrix.mat(Nat.N1(),
|
||||
Nat.N2()).fill(1, 0), 0.001));
|
||||
|
||||
assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testFlywheelSystem() {
|
||||
var model = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00032, 1.0);
|
||||
assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001));
|
||||
|
||||
assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001));
|
||||
|
||||
assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001));
|
||||
|
||||
assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testIdentifyPositionSystem() {
|
||||
// By controls engineering in frc,
|
||||
// x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
|
||||
var kv = 1.0;
|
||||
var ka = 0.5;
|
||||
var model = LinearSystemId.identifyPositionSystem(kv, ka);
|
||||
|
||||
assertEquals(model.getA(), Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka));
|
||||
assertEquals(model.getB(), VecBuilder.fill(0, 1 / ka));
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testIdentifyVelocitySystem() {
|
||||
// By controls engineering in frc,
|
||||
// V = kv * velocity + ka * acceleration
|
||||
// x-dot = -kv/ka * v + 1/ka \cdot V
|
||||
var kv = 1.0;
|
||||
var ka = 0.5;
|
||||
var model = LinearSystemId.identifyVelocitySystem(kv, ka);
|
||||
|
||||
assertEquals(model.getA(), VecBuilder.fill(-kv / ka));
|
||||
assertEquals(model.getB(), VecBuilder.fill(1 / ka));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.system;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
public class RungeKuttaTest {
|
||||
@Test
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public void testExponential() {
|
||||
|
||||
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
|
||||
|
||||
//noinspection SuspiciousNameCombination
|
||||
var y1 = RungeKutta.rungeKutta((Matrix<N1, N1> x) -> {
|
||||
var y = new Matrix<>(Nat.N1(), Nat.N1());
|
||||
y.set(0, 0, Math.exp(x.get(0, 0)));
|
||||
return y; },
|
||||
y0, 0.1
|
||||
);
|
||||
|
||||
assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
|
||||
}
|
||||
}
|
||||
@@ -8,8 +8,6 @@
|
||||
package edu.wpi.first.wpiutil.math;
|
||||
|
||||
import org.ejml.data.SingularMatrixException;
|
||||
import org.ejml.dense.row.MatrixFeatures_DDRM;
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
@@ -17,107 +15,106 @@ import edu.wpi.first.wpiutil.math.numbers.N2;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N3;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N4;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertThrows;
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
public class MatrixTest {
|
||||
@Test
|
||||
void testMatrixMultiplication() {
|
||||
var mat1 = MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
var mat1 = Matrix.mat(Nat.N2(), Nat.N2())
|
||||
.fill(2.0, 1.0,
|
||||
0.0, 1.0);
|
||||
var mat2 = MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
var mat2 = Matrix.mat(Nat.N2(), Nat.N2())
|
||||
.fill(3.0, 0.0,
|
||||
0.0, 2.5);
|
||||
|
||||
Matrix<N2, N2> result = mat1.times(mat2);
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isEquals(
|
||||
MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
.fill(6.0, 2.5,
|
||||
0.0, 2.5).getStorage().getDDRM(),
|
||||
result.getStorage().getDDRM()
|
||||
));
|
||||
assertEquals(result, Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 2.5, 0.0, 2.5));
|
||||
|
||||
var mat3 = MatrixUtils.mat(Nat.N2(), Nat.N3())
|
||||
var mat3 = Matrix.mat(Nat.N2(), Nat.N3())
|
||||
.fill(1.0, 3.0, 0.5,
|
||||
2.0, 4.3, 1.2);
|
||||
var mat4 = MatrixUtils.mat(Nat.N3(), Nat.N4())
|
||||
var mat4 = Matrix.mat(Nat.N3(), Nat.N4())
|
||||
.fill(3.0, 1.5, 2.0, 4.5,
|
||||
2.3, 1.0, 1.6, 3.1,
|
||||
5.2, 2.1, 2.0, 1.0);
|
||||
|
||||
Matrix<N2, N4> result2 = mat3.times(mat4);
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isIdentical(
|
||||
MatrixUtils.mat(Nat.N2(), Nat.N4())
|
||||
assertTrue(Matrix.mat(Nat.N2(), Nat.N4())
|
||||
.fill(12.5, 5.55, 7.8, 14.3,
|
||||
22.13, 9.82, 13.28, 23.53).getStorage().getDDRM(),
|
||||
result2.getStorage().getDDRM(),
|
||||
1E-9
|
||||
22.13, 9.82, 13.28, 23.53).isEqual(
|
||||
result2,
|
||||
1E-9
|
||||
));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMatrixVectorMultiplication() {
|
||||
var mat = MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
var mat = Matrix.mat(Nat.N2(), Nat.N2())
|
||||
.fill(1.0, 1.0,
|
||||
0.0, 1.0);
|
||||
|
||||
var vec = MatrixUtils.vec(Nat.N2())
|
||||
.fill(3.0,
|
||||
2.0);
|
||||
var vec = VecBuilder.fill(3.0, 2.0);
|
||||
|
||||
Matrix<N2, N1> result = mat.times(vec);
|
||||
assertTrue(MatrixFeatures_DDRM.isEquals(
|
||||
MatrixUtils.vec(Nat.N2())
|
||||
.fill(5.0,
|
||||
2.0).getStorage().getDDRM(),
|
||||
result.getStorage().getDDRM()
|
||||
));
|
||||
assertEquals(VecBuilder.fill(5.0, 2.0), result);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testTranspose() {
|
||||
Matrix<N3, N1> vec = MatrixUtils.vec(Nat.N3())
|
||||
Matrix<N3, N1> vec = VecBuilder
|
||||
.fill(1.0,
|
||||
2.0,
|
||||
3.0);
|
||||
|
||||
Matrix<N1, N3> transpose = vec.transpose();
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isEquals(
|
||||
MatrixUtils.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0).getStorage()
|
||||
.getDDRM(),
|
||||
transpose.getStorage().getDDRM()
|
||||
));
|
||||
assertEquals(Matrix.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0), transpose);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testSolve() {
|
||||
var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0);
|
||||
var vec1 = VecBuilder.fill(1.0, 2.0);
|
||||
|
||||
var solve1 = mat1.solve(vec1);
|
||||
|
||||
assertEquals(VecBuilder.fill(0.0, 0.5), solve1);
|
||||
|
||||
var mat2 = Matrix.mat(Nat.N3(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
|
||||
var vec2 = VecBuilder.fill(1.0, 2.0, 3.0);
|
||||
|
||||
var solve2 = mat2.solve(vec2);
|
||||
|
||||
assertEquals(VecBuilder.fill(0.0, 0.5), solve2);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInverse() {
|
||||
var mat = MatrixUtils.mat(Nat.N3(), Nat.N3())
|
||||
var mat = Matrix.mat(Nat.N3(), Nat.N3())
|
||||
.fill(1.0, 3.0, 2.0,
|
||||
5.0, 2.0, 1.5,
|
||||
0.0, 1.3, 2.5);
|
||||
|
||||
var inv = mat.inv();
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isIdentical(
|
||||
MatrixUtils.eye(Nat.N3()).getStorage().getDDRM(),
|
||||
mat.times(inv).getStorage().getDDRM(),
|
||||
assertTrue(Matrix.eye(Nat.N3()).isEqual(
|
||||
mat.times(inv),
|
||||
1E-9
|
||||
));
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isIdentical(
|
||||
MatrixUtils.eye(Nat.N3()).getStorage().getDDRM(),
|
||||
inv.times(mat).getStorage().getDDRM(),
|
||||
assertTrue(Matrix.eye(Nat.N3()).isEqual(
|
||||
inv.times(mat),
|
||||
1E-9
|
||||
));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testUninvertableMatrix() {
|
||||
var singularMatrix = MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
var singularMatrix = Matrix.mat(Nat.N2(), Nat.N2())
|
||||
.fill(2.0, 1.0,
|
||||
2.0, 1.0);
|
||||
|
||||
@@ -126,85 +123,52 @@ public class MatrixTest {
|
||||
|
||||
@Test
|
||||
void testMatrixScalarArithmetic() {
|
||||
var mat = MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
var mat = Matrix.mat(Nat.N2(), Nat.N2())
|
||||
.fill(1.0, 2.0,
|
||||
3.0, 4.0);
|
||||
|
||||
assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 4.0, 5.0, 6.0), mat.plus(2.0));
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isEquals(
|
||||
MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
.fill(3.0, 4.0,
|
||||
5.0, 6.0).getStorage().getDDRM(),
|
||||
mat.plus(2.0).getStorage().getDDRM()
|
||||
));
|
||||
assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 2.0, 3.0), mat.minus(1.0));
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isEquals(
|
||||
MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
.fill(0.0, 1.0,
|
||||
2.0, 3.0).getStorage().getDDRM(),
|
||||
mat.minus(1.0).getStorage().getDDRM()
|
||||
));
|
||||
assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 4.0, 6.0, 8.0), mat.times(2.0));
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isEquals(
|
||||
MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
.fill(2.0, 4.0,
|
||||
6.0, 8.0).getStorage().getDDRM(),
|
||||
mat.times(2.0).getStorage().getDDRM()
|
||||
));
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isIdentical(
|
||||
MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
.fill(0.5, 1.0,
|
||||
1.5, 2.0).getStorage().getDDRM(),
|
||||
mat.div(2.0).getStorage().getDDRM(),
|
||||
assertTrue(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.5, 1.0, 1.5, 2.0).isEqual(
|
||||
mat.div(2.0),
|
||||
1E-3
|
||||
));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMatrixMatrixArithmetic() {
|
||||
var mat1 = MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
var mat1 = Matrix.mat(Nat.N2(), Nat.N2())
|
||||
.fill(1.0, 2.0,
|
||||
3.0, 4.0);
|
||||
|
||||
var mat2 = MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
var mat2 = Matrix.mat(Nat.N2(), Nat.N2())
|
||||
.fill(5.0, 6.0,
|
||||
7.0, 8.0);
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isEquals(
|
||||
MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
.fill(-4.0, -4.0,
|
||||
-4.0, -4.0).getStorage().getDDRM(),
|
||||
mat1.minus(mat2).getStorage().getDDRM()
|
||||
));
|
||||
assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(-4.0, -4.0, -4.0, -4.0),
|
||||
mat1.minus(mat2)
|
||||
);
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isEquals(
|
||||
MatrixUtils.mat(Nat.N2(), Nat.N2())
|
||||
.fill(6.0, 8.0,
|
||||
10.0, 12.0).getStorage().getDDRM(),
|
||||
mat1.plus(mat2).getStorage().getDDRM()
|
||||
));
|
||||
assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 8.0, 10.0, 12.0),
|
||||
mat1.plus(mat2)
|
||||
);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMatrixExponential() {
|
||||
SimpleMatrix matrix = MatrixUtils.eye(Nat.N2()).getStorage();
|
||||
var result = SimpleMatrixUtils.expm(matrix);
|
||||
var matrix = Matrix.eye(Nat.N2());
|
||||
var result = matrix.exp();
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isIdentical(
|
||||
result.getDDRM(),
|
||||
new SimpleMatrix(2, 2, true, new double[]{Math.E, 0, 0, Math.E}).getDDRM(),
|
||||
1E-9
|
||||
));
|
||||
assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
|
||||
|
||||
matrix = new SimpleMatrix(2, 2, true, new double[]{1, 2, 3, 4});
|
||||
result = SimpleMatrixUtils.expm(matrix.scale(0.01));
|
||||
matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4);
|
||||
result = matrix.times(0.01).exp();
|
||||
|
||||
assertTrue(MatrixFeatures_DDRM.isIdentical(
|
||||
result.getDDRM(),
|
||||
new SimpleMatrix(2, 2, true, new double[]{1.01035625, 0.02050912,
|
||||
0.03076368, 1.04111993}).getDDRM(),
|
||||
1E-8
|
||||
));
|
||||
assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912,
|
||||
0.03076368, 1.04111993), 1E-8));
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user