renamed stuff. ready for sqp
have sqp coded up. bug in gmm cost causing it to be nan