Matlab Simulation for equations of motion