[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.

wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp

@@ -80,7 +80,7 @@ TEST(ElevatorSim, Stability) {
 
   Eigen::Matrix<double, 2, 1> x1 = frc::MakeMatrix<2, 1>(0.0, 0.0);
   for (size_t i = 0; i < 50; i++) {
-    x1 = frc::RKF45(
+    x1 = frc::RKDP(
         [&](Eigen::Matrix<double, 2, 1> x,
             Eigen::Matrix<double, 1, 1> u) -> Eigen::Matrix<double, 2, 1> {
           return system.A() * x + system.B() * u;