[262] | 1 | |
---|
[384] | 2 | #include "square_mat.h" |
---|
[2] | 3 | |
---|
[495] | 4 | namespace bdm |
---|
| 5 | { |
---|
| 6 | |
---|
[2] | 7 | using namespace itpp; |
---|
| 8 | |
---|
[7] | 9 | using std::endl; |
---|
[2] | 10 | |
---|
[477] | 11 | void fsqmat::opupdt ( const vec &v, double w ) { |
---|
| 12 | M += outer_product ( v, v * w ); |
---|
[22] | 13 | }; |
---|
[477] | 14 | mat fsqmat::to_mat() const { |
---|
| 15 | return M; |
---|
| 16 | }; |
---|
| 17 | void fsqmat::mult_sym ( const mat &C ) { |
---|
| 18 | M = C * M * C.T(); |
---|
| 19 | }; |
---|
| 20 | void fsqmat::mult_sym_t ( const mat &C ) { |
---|
| 21 | M = C.T() * M * C; |
---|
| 22 | }; |
---|
| 23 | void fsqmat::mult_sym ( const mat &C, fsqmat &U ) const { |
---|
| 24 | U.M = ( C * ( M * C.T() ) ); |
---|
| 25 | }; |
---|
| 26 | void fsqmat::mult_sym_t ( const mat &C, fsqmat &U ) const { |
---|
| 27 | U.M = ( C.T() * ( M * C ) ); |
---|
| 28 | }; |
---|
| 29 | void fsqmat::inv ( fsqmat &Inv ) const { |
---|
| 30 | mat IM = itpp::inv ( M ); |
---|
| 31 | Inv = IM; |
---|
| 32 | }; |
---|
| 33 | void fsqmat::clear() { |
---|
| 34 | M.clear(); |
---|
| 35 | }; |
---|
| 36 | fsqmat::fsqmat ( const mat &M0 ) : sqmat ( M0.cols() ) { |
---|
[565] | 37 | bdm_assert_debug ( ( M0.cols() == M0.rows() ), "M0 must be square" ); |
---|
[477] | 38 | M = M0; |
---|
| 39 | }; |
---|
[26] | 40 | |
---|
[32] | 41 | //fsqmat::fsqmat() {}; |
---|
[2] | 42 | |
---|
[477] | 43 | fsqmat::fsqmat ( const int dim0 ) : sqmat ( dim0 ), M ( dim0, dim0 ) {}; |
---|
[2] | 44 | |
---|
[32] | 45 | std::ostream &operator<< ( std::ostream &os, const fsqmat &ld ) { |
---|
[26] | 46 | os << ld.M << endl; |
---|
| 47 | return os; |
---|
| 48 | } |
---|
| 49 | |
---|
| 50 | |
---|
[477] | 51 | ldmat::ldmat ( const mat &exL, const vec &exD ) : sqmat ( exD.length() ) { |
---|
[5] | 52 | D = exD; |
---|
| 53 | L = exL; |
---|
[2] | 54 | } |
---|
| 55 | |
---|
[477] | 56 | ldmat::ldmat() : sqmat ( 0 ) {} |
---|
[2] | 57 | |
---|
[477] | 58 | ldmat::ldmat ( const int dim0 ) : sqmat ( dim0 ), D ( dim0 ), L ( dim0, dim0 ) {} |
---|
[26] | 59 | |
---|
[477] | 60 | ldmat::ldmat ( const vec D0 ) : sqmat ( D0.length() ) { |
---|
[19] | 61 | D = D0; |
---|
[477] | 62 | L = eye ( dim ); |
---|
[19] | 63 | } |
---|
| 64 | |
---|
[477] | 65 | ldmat::ldmat ( const mat &V ) : sqmat ( V.cols() ) { |
---|
[7] | 66 | |
---|
[565] | 67 | bdm_assert_debug ( dim == V.rows(), "ldmat::ldmat matrix V is not square!" ); |
---|
[477] | 68 | |
---|
[18] | 69 | // L and D will be allocated by ldform() |
---|
[26] | 70 | //Chol is unstable |
---|
[477] | 71 | this->ldform ( chol ( V ), ones ( dim ) ); |
---|
[7] | 72 | } |
---|
| 73 | |
---|
[477] | 74 | void ldmat::opupdt ( const vec &v, double w ) { |
---|
[7] | 75 | int dim = D.length(); |
---|
| 76 | double kr; |
---|
| 77 | vec r = v; |
---|
| 78 | //beware! it is potentionally dangerous, if ITpp change _behaviour of _data()! |
---|
| 79 | double *Lraw = L._data(); |
---|
| 80 | double *Draw = D._data(); |
---|
| 81 | double *rraw = r._data(); |
---|
| 82 | |
---|
[565] | 83 | bdm_assert_debug ( v.length() == dim, "LD::ldupdt vector v is not compatible with this ld." ); |
---|
[7] | 84 | |
---|
| 85 | for ( int i = dim - 1; i >= 0; i-- ) { |
---|
[477] | 86 | dydr ( rraw, Lraw + i, &w, Draw + i, rraw + i, 0, i, &kr, 1, dim ); |
---|
[7] | 87 | } |
---|
| 88 | } |
---|
| 89 | |
---|
[32] | 90 | std::ostream &operator<< ( std::ostream &os, const ldmat &ld ) { |
---|
[12] | 91 | os << "L:" << ld.L << endl; |
---|
| 92 | os << "D:" << ld.D << endl; |
---|
[19] | 93 | return os; |
---|
[7] | 94 | } |
---|
| 95 | |
---|
[168] | 96 | mat ldmat::to_mat() const { |
---|
[7] | 97 | int dim = D.length(); |
---|
[477] | 98 | mat V ( dim, dim ); |
---|
[7] | 99 | double sum; |
---|
| 100 | int r, c, cc; |
---|
| 101 | |
---|
[477] | 102 | for ( r = 0; r < dim; r++ ) { //row cycle |
---|
| 103 | for ( c = r; c < dim; c++ ) { |
---|
[7] | 104 | //column cycle, using symmetricity => c=r! |
---|
| 105 | sum = 0.0; |
---|
[477] | 106 | for ( cc = c; cc < dim; cc++ ) { //cycle over the remaining part of the vector |
---|
| 107 | sum += L ( cc, r ) * D ( cc ) * L ( cc, c ); |
---|
[7] | 108 | //here L(cc,r) = L(r,cc)'; |
---|
| 109 | } |
---|
[477] | 110 | V ( r, c ) = sum; |
---|
[7] | 111 | // symmetricity |
---|
[477] | 112 | if ( r != c ) { |
---|
| 113 | V ( c, r ) = sum; |
---|
| 114 | }; |
---|
[7] | 115 | } |
---|
| 116 | } |
---|
[477] | 117 | mat V2 = L.transpose() * diag ( D ) * L; |
---|
[12] | 118 | return V2; |
---|
[7] | 119 | } |
---|
| 120 | |
---|
| 121 | |
---|
[477] | 122 | void ldmat::add ( const ldmat &ld2, double w ) { |
---|
[7] | 123 | int dim = D.length(); |
---|
| 124 | |
---|
[565] | 125 | bdm_assert_debug ( ld2.D.length() == dim, "LD.add() incompatible sizes of LDs" ); |
---|
[7] | 126 | |
---|
| 127 | //Fixme can be done more efficiently either via dydr or ldform |
---|
| 128 | for ( int r = 0; r < dim; r++ ) { |
---|
| 129 | // Add columns of ld2.L' (i.e. rows of ld2.L) as dyads weighted by ld2.D |
---|
[477] | 130 | this->opupdt ( ld2.L.get_row ( r ), w*ld2.D ( r ) ); |
---|
[7] | 131 | } |
---|
| 132 | } |
---|
| 133 | |
---|
[477] | 134 | void ldmat::clear() { |
---|
| 135 | L.clear(); |
---|
| 136 | for ( int i = 0; i < L.cols(); i++ ) { |
---|
| 137 | L ( i, i ) = 1; |
---|
| 138 | }; |
---|
| 139 | D.clear(); |
---|
| 140 | } |
---|
[7] | 141 | |
---|
[477] | 142 | void ldmat::inv ( ldmat &Inv ) const { |
---|
[7] | 143 | Inv.clear(); //Inv = zero in LD |
---|
[477] | 144 | mat U = ltuinv ( L ); |
---|
[7] | 145 | |
---|
[477] | 146 | Inv.ldform ( U.transpose(), 1.0 / D ); |
---|
[7] | 147 | } |
---|
| 148 | |
---|
[477] | 149 | void ldmat::mult_sym ( const mat &C ) { |
---|
| 150 | mat A = L * C.T(); |
---|
| 151 | this->ldform ( A, D ); |
---|
[26] | 152 | } |
---|
[7] | 153 | |
---|
[477] | 154 | void ldmat::mult_sym_t ( const mat &C ) { |
---|
| 155 | mat A = L * C; |
---|
| 156 | this->ldform ( A, D ); |
---|
[26] | 157 | } |
---|
[7] | 158 | |
---|
[477] | 159 | void ldmat::mult_sym ( const mat &C, ldmat &U ) const { |
---|
| 160 | mat A = L * C.T(); //could be done more efficiently using BLAS |
---|
| 161 | U.ldform ( A, D ); |
---|
[7] | 162 | } |
---|
| 163 | |
---|
[477] | 164 | void ldmat::mult_sym_t ( const mat &C, ldmat &U ) const { |
---|
| 165 | mat A = L * C; |
---|
| 166 | /* vec nD=zeros(U.rows()); |
---|
| 167 | nD.replace_mid(0, D); //I case that D < nD*/ |
---|
| 168 | U.ldform ( A, D ); |
---|
[8] | 169 | } |
---|
| 170 | |
---|
[26] | 171 | |
---|
| 172 | double ldmat::logdet() const { |
---|
[7] | 173 | double ldet = 0.0; |
---|
| 174 | int i; |
---|
| 175 | // sum logarithms of diagobal elements |
---|
[477] | 176 | for ( i = 0; i < D.length(); i++ ) { |
---|
| 177 | ldet += log ( D ( i ) ); |
---|
| 178 | }; |
---|
[19] | 179 | return ldet; |
---|
[7] | 180 | } |
---|
| 181 | |
---|
[477] | 182 | double ldmat::qform ( const vec &v ) const { |
---|
[7] | 183 | double x = 0.0, sum; |
---|
[477] | 184 | int i, j; |
---|
[7] | 185 | |
---|
[477] | 186 | for ( i = 0; i < D.length(); i++ ) { //rows of L |
---|
[7] | 187 | sum = 0.0; |
---|
[477] | 188 | for ( j = 0; j <= i; j++ ) { |
---|
| 189 | sum += L ( i, j ) * v ( j ); |
---|
| 190 | } |
---|
| 191 | x += D ( i ) * sum * sum; |
---|
[7] | 192 | }; |
---|
| 193 | return x; |
---|
| 194 | } |
---|
| 195 | |
---|
[477] | 196 | double ldmat::invqform ( const vec &v ) const { |
---|
[75] | 197 | double x = 0.0; |
---|
| 198 | int i; |
---|
[477] | 199 | vec pom ( v.length() ); |
---|
| 200 | |
---|
| 201 | backward_substitution ( L.T(), v, pom ); |
---|
| 202 | |
---|
| 203 | for ( i = 0; i < D.length(); i++ ) { //rows of L |
---|
| 204 | x += pom ( i ) * pom ( i ) / D ( i ); |
---|
[75] | 205 | }; |
---|
| 206 | return x; |
---|
| 207 | } |
---|
| 208 | |
---|
[12] | 209 | ldmat& ldmat::operator *= ( double x ) { |
---|
[477] | 210 | D *= x; |
---|
[19] | 211 | return *this; |
---|
[7] | 212 | } |
---|
| 213 | |
---|
[477] | 214 | vec ldmat::sqrt_mult ( const vec &x ) const { |
---|
| 215 | int i, j; |
---|
| 216 | vec res ( dim ); |
---|
[19] | 217 | //double sum; |
---|
[477] | 218 | for ( i = 0; i < dim; i++ ) {//for each element of result |
---|
| 219 | res ( i ) = 0.0; |
---|
| 220 | for ( j = i; j < dim; j++ ) {//sum D(j)*L(:,i).*x |
---|
| 221 | res ( i ) += sqrt ( D ( j ) ) * L ( j, i ) * x ( j ); |
---|
[12] | 222 | } |
---|
| 223 | } |
---|
[19] | 224 | // vec res2 = L.transpose()*diag( sqrt( D ) )*x; |
---|
| 225 | return res; |
---|
[12] | 226 | } |
---|
[7] | 227 | |
---|
[477] | 228 | void ldmat::ldform ( const mat &A, const vec &D0 ) { |
---|
[12] | 229 | int m = A.rows(); |
---|
| 230 | int n = A.cols(); |
---|
[477] | 231 | int mn = ( m < n ) ? m : n ; |
---|
[12] | 232 | |
---|
[565] | 233 | bdm_assert_debug ( D0.length() == A.rows(), "ldmat::ldform Vector D must have the length as row count of A" ); |
---|
[12] | 234 | |
---|
[477] | 235 | L = concat_vertical ( zeros ( n, n ), diag ( sqrt ( D0 ) ) * A ); |
---|
| 236 | D = zeros ( n + m ); |
---|
[22] | 237 | |
---|
[477] | 238 | //unnecessary big L and D will be made smaller at the end of file |
---|
| 239 | vec w = zeros ( n + m ); |
---|
| 240 | |
---|
[12] | 241 | double sum, beta, pom; |
---|
| 242 | |
---|
[477] | 243 | int cc = 0; |
---|
| 244 | int i = n; // indexovani o 1 niz, nez v matlabu |
---|
| 245 | int ii, j, jj; |
---|
| 246 | while ( ( i > n - mn - cc ) && ( i > 0 ) ) { |
---|
[12] | 247 | i--; |
---|
| 248 | sum = 0.0; |
---|
[21] | 249 | |
---|
[477] | 250 | int last_v = m + i - n + cc + 1; |
---|
| 251 | |
---|
| 252 | vec v = zeros ( last_v + 1 ); //prepare v |
---|
| 253 | for ( ii = n - cc - 1; ii < m + i + 1; ii++ ) { |
---|
| 254 | sum += L ( ii, i ) * L ( ii, i ); |
---|
| 255 | v ( ii - n + cc + 1 ) = L ( ii, i ); //assign v |
---|
[12] | 256 | } |
---|
[21] | 257 | |
---|
[477] | 258 | if ( L ( m + i, i ) == 0 ) |
---|
| 259 | beta = sqrt ( sum ); |
---|
[21] | 260 | else |
---|
[477] | 261 | beta = L ( m + i, i ) + sign ( L ( m + i, i ) ) * sqrt ( sum ); |
---|
| 262 | |
---|
| 263 | if ( std::fabs ( beta ) < eps ) { |
---|
[12] | 264 | cc++; |
---|
[477] | 265 | L.set_row ( n - cc, L.get_row ( m + i ) ); |
---|
| 266 | L.set_row ( m + i, zeros ( L.cols() ) ); |
---|
| 267 | D ( m + i ) = 0; |
---|
| 268 | L ( m + i, i ) = 1; |
---|
| 269 | L.set_submatrix ( n - cc, m + i - 1, i, i, 0 ); |
---|
[12] | 270 | continue; |
---|
| 271 | } |
---|
| 272 | |
---|
[477] | 273 | sum -= v ( last_v ) * v ( last_v ); |
---|
| 274 | sum /= beta * beta; |
---|
[12] | 275 | sum++; |
---|
| 276 | |
---|
[477] | 277 | v /= beta; |
---|
| 278 | v ( last_v ) = 1; |
---|
[12] | 279 | |
---|
[477] | 280 | pom = -2.0 / sum; |
---|
| 281 | // echo to venca |
---|
[21] | 282 | |
---|
[477] | 283 | for ( j = i; j >= 0; j-- ) { |
---|
| 284 | double w_elem = 0; |
---|
| 285 | for ( ii = n - cc; ii <= m + i + 1; ii++ ) |
---|
| 286 | w_elem += v ( ii - n + cc ) * L ( ii - 1, j ); |
---|
| 287 | w ( j ) = w_elem * pom; |
---|
[12] | 288 | } |
---|
| 289 | |
---|
[477] | 290 | for ( ii = n - cc - 1; ii <= m + i; ii++ ) |
---|
| 291 | for ( jj = 0; jj < i; jj++ ) |
---|
| 292 | L ( ii, jj ) += v ( ii - n + cc + 1 ) * w ( jj ); |
---|
[21] | 293 | |
---|
[477] | 294 | for ( ii = n - cc - 1; ii < m + i; ii++ ) |
---|
| 295 | L ( ii, i ) = 0; |
---|
[21] | 296 | |
---|
[477] | 297 | L ( m + i, i ) += w ( i ); |
---|
| 298 | D ( m + i ) = L ( m + i, i ) * L ( m + i, i ); |
---|
[12] | 299 | |
---|
[477] | 300 | for ( ii = 0; ii <= i; ii++ ) |
---|
| 301 | L ( m + i, ii ) /= L ( m + i, i ); |
---|
[22] | 302 | } |
---|
[21] | 303 | |
---|
[477] | 304 | if ( i >= 0 ) |
---|
| 305 | for ( ii = 0; ii < i; ii++ ) { |
---|
| 306 | jj = D.length() - 1 - n + ii; |
---|
| 307 | D ( jj ) = 0; |
---|
| 308 | L.set_row ( jj, zeros ( L.cols() ) ); //TODO: set_row accepts Num_T |
---|
| 309 | L ( jj, jj ) = 1; |
---|
[12] | 310 | } |
---|
[22] | 311 | |
---|
[477] | 312 | L.del_rows ( 0, m - 1 ); |
---|
| 313 | D.del ( 0, m - 1 ); |
---|
| 314 | |
---|
[98] | 315 | dim = L.rows(); |
---|
[12] | 316 | } |
---|
| 317 | |
---|
[7] | 318 | //////// Auxiliary Functions |
---|
| 319 | |
---|
[477] | 320 | mat ltuinv ( const mat &L ) { |
---|
[7] | 321 | int dim = L.cols(); |
---|
[477] | 322 | mat Il = eye ( dim ); |
---|
[7] | 323 | int i, j, k, m; |
---|
| 324 | double s; |
---|
| 325 | |
---|
| 326 | //Fixme blind transcription of ltuinv.m |
---|
[477] | 327 | for ( k = 1; k < ( dim ); k++ ) { |
---|
| 328 | for ( i = 0; i < ( dim - k ); i++ ) { |
---|
[7] | 329 | j = i + k; //change in .m 1+1=2, here 0+0+1=1 |
---|
[477] | 330 | s = L ( j, i ); |
---|
[75] | 331 | for ( m = i + 1; m < ( j ); m++ ) { |
---|
[477] | 332 | s += L ( m, i ) * Il ( j, m ); |
---|
[7] | 333 | } |
---|
[477] | 334 | Il ( j, i ) = -s; |
---|
[7] | 335 | } |
---|
| 336 | } |
---|
| 337 | |
---|
| 338 | return Il; |
---|
| 339 | } |
---|
| 340 | |
---|
[477] | 341 | void dydr ( double * r, double *f, double *Dr, double *Df, double *R, int jl, int jh, double *kr, int m, int mx ) |
---|
[7] | 342 | /******************************************************************** |
---|
| 343 | |
---|
| 344 | dydr = dyadic reduction, performs transformation of sum of |
---|
| 345 | 2 dyads r*Dr*r'+ f*Df*f' so that the element of r pointed |
---|
| 346 | by R is zeroed. This version allows Dr to be NEGATIVE. Hence the name negdydr or dydr_withneg. |
---|
| 347 | |
---|
| 348 | Parameters : |
---|
| 349 | r ... pointer to reduced dyad |
---|
| 350 | f ... pointer to reducing dyad |
---|
| 351 | Dr .. pointer to the weight of reduced dyad |
---|
| 352 | Df .. pointer to the weight of reducing dyad |
---|
| 353 | R ... pointer to the element of r, which is to be reduced to |
---|
| 354 | zero; the corresponding element of f is assumed to be 1. |
---|
| 355 | jl .. lower index of the range within which the dyads are |
---|
| 356 | modified |
---|
| 357 | ju .. upper index of the range within which the dyads are |
---|
| 358 | modified |
---|
| 359 | kr .. pointer to the coefficient used in the transformation of r |
---|
| 360 | rnew = r + kr*f |
---|
| 361 | m .. number of rows of modified matrix (part of which is r) |
---|
| 362 | Remark : Constant mzero means machine zero and should be modified |
---|
| 363 | according to the precision of particular machine |
---|
| 364 | |
---|
| 365 | V. Peterka 17-7-89 |
---|
| 366 | |
---|
| 367 | Added: |
---|
| 368 | mx .. number of rows of modified matrix (part of which is f) -PN |
---|
| 369 | |
---|
| 370 | ********************************************************************/ |
---|
[5] | 371 | { |
---|
[7] | 372 | int j, jm; |
---|
| 373 | double kD, r0; |
---|
| 374 | double mzero = 2.2e-16; |
---|
| 375 | double threshold = 1e-4; |
---|
| 376 | |
---|
[477] | 377 | if ( fabs ( *Dr ) < mzero ) *Dr = 0; |
---|
[7] | 378 | r0 = *R; |
---|
| 379 | *R = 0.0; |
---|
| 380 | kD = *Df; |
---|
| 381 | *kr = r0 * *Dr; |
---|
| 382 | *Df = kD + r0 * ( *kr ); |
---|
| 383 | if ( *Df > mzero ) { |
---|
| 384 | kD /= *Df; |
---|
| 385 | *kr /= *Df; |
---|
| 386 | } else { |
---|
| 387 | kD = 1.0; |
---|
| 388 | *kr = 0.0; |
---|
[8] | 389 | if ( *Df < -threshold ) { |
---|
[565] | 390 | bdm_warning ( "Problem in dydr: subraction of dyad results in negative definitness. Likely mistake in calling function." ); |
---|
[12] | 391 | } |
---|
[7] | 392 | *Df = 0.0; |
---|
| 393 | } |
---|
| 394 | *Dr *= kD; |
---|
| 395 | jm = mx * jl; |
---|
| 396 | for ( j = m * jl; j < m*jh; j += m ) { |
---|
| 397 | r[j] -= r0 * f[jm]; |
---|
| 398 | f[jm] += *kr * r[j]; |
---|
| 399 | jm += mx; |
---|
| 400 | } |
---|
[2] | 401 | } |
---|
[495] | 402 | |
---|
| 403 | } |
---|