added newton sqrt alg

include/sta/math/linalg/linalg.hpp

@@ -30,6 +30,8 @@ matrix inv_schur_dec(matrix);
 matrix _inv_char_poly_3x3(matrix);
 matrix _inv_char_poly_2x2(matrix);
 
+matrix sqrtm(matrix);
+
 } // namespace linalg
 } // namespace math
 } // namespace sta

src/linalg/linalg.cpp

@@ -355,6 +355,27 @@ matrix _inv_char_poly_2x2(matrix m) {
 
 }
 
+matrix sqrtm(matrix m) {
+
+    //to-do: matrix positive semidefinite?
+    float threshold = 0.0001;
+    int count = 15;
+    float diff = 100;
+
+    sta::math::matrix x0 = m;
+    sta::math::matrix x1;
+
+    do {
+        x1 = (x0 + m*sta::math::linalg::inv(x0))*0.5;
+        diff = sta::math::linalg::norm(x1 - x0);
+        count--;
+        x0 = x1;
+    }
+    while(diff > threshold && count > 0);
+
+    return x1;
+}
+
 
 } // namespace linalg