Square root kalman filter