Changeset 1064 for library/bdm/math
- Timestamp:
- 06/09/10 14:00:40 (14 years ago)
- Location:
- library/bdm/math
- Files:
-
- 6 modified
-
chmat.cpp (modified) (1 diff)
-
chmat.h (modified) (2 diffs)
-
functions.h (modified) (3 diffs)
-
psi.cpp (modified) (1 diff)
-
square_mat.cpp (modified) (5 diffs)
-
square_mat.h (modified) (4 diffs)
Legend:
- Unmodified
- Added
- Removed
-
library/bdm/math/chmat.cpp
r811 r1064 7 7 8 8 void chmat::add ( const chmat &A2, double w ) { 9 10 11 12 13 14 15 9 bdm_assert_debug ( dim == A2.dim, "Matrices of unequal dimension" ); 10 mat pre = concat_vertical ( Ch, sqrt ( w ) * A2.Ch ); 11 mat post = zeros ( pre.rows(), pre.cols() ); 12 if ( !qr ( pre, post ) ) { 13 bdm_warning ( "Unstable QR in chmat add" ); 14 } 15 Ch = post ( 0, dim - 1, 0, dim - 1 ); 16 16 }; 17 17 18 18 void chmat::opupdt ( const vec &v, double w ) { 19 19 //TODO see cholupdt in lhotse 20 21 22 23 24 25 26 20 mat Z; 21 mat R; 22 mat V ( 1, v.length() ); 23 V.set_row ( 0, v*sqrt ( w ) ); 24 Z = concat_vertical ( Ch, V ); 25 qr ( Z, R ); 26 Ch = R ( 0, Ch.rows() - 1, 0, Ch.cols() - 1 ); 27 27 } 28 28 29 29 mat chmat::to_mat() const { 30 31 30 mat F = Ch.T() * Ch; 31 return F; 32 32 } 33 33 34 34 void chmat::mult_sym ( const mat &C ) { 35 36 37 38 35 bdm_assert_debug ( C.rows() == dim, "Wrong dimension of U" ); 36 if ( !qr ( Ch*C.T(), Ch ) ) { 37 bdm_warning ( "QR unstable in chmat mult_sym" ); 38 } 39 39 } 40 40 41 41 void chmat::mult_sym ( const mat &C , chmat &U ) const { 42 43 44 45 42 bdm_assert_debug ( C.rows() == U.dim, "Wrong dimension of U" ); 43 if ( !qr ( Ch*C.T(), U.Ch ) ) { 44 bdm_warning ( "QR unstable in chmat mult_sym" ); 45 } 46 46 } 47 47 48 48 void chmat::mult_sym_t ( const mat &C ) { 49 50 51 52 49 bdm_assert_debug ( C.cols() == dim, "Wrong dimension of U" ); 50 if ( !qr ( Ch*C, Ch ) ) { 51 bdm_warning ( "QR unstable in chmat mult_sym" ); 52 } 53 53 } 54 54 55 55 void chmat::mult_sym_t ( const mat &C, chmat &U ) const { 56 57 58 59 56 bdm_assert_debug ( C.cols() == U.dim, "Wrong dimension of U" ); 57 if ( !qr ( Ch*C, U.Ch ) ) { 58 bdm_warning ( "QR unstable in chmat mult_sym" ); 59 } 60 60 } 61 61 62 62 double chmat::logdet() const { 63 64 65 66 67 68 69 63 double ldet = 0.0; 64 int i; 65 //sum of logs of (possibly negative!) diagonal entries 66 for ( i = 0; i < Ch.rows(); i++ ) { 67 ldet += log ( std::fabs ( Ch ( i, i ) ) ); 68 } 69 return 2*ldet; //compensate for Ch being sqrt() 70 70 } 71 71 72 72 //TODO can be done more efficiently using BLAS, see triangular matrices 73 73 vec chmat::sqrt_mult ( const vec &v ) const { 74 75 76 74 vec pom; 75 pom = Ch.T() * v; 76 return pom; 77 77 } 78 78 79 79 double chmat::qform ( const vec &v ) const { 80 81 82 80 vec pom; 81 pom = Ch * v; 82 return pom*pom; 83 83 } 84 84 85 85 double chmat::invqform ( const vec &v ) const { 86 87 88 86 vec pom ( v.length() ); 87 forward_substitution ( Ch.T(), v, pom ); 88 return pom*pom; 89 89 } 90 90 91 91 void chmat::clear() { 92 92 Ch.clear(); 93 93 } 94 94 -
library/bdm/math/chmat.h
r772 r1064 27 27 protected: 28 28 //! Upper triangle of the cholesky matrix 29 29 mat Ch; 30 30 public: 31 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 32 void opupdt ( const vec &v, double w ); 33 mat to_mat() const; 34 void mult_sym ( const mat &C ); 35 //! mult_sym with return value in parameter \c U 36 void mult_sym ( const mat &C , chmat &U ) const; 37 void mult_sym_t ( const mat &C ); 38 //! mult_sym with return value in parameter \c U 39 void mult_sym_t ( const mat &C, chmat &U ) const; 40 double logdet() const; 41 vec sqrt_mult ( const vec &v ) const; 42 double qform ( const vec &v ) const; 43 double invqform ( const vec &v ) const; 44 void clear(); 45 //! add another chmat \c A2 with weight \c w. 46 void add ( const chmat &A2, double w = 1.0 ); 47 //!Inversion in the same form, i.e. cholesky 48 void inv ( chmat &Inv ) const { 49 ( Inv.Ch ) = itpp::inv ( Ch ).T(); 50 Inv.dim = dim; 51 }; //Fixme: can be more efficient 52 ; 53 53 // void inv ( mat &Inv ); 54 54 55 56 57 58 59 60 61 62 63 64 65 66 55 //! Destructor for future use; 56 virtual ~chmat() {}; 57 //! 58 chmat ( ) : sqmat (), Ch ( ) {}; 59 //! Default constructor 60 chmat ( const int dim0 ) : sqmat ( dim0 ), Ch ( dim0, dim0 ) {}; 61 //! Default constructor 62 chmat ( const vec &v ) : sqmat ( v.length() ), Ch ( diag ( sqrt ( v ) ) ) {}; 63 //! Copy constructor 64 chmat ( const chmat &Ch0 ) : sqmat ( Ch0.dim ), Ch ( Ch0.dim, Ch0.dim ) { 65 Ch = Ch0.Ch; 66 } 67 67 68 69 70 71 72 73 68 //! Default constructor (m3k:cholform) 69 chmat ( const mat &M ) : sqmat ( M.rows() ), Ch ( M.rows(), M.cols() ) { 70 mat Q; 71 bdm_assert_debug ( M.rows() == M.cols(), "chmat:: input matrix must be square!" ); 72 Ch = chol ( M ); 73 } 74 74 75 76 77 78 79 80 81 82 83 84 75 /*! 76 Some templates require this constructor to compile, but 77 it shouldn't actually be called. 78 \note may be numerically unstable 79 */ 80 chmat ( const chmat &M, const ivec &perm ): sqmat(perm.length()) { 81 mat complete = M.to_mat (); 82 mat subs = complete.get_cols(perm); 83 Ch = chol(subs.get_rows(perm)); 84 } 85 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 86 //! Access function 87 mat & _Ch() { 88 return Ch; 89 } 90 //! Access function 91 const mat & _Ch() const { 92 return Ch; 93 } 94 //! Access functions 95 void setD ( const vec &nD ) { 96 Ch = diag ( sqrt ( nD ) ); 97 } 98 //! Access functions 99 void setCh ( const vec &chQ ) { 100 bdm_assert_debug ( chQ.length() == dim * dim, "wrong length" ); 101 copy_vector ( dim*dim, chQ._data(), Ch._data() ); 102 } 103 //! access function 104 void setCh ( const chmat &Ch0 ) { 105 Ch = Ch0._Ch(); 106 dim = Ch0.rows(); 107 } 108 //! access function 109 void setCh ( const mat &Ch0 ) { 110 //TODO check if Ch0 is OK!!! 111 Ch = Ch0; 112 dim = Ch0.rows(); 113 } 114 114 115 116 117 118 119 120 115 //! Access functions 116 void setD ( const vec &nD, int i ) { 117 for ( int j = i; j < nD.length(); j++ ) { 118 Ch ( j, j ) = sqrt ( nD ( j - i ) ); //Fixme can be more general 119 } 120 } 121 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 122 //! Operator 123 chmat& operator += ( const chmat &A2 ); 124 //! Operator 125 chmat& operator -= ( const chmat &A2 ); 126 //! Operator 127 chmat& operator * ( const double &d ) { 128 Ch*sqrt ( d ); 129 return *this; 130 }; 131 //! Operator 132 chmat& operator = ( const chmat &A2 ) { 133 Ch = A2.Ch; 134 dim = A2.dim; 135 return *this; 136 } 137 //! Operator 138 chmat& operator *= ( double x ) { 139 Ch *= sqrt ( x ); 140 return *this; 141 }; 142 142 }; 143 143 … … 146 146 //!mapping of add operation to operators 147 147 inline chmat& chmat::operator += ( const chmat & A2 ) { 148 149 148 this->add ( A2 ); 149 return *this; 150 150 } 151 151 //!mapping of negative add operation to operators 152 152 inline chmat& chmat::operator -= ( const chmat & A2 ) { 153 154 153 this->add ( A2, -1.0 ); 154 return *this; 155 155 } 156 156 -
library/bdm/math/functions.h
r800 r1064 20 20 //! class representing function \f$f(x) = a\f$, here \c rv is empty 21 21 class constfn : public fnc { 22 23 22 //! value of the function 23 vec val; 24 24 25 25 public: 26 27 28 29 30 31 32 33 34 35 26 //vec eval() {return val;}; 27 //! inherited 28 vec eval ( const vec &cond ) { 29 return val; 30 }; 31 //!Default constructor 32 constfn ( const vec &val0 ) : fnc(), val ( val0 ) { 33 dimy = val.length(); 34 dimc = 0; 35 }; 36 36 }; 37 37 38 38 //! Class representing function \f$f(x) = Ax+B\f$ 39 39 class linfn: public fnc { 40 41 42 43 44 45 40 //! Identification of \f$x\f$ 41 RV rv; 42 //! Matrix A 43 mat A; 44 //! vector B 45 vec B; 46 46 public : 47 48 49 50 47 vec eval ( const vec &cond ) { 48 bdm_assert_debug ( cond.length() == A.cols(), "linfn::eval Wrong cond." ); 49 return A * cond + B; 50 } 51 51 52 52 // linfn evalsome ( ivec &rvind ); 53 54 55 56 57 58 59 60 void from_setting(const Setting &set){61 62 63 64 void validate(){65 66 67 68 53 //!default constructor 54 linfn ( ) : fnc(), A ( ), B () { }; 55 //! Set values of \c A and \c B 56 void set_parameters ( const mat &A0 , const vec &B0 ) { 57 A = A0; 58 B = B0; 59 }; 60 void from_setting(const Setting &set) { 61 UI::get(A,set,"A",UI::compulsory); 62 UI::get(B,set,"B",UI::compulsory); 63 } 64 void validate() { 65 dimy = A.rows(); 66 dimc = A.cols(); 67 } 68 69 69 }; 70 70 UIREGISTER(linfn); … … 82 82 class diffbifn: public fnc { 83 83 protected: 84 85 86 87 88 89 90 91 84 //! Indentifier of the first rv. 85 RV rvx; 86 //! Indentifier of the second rv. 87 RV rvu; 88 //! cache for rvx.count() 89 int dimx; 90 //! cache for rvu.count() 91 int dimu; 92 92 public: 93 94 95 96 97 98 99 100 93 //! Evaluates \f$f(x0,u0)\f$ (VS: Do we really need common eval? ) 94 vec eval ( const vec &cond ) { 95 bdm_assert_debug ( cond.length() == ( dimx + dimu ), "linfn::eval Wrong cond." ); 96 if ( dimu > 0 ) { 97 return eval ( cond ( 0, dimx - 1 ), cond ( dimx, dimx + dimu - 1 ) );//-1 = end (in matlab) 98 } else { 99 return eval ( cond ( 0, dimx - 1 ), vec ( 0 ) );//-1 = end (in matlab) 100 } 101 101 102 102 } 103 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 104 //! Evaluates \f$f(x0,u0)\f$ 105 virtual vec eval ( const vec &x0, const vec &u0 ) { 106 return zeros ( dimy ); 107 }; 108 //! Evaluates \f$A=\frac{d}{dx}f(x,u)|_{x0,u0}\f$ and writes result into \c A . @param full denotes that even unchanged entries are to be rewritten. When, false only the changed elements are computed. @param x0 numeric value of \f$x\f$, @param u0 numeric value of \f$u\f$ @param A a place where the result will be stored. 109 virtual void dfdx_cond ( const vec &x0, const vec &u0, mat &A , bool full = true ) {}; 110 //! Evaluates \f$A=\frac{d}{du}f(x,u)|_{x0,u0}\f$ and writes result into \c A . @param full denotes that even unchanged entries are to be rewritten. When, false only the changed elements are computed. @param x0 numeric value of \f$x\f$, @param u0 numeric value of \f$u\f$ @param A a place where the result will be stored. 111 virtual void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full = true ) {}; 112 //!Default constructor (dimy is not set!) 113 diffbifn () : fnc() {}; 114 //! access function 115 int _dimx() const { 116 return dimx; 117 } 118 //! access function 119 int _dimu() const { 120 return dimu; 121 } 122 122 }; 123 123 … … 125 125 //TODO can be generalized into multilinear form! 126 126 class bilinfn: public diffbifn { 127 128 127 mat A; 128 mat B; 129 129 public : 130 131 130 //!\name Constructors 131 //!@{ 132 132 133 133 bilinfn () : diffbifn (), A(), B() { } 134 134 135 136 137 135 bilinfn ( const mat &A0, const mat &B0 ) { 136 set_parameters ( A0, B0 ); 137 } 138 138 139 140 141 142 143 144 145 146 147 148 139 //! Alternative initialization 140 void set_parameters ( const mat &A0, const mat &B0 ) { 141 bdm_assert ( A0.rows() == B0.rows(), "bilinfn matrices must have the same number of rows" ); 142 A = A0; 143 B = B0; 144 dimy = A.rows(); 145 dimx = A.cols(); 146 dimu = B.cols(); 147 } 148 //!@} 149 149 150 151 152 153 154 155 156 150 //!\name Mathematical operations 151 //!@{ 152 inline vec eval ( const vec &x0, const vec &u0 ) { 153 bdm_assert_debug ( x0.length() == dimx, "bilinfn::eval Wrong xcond." ); 154 bdm_assert_debug ( u0.length() == dimu, "bilinfn::eval Wrong ucond." ); 155 return A*x0 + B*u0; 156 } 157 157 158 159 160 161 158 void dfdx_cond ( const vec &x0, const vec &u0, mat &F, bool full ) { 159 bdm_assert_debug ( ( F.cols() == A.cols() ) && ( F.rows() == A.rows() ), "Allocated F is not compatible." ); 160 if ( full ) F = A; //else : nothing has changed no need to regenerate 161 } 162 162 163 164 165 166 167 163 void dfdu_cond ( const vec &x0, const vec &u0, mat &F, bool full = true ) { 164 bdm_assert_debug ( ( F.cols() == B.cols() ) && ( F.rows() == B.rows() ), "Allocated F is not compatible." ); 165 if ( full ) F = B; //else : nothing has changed no need to regenerate 166 } 167 //!@} 168 168 }; 169 169 -
library/bdm/math/psi.cpp
r706 r1064 13 13 14 14 double psi ( double x ) { 15 16 17 18 19 20 21 22 23 24 25 26 15 double s, ps, xa, x2; 16 int n, k; 17 static double a[] = { 18 -0.8333333333333e-01, 19 0.83333333333333333e-02, 20 -0.39682539682539683e-02, 21 0.41666666666666667e-02, 22 -0.75757575757575758e-02, 23 0.21092796092796093e-01, 24 -0.83333333333333333e-01, 25 0.4432598039215686 26 }; 27 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 28 xa = fabs ( x ); 29 s = 0.0; 30 if ( ( x == ( int ) x ) && ( x <= 0.0 ) ) { 31 ps = 1e308; 32 return ps; 33 } 34 if ( xa == ( int ) xa ) { 35 n = xa; 36 for ( k = 1; k < n; k++ ) { 37 s += 1.0 / k; 38 } 39 ps = s - el; 40 } else if ( ( xa + 0.5 ) == ( ( int ) ( xa + 0.5 ) ) ) { 41 n = xa - 0.5; 42 for ( k = 1; k <= n; k++ ) { 43 s += 1.0 / ( 2.0 * k - 1.0 ); 44 } 45 ps = 2.0 * s - el - 1.386294361119891; 46 } else { 47 if ( xa < 10.0 ) { 48 n = 10 - ( int ) xa; 49 for ( k = 0; k < n; k++ ) { 50 s += 1.0 / ( xa + k ); 51 } 52 xa += n; 53 } 54 x2 = 1.0 / ( xa * xa ); 55 ps = log ( xa ) - 0.5 / xa + x2 * ( ( ( ( ( ( ( a[7] * x2 + a[6] ) * x2 + a[5] ) * x2 + 56 a[4] ) * x2 + a[3] ) * x2 + a[2] ) * x2 + a[1] ) * x2 + a[0] ); 57 ps -= s; 58 } 59 if ( x < 0.0 ) 60 ps = ps - M_PI * cos ( M_PI * x ) / sin ( M_PI * x ) - 1.0 / x; 61 return ps; 62 62 } -
library/bdm/math/square_mat.cpp
r737 r1064 9 9 10 10 void fsqmat::opupdt ( const vec &v, double w ) { 11 11 M += outer_product ( v, v * w ); 12 12 }; 13 13 mat fsqmat::to_mat() const { 14 14 return M; 15 15 }; 16 16 void fsqmat::mult_sym ( const mat &C ) { 17 17 M = C * M * C.T(); 18 18 }; 19 19 void fsqmat::mult_sym_t ( const mat &C ) { 20 20 M = C.T() * M * C; 21 21 }; 22 22 void fsqmat::mult_sym ( const mat &C, fsqmat &U ) const { 23 23 U.M = ( C * ( M * C.T() ) ); 24 24 }; 25 25 void fsqmat::mult_sym_t ( const mat &C, fsqmat &U ) const { 26 26 U.M = ( C.T() * ( M * C ) ); 27 27 }; 28 28 void fsqmat::inv ( fsqmat &Inv ) const { 29 30 29 mat IM = itpp::inv ( M ); 30 Inv = IM; 31 31 }; 32 32 void fsqmat::clear() { 33 33 M.clear(); 34 34 }; 35 35 fsqmat::fsqmat ( const mat &M0 ) : sqmat ( M0.cols() ) { 36 37 36 bdm_assert_debug ( ( M0.cols() == M0.rows() ), "M0 must be square" ); 37 M = M0; 38 38 }; 39 39 … … 43 43 44 44 std::ostream &operator<< ( std::ostream &os, const fsqmat &ld ) { 45 46 45 os << ld.M << endl; 46 return os; 47 47 } 48 48 49 49 50 50 ldmat::ldmat ( const mat &exL, const vec &exD ) : sqmat ( exD.length() ) { 51 52 51 D = exD; 52 L = exL; 53 53 } 54 54 … … 58 58 59 59 ldmat::ldmat ( const vec D0 ) : sqmat ( D0.length() ) { 60 61 60 D = D0; 61 L = eye ( dim ); 62 62 } 63 63 64 64 ldmat::ldmat ( const mat &V ) : sqmat ( V.cols() ) { 65 65 66 67 68 69 70 66 bdm_assert_debug ( dim == V.rows(), "ldmat::ldmat matrix V is not square!" ); 67 68 // L and D will be allocated by ldform() 69 //Chol is unstable 70 this->ldform ( chol ( V ), ones ( dim ) ); 71 71 } 72 72 73 73 void ldmat::opupdt ( const vec &v, double w ) { 74 75 76 77 78 79 80 81 82 83 84 85 86 74 int dim = D.length(); 75 double kr; 76 vec r = v; 77 //beware! it is potentionally dangerous, if ITpp change _behaviour of _data()! 78 double *Lraw = L._data(); 79 double *Draw = D._data(); 80 double *rraw = r._data(); 81 82 bdm_assert_debug ( v.length() == dim, "LD::ldupdt vector v is not compatible with this ld." ); 83 84 for ( int i = dim - 1; i >= 0; i-- ) { 85 dydr ( rraw, Lraw + i, &w, Draw + i, rraw + i, 0, i, &kr, 1, dim ); 86 } 87 87 } 88 88 89 89 std::ostream &operator<< ( std::ostream &os, const ldmat &ld ) { 90 91 92 90 os << "L:" << ld.L << endl; 91 os << "D:" << ld.D << endl; 92 return os; 93 93 } 94 94 95 95 mat ldmat::to_mat() const { 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 96 int dim = D.length(); 97 mat V ( dim, dim ); 98 double sum; 99 int r, c, cc; 100 101 for ( r = 0; r < dim; r++ ) { //row cycle 102 for ( c = r; c < dim; c++ ) { 103 //column cycle, using symmetricity => c=r! 104 sum = 0.0; 105 for ( cc = c; cc < dim; cc++ ) { //cycle over the remaining part of the vector 106 sum += L ( cc, r ) * D ( cc ) * L ( cc, c ); 107 //here L(cc,r) = L(r,cc)'; 108 } 109 V ( r, c ) = sum; 110 // symmetricity 111 if ( r != c ) { 112 V ( c, r ) = sum; 113 }; 114 } 115 } 116 //mat V2 = L.transpose() * diag ( D ) * L; 117 return V; 118 118 } 119 119 120 120 121 121 void ldmat::add ( const ldmat &ld2, double w ) { 122 123 124 125 126 127 128 129 130 122 int dim = D.length(); 123 124 bdm_assert_debug ( ld2.D.length() == dim, "LD.add() incompatible sizes of LDs" ); 125 126 //Fixme can be done more efficiently either via dydr or ldform 127 for ( int r = 0; r < dim; r++ ) { 128 // Add columns of ld2.L' (i.e. rows of ld2.L) as dyads weighted by ld2.D 129 this->opupdt ( ld2.L.get_row ( r ), w*ld2.D ( r ) ); 130 } 131 131 } 132 132 133 133 void ldmat::clear() { 134 135 136 137 138 134 L.clear(); 135 for ( int i = 0; i < L.cols(); i++ ) { 136 L ( i, i ) = 1; 137 }; 138 D.clear(); 139 139 } 140 140 141 141 void ldmat::inv ( ldmat &Inv ) const { 142 143 144 145 142 Inv.clear(); //Inv = zero in LD 143 mat U = ltuinv ( L ); 144 145 Inv.ldform ( U.transpose(), 1.0 / D ); 146 146 } 147 147 148 148 void ldmat::mult_sym ( const mat &C ) { 149 150 149 mat A = L * C.T(); 150 this->ldform ( A, D ); 151 151 } 152 152 153 153 void ldmat::mult_sym_t ( const mat &C ) { 154 155 154 mat A = L * C; 155 this->ldform ( A, D ); 156 156 } 157 157 158 158 void ldmat::mult_sym ( const mat &C, ldmat &U ) const { 159 160 159 mat A = L * C.T(); //could be done more efficiently using BLAS 160 U.ldform ( A, D ); 161 161 } 162 162 163 163 void ldmat::mult_sym_t ( const mat &C, ldmat &U ) const { 164 165 166 167 164 mat A = L * C; 165 /* vec nD=zeros(U.rows()); 166 nD.replace_mid(0, D); //I case that D < nD*/ 167 U.ldform ( A, D ); 168 168 } 169 169 170 170 171 171 double ldmat::logdet() const { 172 173 172 double ldet = 0.0; 173 int i; 174 174 // sum logarithms of diagobal elements 175 176 177 178 175 for ( i = 0; i < D.length(); i++ ) { 176 ldet += log ( D ( i ) ); 177 }; 178 return ldet; 179 179 } 180 180 181 181 double ldmat::qform ( const vec &v ) const { 182 183 184 185 186 187 188 189 190 191 192 182 double x = 0.0, sum; 183 int i, j; 184 185 for ( i = 0; i < D.length(); i++ ) { //rows of L 186 sum = 0.0; 187 for ( j = 0; j <= i; j++ ) { 188 sum += L ( i, j ) * v ( j ); 189 } 190 x += D ( i ) * sum * sum; 191 }; 192 return x; 193 193 } 194 194 195 195 double ldmat::invqform ( const vec &v ) const { 196 197 198 199 200 201 202 203 204 205 196 double x = 0.0; 197 int i; 198 vec pom ( v.length() ); 199 200 backward_substitution ( L.T(), v, pom ); 201 202 for ( i = 0; i < D.length(); i++ ) { //rows of L 203 x += pom ( i ) * pom ( i ) / D ( i ); 204 }; 205 return x; 206 206 } 207 207 208 208 ldmat& ldmat::operator *= ( double x ) { 209 210 209 D *= x; 210 return *this; 211 211 } 212 212 213 213 vec ldmat::sqrt_mult ( const vec &x ) const { 214 215 216 217 218 219 220 221 222 214 int i, j; 215 vec res ( dim ); 216 //double sum; 217 for ( i = 0; i < dim; i++ ) {//for each element of result 218 res ( i ) = 0.0; 219 for ( j = i; j < dim; j++ ) {//sum D(j)*L(:,i).*x 220 res ( i ) += sqrt ( D ( j ) ) * L ( j, i ) * x ( j ); 221 } 222 } 223 223 // vec res2 = L.transpose()*diag( sqrt( D ) )*x; 224 224 return res; 225 225 } 226 226 227 227 void ldmat::ldform ( const mat &A, const vec &D0 ) { 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 228 int m = A.rows(); 229 int n = A.cols(); 230 int mn = ( m < n ) ? m : n ; 231 232 bdm_assert_debug ( D0.length() == A.rows(), "ldmat::ldform Vector D must have the length as row count of A" ); 233 234 L = concat_vertical ( zeros ( n, n ), diag ( sqrt ( D0 ) ) * A ); 235 D = zeros ( n + m ); 236 237 //unnecessary big L and D will be made smaller at the end of file 238 vec w = zeros ( n + m ); 239 240 double sum, beta, pom; 241 242 int cc = 0; 243 int i = n; // indexovani o 1 niz, nez v matlabu 244 int ii, j, jj; 245 while ( ( i > n - mn - cc ) && ( i > 0 ) ) { 246 i--; 247 sum = 0.0; 248 249 int last_v = m + i - n + cc + 1; 250 251 vec v = zeros ( last_v + 1 ); //prepare v 252 for ( ii = n - cc - 1; ii < m + i + 1; ii++ ) { 253 sum += L ( ii, i ) * L ( ii, i ); 254 v ( ii - n + cc + 1 ) = L ( ii, i ); //assign v 255 } 256 257 if ( L ( m + i, i ) == 0 ) 258 beta = sqrt ( sum ); 259 else 260 beta = L ( m + i, i ) + sign ( L ( m + i, i ) ) * sqrt ( sum ); 261 262 if ( std::fabs ( beta ) < eps ) { 263 cc++; 264 L.set_row ( n - cc, L.get_row ( m + i ) ); 265 L.set_row ( m + i, zeros ( L.cols() ) ); 266 D ( m + i ) = 0; 267 L ( m + i, i ) = 1; 268 L.set_submatrix ( n - cc, m + i - 1, i, i, 0 ); 269 continue; 270 } 271 272 sum -= v ( last_v ) * v ( last_v ); 273 sum /= beta * beta; 274 sum++; 275 276 v /= beta; 277 v ( last_v ) = 1; 278 279 pom = -2.0 / sum; 280 // echo to venca 281 282 for ( j = i; j >= 0; j-- ) { 283 double w_elem = 0; 284 for ( ii = n - cc; ii <= m + i + 1; ii++ ) 285 w_elem += v ( ii - n + cc ) * L ( ii - 1, j ); 286 w ( j ) = w_elem * pom; 287 } 288 289 for ( ii = n - cc - 1; ii <= m + i; ii++ ) 290 for ( jj = 0; jj < i; jj++ ) 291 L ( ii, jj ) += v ( ii - n + cc + 1 ) * w ( jj ); 292 293 for ( ii = n - cc - 1; ii < m + i; ii++ ) 294 L ( ii, i ) = 0; 295 296 L ( m + i, i ) += w ( i ); 297 D ( m + i ) = L ( m + i, i ) * L ( m + i, i ); 298 299 for ( ii = 0; ii <= i; ii++ ) 300 L ( m + i, ii ) /= L ( m + i, i ); 301 } 302 303 if ( i >= 0 ) 304 for ( ii = 0; ii < i; ii++ ) { 305 jj = D.length() - 1 - n + ii; 306 D ( jj ) = 0; 307 L.set_row ( jj, zeros ( L.cols() ) ); //TODO: set_row accepts Num_T 308 L ( jj, jj ) = 1; 309 } 310 311 L.del_rows ( 0, m - 1 ); 312 D.del ( 0, m - 1 ); 313 314 dim = L.rows(); 315 315 } 316 316 … … 318 318 319 319 mat ltuinv ( const mat &L ) { 320 321 322 323 320 int dim = L.cols(); 321 mat Il = eye ( dim ); 322 int i, j, k, m; 323 double s; 324 324 325 325 //Fixme blind transcription of ltuinv.m 326 327 328 329 330 331 332 333 334 335 336 337 326 for ( k = 1; k < ( dim ); k++ ) { 327 for ( i = 0; i < ( dim - k ); i++ ) { 328 j = i + k; //change in .m 1+1=2, here 0+0+1=1 329 s = L ( j, i ); 330 for ( m = i + 1; m < ( j ); m++ ) { 331 s += L ( m, i ) * Il ( j, m ); 332 } 333 Il ( j, i ) = -s; 334 } 335 } 336 337 return Il; 338 338 } 339 339 … … 369 369 ********************************************************************/ 370 370 { 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 } 401 402 } 371 int j, jm; 372 double kD, r0; 373 double mzero = 2.2e-16; 374 double threshold = 1e-4; 375 376 if ( fabs ( *Dr ) < mzero ) *Dr = 0; 377 r0 = *R; 378 *R = 0.0; 379 kD = *Df; 380 *kr = r0 * *Dr; 381 *Df = kD + r0 * ( *kr ); 382 if ( *Df > mzero ) { 383 kD /= *Df; 384 *kr /= *Df; 385 } else { 386 kD = 1.0; 387 *kr = 0.0; 388 if ( *Df < -threshold ) { 389 bdm_warning ( "Problem in dydr: subraction of dyad results in negative definitness. Likely mistake in calling function." ); 390 } 391 *Df = 0.0; 392 } 393 *Dr *= kD; 394 jm = mx * jl; 395 for ( j = m * jl; j < m*jh; j += m ) { 396 r[j] -= r0 * f[jm]; 397 f[jm] += *kr * r[j]; 398 jm += mx; 399 } 400 } 401 402 } -
library/bdm/math/square_mat.h
r1015 r1064 35 35 class sqmat { 36 36 public: 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 37 /*! 38 * Perfroms a rank-1 update by outer product of vectors: \f$V = V + w v v'\f$. 39 * @param v Vector forming the outer product to be added 40 * @param w weight of updating; can be negative 41 42 BLAS-2b operation. 43 */ 44 virtual void opupdt ( const vec &v, double w ) = 0; 45 46 /*! \brief Conversion to full matrix. 47 */ 48 virtual mat to_mat() const = 0; 49 50 /*! \brief Inplace symmetric multiplication by a SQUARE matrix \f$C\f$, i.e. \f$V = C*V*C'\f$ 51 @param C multiplying matrix, 52 */ 53 virtual void mult_sym ( const mat &C ) = 0; 54 55 /*! \brief Inplace symmetric multiplication by a SQUARE transpose of matrix \f$C\f$, i.e. \f$V = C'*V*C\f$ 56 @param C multiplying matrix, 57 */ 58 virtual void mult_sym_t ( const mat &C ) = 0; 59 60 /*! 61 \brief Logarithm of a determinant. 62 63 */ 64 virtual double logdet() const = 0; 65 66 /*! 67 \brief Multiplies square root of \f$V\f$ by vector \f$x\f$. 68 69 Used e.g. in generating normal samples. 70 */ 71 virtual vec sqrt_mult ( const vec &v ) const = 0; 72 73 /*! 74 \brief Evaluates quadratic form \f$x= v'*V*v\f$; 75 76 */ 77 virtual double qform ( const vec &v ) const = 0; 78 79 /*! 80 \brief Evaluates quadratic form \f$x= v'*inv(V)*v\f$; 81 82 */ 83 virtual double invqform ( const vec &v ) const = 0; 84 84 85 85 // //! easy version of the 86 86 // sqmat inv(); 87 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 88 //! Clearing matrix so that it corresponds to zeros. 89 virtual void clear() = 0; 90 91 //! Reimplementing common functions of mat: cols(). 92 int cols() const { 93 return dim; 94 }; 95 96 //! Reimplementing common functions of mat: rows(). 97 int rows() const { 98 return dim; 99 }; 100 101 //! Destructor for future use; 102 virtual ~sqmat() {}; 103 //! Default constructor 104 sqmat ( const int dim0 ) : dim ( dim0 ) {}; 105 //! Default constructor 106 sqmat() : dim ( 0 ) {}; 107 107 protected: 108 109 108 //! dimension of the square matrix 109 int dim; 110 110 }; 111 111 … … 117 117 class fsqmat: public sqmat { 118 118 protected: 119 120 119 //! Full matrix on which the operations are performed 120 mat M; 121 121 public: 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 122 void opupdt ( const vec &v, double w ); 123 mat to_mat() const; 124 void mult_sym ( const mat &C ); 125 void mult_sym_t ( const mat &C ); 126 //! store result of \c mult_sym in external matrix \f$U\f$ 127 void mult_sym ( const mat &C, fsqmat &U ) const; 128 //! store result of \c mult_sym_t in external matrix \f$U\f$ 129 void mult_sym_t ( const mat &C, fsqmat &U ) const; 130 void clear(); 131 132 //! Default initialization 133 fsqmat() {}; // mat will be initialized OK 134 //! Default initialization with proper size 135 fsqmat ( const int dim0 ); // mat will be initialized OK 136 //! Constructor 137 fsqmat ( const mat &M ); 138 139 /*! 140 Some templates require this constructor to compile, but 141 it shouldn't actually be called. 142 */ 143 fsqmat ( const fsqmat &M, const ivec &perm ) { 144 bdm_error ( "not implemented" ); 145 } 146 147 //! Constructor 148 fsqmat ( const vec &d ) : sqmat ( d.length() ) { 149 M = diag ( d ); 150 }; 151 152 //! Destructor for future use; 153 virtual ~fsqmat() {}; 154 155 156 /*! \brief Matrix inversion preserving the chosen form. 157 158 @param Inv a space where the inverse is stored. 159 160 */ 161 void inv ( fsqmat &Inv ) const; 162 163 double logdet() const { 164 return log ( det ( M ) ); 165 }; 166 double qform ( const vec &v ) const { 167 return ( v* ( M*v ) ); 168 }; 169 double invqform ( const vec &v ) const { 170 return ( v* ( itpp::inv ( M ) *v ) ); 171 }; 172 vec sqrt_mult ( const vec &v ) const { 173 mat Ch = chol ( M ); 174 return Ch*v; 175 }; 176 177 //! Add another matrix in fsq form with weight w 178 void add ( const fsqmat &fsq2, double w = 1.0 ) { 179 M += fsq2.M; 180 }; 181 182 //! Access functions 183 void setD ( const vec &nD ) { 184 M = diag ( nD ); 185 } 186 //! Access functions 187 vec getD () { 188 return diag ( M ); 189 } 190 //! Access functions 191 void setD ( const vec &nD, int i ) { 192 for ( int j = i; j < nD.length(); j++ ) { 193 M ( j, j ) = nD ( j - i ); //Fixme can be more general 194 } 195 } 196 197 198 //! add another fsqmat matrix 199 fsqmat& operator += ( const fsqmat &A ) { 200 M += A.M; 201 return *this; 202 }; 203 //! subtrack another fsqmat matrix 204 fsqmat& operator -= ( const fsqmat &A ) { 205 M -= A.M; 206 return *this; 207 }; 208 //! multiply by a scalar 209 fsqmat& operator *= ( double x ) { 210 M *= x; 211 return *this; 212 }; 213 214 //! cast to normal mat 215 operator mat&() { 216 return M; 217 }; 218 218 219 219 // fsqmat& operator = ( const fsqmat &A) {M=A.M; return *this;}; 220 221 222 223 224 225 220 //! print full matrix 221 friend std::ostream &operator<< ( std::ostream &os, const fsqmat &sq ); 222 //!access function 223 mat & _M ( ) { 224 return M; 225 }; 226 226 227 227 }; … … 235 235 class ldmat: public sqmat { 236 236 public: 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 237 //! Construct by copy of L and D. 238 ldmat ( const mat &L, const vec &D ); 239 //! Construct by decomposition of full matrix V. 240 ldmat ( const mat &V ); 241 //! Construct by restructuring of V0 accordint to permutation vector perm. 242 ldmat ( const ldmat &V0, const ivec &perm ) : sqmat ( V0.rows() ) { 243 ldform ( V0.L.get_cols ( perm ), V0.D ); 244 }; 245 //! Construct diagonal matrix with diagonal D0 246 ldmat ( vec D0 ); 247 //!Default constructor 248 ldmat (); 249 //! Default initialization with proper size 250 ldmat ( const int dim0 ); 251 252 //! Destructor for future use; 253 virtual ~ldmat() {}; 254 255 // Reimplementation of compulsory operatios 256 257 void opupdt ( const vec &v, double w ); 258 mat to_mat() const; 259 void mult_sym ( const mat &C ); 260 void mult_sym_t ( const mat &C ); 261 //! Add another matrix in LD form with weight w 262 void add ( const ldmat &ld2, double w = 1.0 ); 263 double logdet() const; 264 double qform ( const vec &v ) const; 265 double invqform ( const vec &v ) const; 266 void clear(); 267 vec sqrt_mult ( const vec &v ) const; 268 269 270 /*! \brief Matrix inversion preserving the chosen form. 271 @param Inv a space where the inverse is stored. 272 */ 273 void inv ( ldmat &Inv ) const; 274 275 /*! \brief Symmetric multiplication of \f$U\f$ by a general matrix \f$C\f$, result of which is stored in the current class. 276 @param C matrix to multiply with 277 @param U a space where the inverse is stored. 278 */ 279 void mult_sym ( const mat &C, ldmat &U ) const; 280 281 /*! \brief Symmetric multiplication of \f$U\f$ by a transpose of a general matrix \f$C\f$, result of which is stored in the current class. 282 @param C matrix to multiply with 283 @param U a space where the inverse is stored. 284 */ 285 void mult_sym_t ( const mat &C, ldmat &U ) const; 286 287 288 /*! \brief Transforms general \f$A'D0 A\f$ into pure \f$L'DL\f$ 289 290 The new decomposition fullfills: \f$A'*diag(D)*A = self.L'*diag(self.D)*self.L\f$ 291 @param A general matrix 292 @param D0 general vector 293 */ 294 void ldform ( const mat &A, const vec &D0 ); 295 296 //! Access functions 297 void setD ( const vec &nD ) { 298 D = nD; 299 } 300 //! Access functions 301 void setD ( const vec &nD, int i ) { 302 D.replace_mid ( i, nD ); //Fixme can be more general 303 } 304 //! Access functions 305 void setL ( const vec &nL ) { 306 L = nL; 307 } 308 308 309 309 //! Access functions 310 const vec& _D() const {311 312 }310 const vec& _D() const { 311 return D; 312 } 313 313 //! Access functions 314 const mat& _L() const {315 316 }314 const mat& _L() const { 315 return L; 316 } 317 317 //! Access functions 318 vec& __D() {319 320 }318 vec& __D() { 319 return D; 320 } 321 321 //! Access functions 322 mat& __L() {323 324 }325 void validate(){326 327 }328 329 330 331 332 333 334 335 336 337 322 mat& __L() { 323 return L; 324 } 325 void validate() { 326 dim= L.rows(); 327 } 328 329 //! add another ldmat matrix 330 ldmat& operator += ( const ldmat &ldA ); 331 //! subtract another ldmat matrix 332 ldmat& operator -= ( const ldmat &ldA ); 333 //! multiply by a scalar 334 ldmat& operator *= ( double x ); 335 336 //! print both \c L and \c D 337 friend std::ostream &operator<< ( std::ostream &os, const ldmat &sq ); 338 338 339 339 protected: 340 341 342 343 340 //! Positive vector \f$D\f$ 341 vec D; 342 //! Lower-triangular matrix \f$L\f$ 343 mat L; 344 344 345 345 }; … … 348 348 //!mapping of add operation to operators 349 349 inline ldmat& ldmat::operator += ( const ldmat & ldA ) { 350 351 350 this->add ( ldA ); 351 return *this; 352 352 } 353 353 //!mapping of negative add operation to operators 354 354 inline ldmat& ldmat::operator -= ( const ldmat & ldA ) { 355 356 355 this->add ( ldA, -1.0 ); 356 return *this; 357 357 } 358 358
