cholesky.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00008 
00009 
00010 // Copyright (C) 1991,2,3,4: R B Davies
00011 
00012 #define WANT_MATH
00013 //#define WANT_STREAM
00014 
00015 #include "include.h"
00016 
00017 #include "newmat.h"
00018 #include "newmatrm.h"
00019 
00020 #ifdef use_namespace
00021 namespace NEWMAT {
00022 #endif
00023 
00024 #ifdef DO_REPORT
00025 #define REPORT { static ExeCounter ExeCount(__LINE__,14); ++ExeCount; }
00026 #else
00027 #define REPORT {}
00028 #endif
00029 
00030 /********* Cholesky decomposition of a positive definite matrix *************/
00031 
00032 // Suppose S is symmetrix and positive definite. Then there exists a unique
00033 // lower triangular matrix L such that L L.t() = S;
00034 
00035 
00036 ReturnMatrix Cholesky(const SymmetricMatrix& S)
00037 {
00038    REPORT
00039    Tracer trace("Cholesky");
00040    int nr = S.Nrows();
00041    LowerTriangularMatrix T(nr);
00042    Real* s = S.Store(); Real* t = T.Store(); Real* ti = t;
00043    for (int i=0; i<nr; i++)
00044    {
00045       Real* tj = t; Real sum; int k;
00046       for (int j=0; j<i; j++)
00047       {
00048          Real* tk = ti; sum = 0.0; k = j;
00049          while (k--) { sum += *tj++ * *tk++; }
00050          *tk = (*s++ - sum) / *tj++;
00051       }
00052       sum = 0.0; k = i;
00053       while (k--) { sum += square(*ti++); }
00054       Real d = *s++ - sum;
00055       if (d<=0.0)  Throw(NPDException(S));
00056       *ti++ = sqrt(d);
00057    }
00058    T.release(); return T.for_return();
00059 }
00060 
00061 ReturnMatrix Cholesky(const SymmetricBandMatrix& S)
00062 {
00063    REPORT
00064    Tracer trace("Band-Cholesky");
00065    int nr = S.Nrows(); int m = S.lower_val;
00066    LowerBandMatrix T(nr,m);
00067    Real* s = S.Store(); Real* t = T.Store(); Real* ti = t;
00068 
00069    for (int i=0; i<nr; i++)
00070    {
00071       Real* tj = t; Real sum; int l;
00072       if (i<m) { REPORT l = m-i; s += l; ti += l; l = i; }
00073       else { REPORT t += (m+1); l = m; }
00074 
00075       for (int j=0; j<l; j++)
00076       {
00077          Real* tk = ti; sum = 0.0; int k = j; tj += (m-j);
00078          while (k--) { sum += *tj++ * *tk++; }
00079          *tk = (*s++ - sum) / *tj++;
00080       }
00081       sum = 0.0;
00082       while (l--) { sum += square(*ti++); }
00083       Real d = *s++ - sum;
00084       if (d<=0.0)  Throw(NPDException(S));
00085       *ti++ = sqrt(d);
00086    }
00087 
00088    T.release(); return T.for_return();
00089 }
00090 
00091 
00092 
00093 
00094 // Contributed by Nick Bennett of Schlumberger-Doll Research; modified by RBD
00095 
00096 // The enclosed routines can be used to update the Cholesky decomposition of
00097 // a positive definite symmetric matrix.  A good reference for this routines
00098 // can be found in
00099 // LINPACK User's Guide, Chapter 10, Dongarra et. al., SIAM, Philadelphia, 1979
00100 
00101 // produces the Cholesky decomposition of A + x.t() * x where A = chol.t() * chol
00102 void update_Cholesky(UpperTriangularMatrix &chol, RowVector x)
00103 {
00104    int nc = chol.Nrows();
00105    ColumnVector cGivens(nc); cGivens = 0.0;
00106    ColumnVector sGivens(nc); sGivens = 0.0;
00107         
00108    for(int j = 1; j <= nc; ++j) // process the jth column of chol
00109    {
00110       // apply the previous Givens rotations k = 1,...,j-1 to column j
00111       for(int k = 1; k < j; ++k)
00112          GivensRotation(cGivens(k), sGivens(k), chol(k,j), x(j));
00113 
00114       // determine the jth Given's rotation
00115       pythag(chol(j,j), x(j), cGivens(j), sGivens(j));
00116 
00117       // apply the jth Given's rotation
00118       {
00119          Real tmp0 = cGivens(j) * chol(j,j) + sGivens(j) * x(j);
00120          chol(j,j) = tmp0; x(j) = 0.0;
00121       }
00122 
00123    }
00124 
00125 }
00126 
00127 
00128 // produces the Cholesky decomposition of A - x.t() * x where A = chol.t() * chol
00129 void downdate_Cholesky(UpperTriangularMatrix &chol, RowVector x)
00130 {
00131    int nRC = chol.Nrows();
00132         
00133    // solve R^T a = x
00134    LowerTriangularMatrix L = chol.t();
00135    ColumnVector a(nRC); a = 0.0;
00136    int i, j;
00137         
00138    for (i = 1; i <= nRC; ++i)
00139    {
00140       // accumulate subtr sum
00141       Real subtrsum = 0.0;
00142       for(int k = 1; k < i; ++k) subtrsum += a(k) * L(i,k);
00143 
00144       a(i) = (x(i) - subtrsum) / L(i,i);
00145    }
00146 
00147    // test that l2 norm of a is < 1
00148    Real squareNormA = a.SumSquare();
00149    if (squareNormA >= 1.0)
00150       Throw(ProgramException("downdate_Cholesky() fails", chol));
00151 
00152    Real alpha = sqrt(1.0 - squareNormA);
00153 
00154    // compute and apply Givens rotations to the vector a
00155    ColumnVector cGivens(nRC);  cGivens = 0.0;
00156    ColumnVector sGivens(nRC);  sGivens = 0.0;
00157    for(i = nRC; i >= 1; i--)
00158       alpha = pythag(alpha, a(i), cGivens(i), sGivens(i));
00159 
00160    // apply Givens rotations to the jth column of chol
00161    ColumnVector xtilde(nRC); xtilde = 0.0;
00162    for(j = nRC; j >= 1; j--)
00163    {
00164       // only the first j rotations have an affect on chol,0
00165       for(int k = j; k >= 1; k--)
00166          GivensRotation(cGivens(k), -sGivens(k), chol(k,j), xtilde(j));
00167    }
00168 }
00169 
00170 
00171 
00172 // produces the Cholesky decomposition of EAE where A = chol.t() * chol
00173 // and E produces a RIGHT circular shift of the rows and columns from
00174 // 1,...,k-1,k,k+1,...l,l+1,...,p to
00175 // 1,...,k-1,l,k,k+1,...l-1,l+1,...p
00176 void right_circular_update_Cholesky(UpperTriangularMatrix &chol, int k, int l)
00177 {
00178    int nRC = chol.Nrows();
00179    int i, j;
00180         
00181    // I. compute shift of column l to the kth position
00182    Matrix cholCopy = chol;
00183    // a. grab column l
00184    ColumnVector columnL = cholCopy.Column(l);
00185    // b. shift columns k,...l-1 to the RIGHT
00186    for(j = l-1; j >= k; --j)
00187       cholCopy.Column(j+1) = cholCopy.Column(j);
00188    // c. copy the top k-1 elements of columnL into the kth column of cholCopy
00189    cholCopy.Column(k) = 0.0;
00190    for(i = 1; i < k; ++i) cholCopy(i,k) = columnL(i);
00191 
00192     // II. determine the l-k Given's rotations
00193    int nGivens = l-k;
00194    ColumnVector cGivens(nGivens); cGivens = 0.0;
00195    ColumnVector sGivens(nGivens); sGivens = 0.0;
00196    for(i = l; i > k; i--)
00197    {
00198       int givensIndex = l-i+1;
00199       columnL(i-1) = pythag(columnL(i-1), columnL(i),
00200          cGivens(givensIndex), sGivens(givensIndex));
00201       columnL(i) = 0.0;
00202    }
00203    // the kth entry of columnL is the new diagonal element in column k of cholCopy
00204    cholCopy(k,k) = columnL(k);
00205         
00206    // III. apply these Given's rotations to subsequent columns
00207    // for columns k+1,...,l-1 we only need to apply the last nGivens-(j-k) rotations
00208    for(j = k+1; j <= nRC; ++j)
00209    {
00210       ColumnVector columnJ = cholCopy.Column(j);
00211       int imin = nGivens - (j-k) + 1; if (imin < 1) imin = 1;
00212       for(int gIndex = imin; gIndex <= nGivens; ++gIndex)
00213       {
00214          // apply gIndex Given's rotation
00215          int topRowIndex = k + nGivens - gIndex;
00216          GivensRotationR(cGivens(gIndex), sGivens(gIndex),
00217             columnJ(topRowIndex), columnJ(topRowIndex+1));
00218       }
00219       cholCopy.Column(j) = columnJ;
00220    }
00221 
00222    chol << cholCopy;
00223 }
00224 
00225 
00226 
00227 // produces the Cholesky decomposition of EAE where A = chol.t() * chol
00228 // and E produces a LEFT circular shift of the rows and columns from
00229 // 1,...,k-1,k,k+1,...l,l+1,...,p to
00230 // 1,...,k-1,k+1,...l,k,l+1,...,p to
00231 void left_circular_update_Cholesky(UpperTriangularMatrix &chol, int k, int l)
00232 {
00233    int nRC = chol.Nrows();
00234    int i, j;
00235 
00236    // I. compute shift of column k to the lth position
00237    Matrix cholCopy = chol;
00238    // a. grab column k
00239    ColumnVector columnK = cholCopy.Column(k);
00240    // b. shift columns k+1,...l to the LEFT
00241    for(j = k+1; j <= l; ++j)
00242       cholCopy.Column(j-1) = cholCopy.Column(j);
00243    // c. copy the elements of columnK into the lth column of cholCopy
00244    cholCopy.Column(l) = 0.0;
00245    for(i = 1; i <= k; ++i)
00246       cholCopy(i,l) = columnK(i);
00247 
00248    // II. apply and compute Given's rotations
00249    int nGivens = l-k;
00250    ColumnVector cGivens(nGivens); cGivens = 0.0;
00251    ColumnVector sGivens(nGivens); sGivens = 0.0;
00252    for(j = k; j <= nRC; ++j)
00253    {
00254       ColumnVector columnJ = cholCopy.Column(j);
00255 
00256       // apply the previous Givens rotations to columnJ
00257       int imax = j - k; if (imax > nGivens) imax = nGivens;
00258       for(int i = 1; i <= imax; ++i)
00259       {
00260          int gIndex = i;
00261          int topRowIndex = k + i - 1;
00262          GivensRotationR(cGivens(gIndex), sGivens(gIndex),
00263             columnJ(topRowIndex), columnJ(topRowIndex+1));
00264       }
00265 
00266       // compute a new Given's rotation when j < l
00267       if(j < l)
00268       {
00269          int gIndex = j-k+1;
00270          columnJ(j) = pythag(columnJ(j), columnJ(j+1), cGivens(gIndex),
00271             sGivens(gIndex));
00272          columnJ(j+1) = 0.0;
00273       }
00274 
00275       cholCopy.Column(j) = columnJ;
00276    }
00277 
00278    chol << cholCopy;
00279         
00280 }
00281 
00282 
00283 
00284 
00285 #ifdef use_namespace
00286 }
00287 #endif
00288 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Tue May 28 2013 14:52:53