Fix bug in swerve Runge Kutta implementation
MPC was predicting much slower motion than actually was happening by a
factor of 1/M... Make things match the wikipedia article, and it looks
a lot better.
Change-Id: I20534f971f6ca199bfc5883c28dc908277587d48
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/swerve/casadi_velocity_mpc.py b/frc971/control_loops/swerve/casadi_velocity_mpc.py
index 2da31d5..09fc5bd 100644
--- a/frc971/control_loops/swerve/casadi_velocity_mpc.py
+++ b/frc971/control_loops/swerve/casadi_velocity_mpc.py
@@ -123,7 +123,7 @@
k2 = self.velocity_fdot(X + DT / 2 * k1, U)
k3 = self.velocity_fdot(X + DT / 2 * k2, U)
k4 = self.velocity_fdot(X + DT * k3, U)
- X = X + DT / 6 / M * (k1 + 2 * k2 + 2 * k3 + k4)
+ X = X + DT / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
return casadi.Function("next_X", [X0, U], [X])