int integrate_rk4(Vector< Type, M >(*f)(Type, const Matrix< Type, M, 1 > &x, const Matrix< Type, N, 1 > &u), const Matrix< Type, M, 1 > &y0, const Matrix< Type, N, 1 > &u, Type t0, Type tf, Type h0, Matrix< Type, M, 1 > &y1)
bool isEqual(const Matrix< Type, M, N > &x, const Matrix< Type, M, N > &y, const Type eps=1e-4f)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)