Small bug fix from Rien for non-equidistant grids.

git-svn-id: svn://svn.code.sf.net/p/acado/code/trunk@173 f4037fd0-e48f-4cc9-9f8e-e12ed3b76bf3