[wpimath] Implement Dormand-Prince integration method (#3476)

Also refactored RKF45 implementation to match the new style, which is
easier to read.

The tests were switched from RKF45 to RKDP since it's more accurate.

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java

@@ -221,7 +221,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
   protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
     // Calculate updated x-hat from Runge-Kutta.
     var updatedXhat =
-        NumericalIntegration.rkf45(
+        NumericalIntegration.rkdp(
             (x, u_) ->
                 (m_plant.getA().times(x))
                     .plus(m_plant.getB().times(u_))

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java

@@ -282,7 +282,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
     // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
     // cos(theta)]]
     Matrix<N2, N1> updatedXhat =
-        NumericalIntegration.rkf45(
+        NumericalIntegration.rkdp(
             (Matrix<N2, N1> x, Matrix<N1, N1> u_) -> {
               Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(u_));
               if (m_simulateGravity) {