added LQG, and switched to using matrices in some places