diff --git a/miscmaths.cc b/miscmaths.cc index a89adbfb2629111c7e674fe974805910e87b761d..c047a3335876cc836c505b062da164758a4d85bb 100644 --- a/miscmaths.cc +++ b/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()); diff --git a/miscmaths.h b/miscmaths.h index ea9762894407bfea3db4fd48a7324019ae5d3f52..430e8ea8dac71a9baa2cf8a71d194c4be76129b2 100644 --- a/miscmaths.h +++ b/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); ///////////////////////////////////////////////////////////////////////////