Added square root Kalman filter implementation.

Implementation and simple test of a cholesky based square
root Kalman filter. Not tested in any depth yet.