Shallow water: misc
Shallow water: refactoring
Shallow water: refactored step expansion
Shallow water: split up calculation of step expansion
Shallow water: reimplemented QP generation (debug)
Shallow water: Fixed gauss-newton, now consistent with full-space
Merge branch 'develop' of github.com:jaeandersson/casadi into develop