[wpimath] Implement Tsitouras 5th order numerical integrator

[Advay17]

Closes #7123

[calcmogul]

/format

[calcmogul]

You forgot the WPI_UNIGNORE_DEPRECATED macro that pops the diagnostic state.

[calcmogul]

Looks like the arm simulation test hung in C++ and Java on a few platforms. This could indicate a bug in the new integrator that's causing the max error to be really large (so the steps are tiny and the solver takes forever).

[calcmogul]

The issue could be related to numerical precision, a bug in the implementation, or a bug in the paper. You could compare it against https://github.com/SciML/OrdinaryDiffEq.jl/tree/master/lib/OrdinaryDiffEqTsit5/src.

[Advay17]

The float method in the linked repo appeared to be at most 4 decimal places more precise, I can try updating the constants. In addition, the signs for the b2 constants are flipped in my code and the paper when compared to the other repo, so I will try swapping those as well.

[Advay17]

Realized it was just because instead of using b2-b1, the error was supposed to use b2.

[calcmogul]

Realized it was just because instead of using b2-b1, the error was supposed to use b2.

Are you sure? https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method says b1 gives the 5th order solution and b2 gives the 4th order solution, and the error tolerance is the difference between the two outputs.

More specifically, it uses six function evaluations to calculate fourth- and fifth-order accurate solutions. The difference between these solutions is then taken to be the error of the (fourth-order) solution. This error estimate is very convenient for adaptive stepsize integration algorithms.

Section 1 of the Tsitouras paper says the same thing:

The vectors b̂, b define the coefficients of the (p − 1)−th and p−th order approximations respectively.

The local error estimate Eₙ = ‖yₙ − ŷₙ‖ of the (p − 1)−th order Runge-Kutta pair is used for the automatic selection of the step size.

[Advay17]

@calcmogul The way I used was the way the link you sent me does it; the b1 values are all commented out and unused, instead only btilde (which are the b2 values) are used. Perhaps I'm wrong, but I ran the tests locally and everything checked out.

[calcmogul]

So does that mean the paper is wrong?

[Advay17]

I am not sure, I need to look deeper into the other repository to see why b1 was commented out to have a clearer answer. Obviously, the build checks failing are indicating that there is some other issue at play on the c++ side, however swapping to using purely the b2 values did allow the testTsit5TimeVarying function to run quickly for me in Java when it couldn't previously.

[Advay17]

Looks like the java errors are from the unit test not having enough tolerance(just barely), and the c++ errors should theoretically be fixed by updating some of the constants I missed? I'll test some more before making another push.

[KangarooKoala]

Since a₇,ᵢ = bᵢ, it seems like the linked repository uses the a values instead of the b1 values. The linked repository also seems to have some additional behavior that may be contributing to different results- For example, it calculates the error estimate EEst using a calculate_residuals function that uses err_scaled = err / (abstol + max(uprev, u) * reltol). (See also https://docs.sciml.ai/DiffEqDocs/latest/basics/common_solver_opts/).