jrvanwhy/traj_opt

Implement arbitrary-order integration setup

Closed this issue · 0 comments

Using Taylor series, an implicit integration formula of arbitrary order may be derived:

Let dy = f(y, u, t), where u is an affine function of time (over one integration interval)

Suppose t2 = t1 + dt

Expanding the Taylor series for y about y(1) at y(t2), we get:
y(t2) = y(t1) + dt_dy(t1) + dt^2_ddy(t1)/2 + dt^3*dddy(t1)/6 + ...

Expanding the Taylor series for y about y(t2) at y(t1), we get:
y(t1) = y(t2) - dt_dy(t2) + dt^2_ddy(t2)/2 - dt^3*dddy(t2)/6 + ...

Subtracting the last equation from the previous equation we get:
y(t2) - y(t1) = y(t1) - y(t2) + dt_(dy(t1) + dy(t2)) + dt^2_(ddy(t1)-ddy(t2))/2 + dt^3*(dddy(t1) + dddy(t2))/6 + ...

Adding y(t2) - y(t1) to each side gives:
2_(y(t2) - y(t1)) = dt_(dy(t2) + dy(t2)) + dt^2_(ddy(t1) - ddy(t2))/2 + dt^3_(dddy(t1) + dddy(t2))/6 + ...

Note that if we only carry this out to one term, it is simply the trapezoidal integration rule. If you carry out more, you can exactly solve affine systems.

The configuration parameter for order could specify a range; it will create a solver of at least the minimum order, but if it can do so and produce a linear constraint, it should increase the order up to the maximum (maximizing accuracy w/out sacrificing linearity of that would make it nonlinear).