The Dynamics of the robot are taken from:
Brian Armstrong, Oussama Khatib & Joel Burdick , “The Explicit Dynamic Model and Inertial Parameters of the PUMA 560 Arm” , Stanford University, Artificial Intelligence Laboratory, IEEE 1986
Although Uncertainty is not added, adding uncertainty is easy (check the paper).
I couldn't find such a program in the internet, so I learned using the ODE function and I have written this program. It's opened for your advises.
I still have a small problem concerning the derivative and the integral error, perhaps I have to multiply and dicide by the time division.
BSD licence update
The error function is changed