Added test to check equality of rhs of discretized system and continous system.
Broke up the simulate function.