diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java index e8343030a3..6d620014a5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java @@ -7,7 +7,6 @@ package edu.wpi.first.math.controller; 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; @@ -40,13 +39,10 @@ public class ImplicitModelFollower plant, - LinearSystem plantRef, - double dtSeconds) { - this(plant.getA(), plant.getB(), plantRef.getA(), plantRef.getB(), dtSeconds); + LinearSystem plant, LinearSystem plantRef) { + this(plant.getA(), plant.getB(), plantRef.getA(), plantRef.getB()); } /** @@ -56,46 +52,34 @@ public class ImplicitModelFollower A, Matrix B, Matrix Aref, - Matrix Bref, - double dtSeconds) { + Matrix Bref) { m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1)); - // Discretize real dynamics - var discABPair = Discretization.discretizeAB(A, B, dtSeconds); - var discA = discABPair.getFirst(); - var discB = discABPair.getSecond(); - - // Discretize desired dynamics - var discABrefPair = Discretization.discretizeAB(Aref, Bref, dtSeconds); - var discAref = discABrefPair.getFirst(); - var discBref = discABrefPair.getSecond(); - // Find u_imf that makes real model match reference model. // - // x_k+1 = Ax_k + Bu_imf - // z_k+1 = Aref z_k + Bref u_k + // dx/dt = Ax + Bu_imf + // dz/dt = A_ref z + B_ref u // - // Let x_k = z_k. + // Let x = z. // - // x_k+1 = z_k+1 - // Ax_k + Bu_imf = Aref x_k + Bref u_k - // Bu_imf = Aref x_k - Ax_k + Bref u_k - // Bu_imf = (Aref - A)x_k + Bref u_k - // u_imf = B^+ ((Aref - A)x_k + Bref u_k) - // u_imf = -B^+ (A - Aref)x_k + B^+ Bref u_k + // dx/dt = dz/dt + // Ax + Bu_imf = Aref x + B_ref u + // Bu_imf = A_ref x - Ax + B_ref u + // Bu_imf = (A_ref - A)x + B_ref u + // u_imf = B⁻¹((A_ref - A)x + Bref u) + // u_imf = -B⁻¹(A - A_ref)x + B⁻¹B_ref u // The first term makes the open-loop poles that of the reference // system, and the second term makes the input behave like that of the // reference system. - m_A = discB.solve(discA.minus(discAref)).times(-1.0); - m_B = discB.solve(discBref); + m_A = B.solve(A.minus(Aref)).times(-1.0); + m_B = B.solve(Bref); reset(); } diff --git a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h index e8ebcf620b..e093582bfd 100644 --- a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h +++ b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h @@ -4,7 +4,6 @@ #pragma once -#include #include #include "Eigen/Core" @@ -33,58 +32,44 @@ class ImplicitModelFollower { * * @param plant The plant being controlled. * @param plantRef The plant whose dynamics should be followed. - * @param dt Discretization timestep. */ template ImplicitModelFollower(const LinearSystem& plant, - const LinearSystem& plantRef, - units::second_t dt) + const LinearSystem& plantRef) : ImplicitModelFollower(plant.A(), plant.B(), - plantRef.A(), plantRef.B(), dt) {} + plantRef.A(), plantRef.B()) {} /** * Constructs a controller with the given coefficients and plant. * - * @param A Continuous system matrix of the plant being controlled. - * @param B Continuous input matrix of the plant being controlled. - * @param Aref Continuous system matrix whose dynamics should be followed. - * @param Bref Continuous input matrix whose dynamics should be followed. - * @param dt Discretization timestep. + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param Aref Continuous system matrix whose dynamics should be followed. + * @param Bref Continuous input matrix whose dynamics should be followed. */ ImplicitModelFollower(const Eigen::Matrix& A, const Eigen::Matrix& B, const Eigen::Matrix& Aref, - const Eigen::Matrix& Bref, - units::second_t dt) { - // Discretize real dynamics - Eigen::Matrix discA; - Eigen::Matrix discB; - frc::DiscretizeAB(A, B, dt, &discA, &discB); - - // Discretize desired dynamics - Eigen::Matrix discAref; - Eigen::Matrix discBref; - frc::DiscretizeAB(Aref, Bref, dt, &discAref, &discBref); - + const Eigen::Matrix& Bref) { // Find u_imf that makes real model match reference model. // - // x_k+1 = Ax_k + Bu_imf - // z_k+1 = Aref z_k + Bref u_k + // dx/dt = Ax + Bu_imf + // dz/dt = A_ref z + B_ref u // - // Let x_k = z_k. + // Let x = z. // - // x_k+1 = z_k+1 - // Ax_k + Bu_imf = Aref x_k + Bref u_k - // Bu_imf = Aref x_k - Ax_k + Bref u_k - // Bu_imf = (Aref - A)x_k + Bref u_k - // u_imf = B^+ ((Aref - A)x_k + Bref u_k) - // u_imf = -B^+ (A - Aref)x_k + B^+ Bref u_k + // dx/dt = dz/dt + // Ax + Bu_imf = Aref x + B_ref u + // Bu_imf = A_ref x - Ax + B_ref u + // Bu_imf = (A_ref - A)x + B_ref u + // u_imf = B⁻¹((A_ref - A)x + Bref u) + // u_imf = -B⁻¹(A - A_ref)x + B⁻¹B_ref u // The first term makes the open-loop poles that of the reference // system, and the second term makes the input behave like that of the // reference system. - m_A = -discB.householderQr().solve(discA - discAref); - m_B = discB.householderQr().solve(discBref); + m_A = -B.householderQr().solve(A - Aref); + m_B = B.householderQr().solve(Bref); Reset(); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java index e16dcb629d..e3d8aeec59 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java @@ -21,7 +21,7 @@ class ImplicitModelFollowerTest { var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); - var imf = new ImplicitModelFollower(plant, plant, dt); + var imf = new ImplicitModelFollower(plant, plant); var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); @@ -67,7 +67,7 @@ class ImplicitModelFollowerTest { // Linear acceleration is slower, but angular acceleration is the same var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0); - var imf = new ImplicitModelFollower(plant, plantRef, dt); + var imf = new ImplicitModelFollower(plant, plantRef); var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); diff --git a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp index 05a6b7100c..9916370ef9 100644 --- a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp @@ -17,7 +17,7 @@ TEST(ImplicitModelFollowerTest, SameModel) { auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, Kv_t{1.0}, Ka_t{1.0}); - ImplicitModelFollower<2, 2> imf{plant, plant, dt}; + ImplicitModelFollower<2, 2> imf{plant, plant}; Eigen::Vector x{0.0, 0.0}; Eigen::Vector xImf{0.0, 0.0}; @@ -66,7 +66,7 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) { auto plantRef = LinearSystemId::IdentifyDrivetrainSystem( Kv_t{1.0}, Ka_t{2.0}, Kv_t{1.0}, Ka_t{1.0}); - ImplicitModelFollower<2, 2> imf{plant, plantRef, dt}; + ImplicitModelFollower<2, 2> imf{plant, plantRef}; Eigen::Vector x{0.0, 0.0}; Eigen::Vector xImf{0.0, 0.0};