Added QR decomposition with simple test. Dontmind cpp just copy function into final file