Added a simple Gram-Schmidt orthogonalization function