Added conjugate gradient matrix solver (conflicts should be resolved)

miscmaths.cc

@@ -1647,6 +1647,35 @@ ReturnMatrix read_vest(string p_fname)
   return p_mat;
 }
 
+int conjgrad(ColumnVector x, const Matrix& A, const ColumnVector& b, int maxit)
+{
+  // solves:  A * x = b    (for x)
+  // implementation of algorithm in Golub and Van Loan (3rd ed, page 527)
+  ColumnVector rk1, rk2, pk, apk;
+  double betak, alphak, rk1rk1=0, rk2rk2;
+  int k=0;
+  rk1 = b - A*x;  // a *big* calculation
+  for (int n=1; n<=maxit; n++) {
+    //if (sufficiently accurate) break;
+    k++;
+    if (k==1) {
+      pk = rk1;
+      rk1rk1 = (rk1.t() * rk1).AsScalar();
+    } else {
+      rk2rk2 = rk1rk1;  // from before
+      rk1rk1 = (rk1.t() * rk1).AsScalar();
+      betak = rk1rk1 / rk2rk2;
+      pk = rk1 + betak * pk;  // note RHS pk is p(k-1) in algorithm
+    }  
+    apk = A * pk;  // the *big* calculation in this algorithm
+    alphak = rk1rk1 / (pk*apk).AsScalar();
+    x = x + alphak * pk;  // note LHS is x(k) and RHS is x(k-1) in algorithm
+    rk2 = rk1;  // update prior to the next step
+    rk1 = rk1 - alphak * apk;  // note LHS is r(k) in algorithm
+  }
+  return 0;
+}
+
 vector<float> ColumnVector2vector(const ColumnVector& col)
 {
   vector<float> vec(col.Nrows());

miscmaths.h

@@ -193,6 +193,10 @@ namespace MISCMATHS {
   ReturnMatrix corrcoef(const Matrix& mat, const int norm = 0);
   void symm_orth(Matrix &Mat);
   void powerspectrum(const Matrix &Mat1, Matrix &Result, bool useLog);
+
+  int conjgrad(ColumnVector x, const Matrix& A, const ColumnVector& b, 
+	       int maxit=3);   // solves (for x)   A * x = b 
+
   vector<float> ColumnVector2vector(const ColumnVector& col);
   
   ///////////////////////////////////////////////////////////////////////////