Refactor for performance, add minimal tests.

Refactor: more use of matrices, provide gradient for KL minimization.