Fixed various bugs

Assuming that your linear algebra is right is a bad idea.