1 //============================================================================== 2 // 3 // This file is part of GPSTk, the GPS Toolkit. 4 // 5 // The GPSTk is free software; you can redistribute it and/or modify 6 // it under the terms of the GNU Lesser General Public License as published 7 // by the Free Software Foundation; either version 3.0 of the License, or 8 // any later version. 9 // 10 // The GPSTk is distributed in the hope that it will be useful, 11 // but WITHOUT ANY WARRANTY; without even the implied warranty of 12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 // GNU Lesser General Public License for more details. 14 // 15 // You should have received a copy of the GNU Lesser General Public 16 // License along with GPSTk; if not, write to the Free Software Foundation, 17 // Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA 18 // 19 // This software was developed by Applied Research Laboratories at the 20 // University of Texas at Austin. 21 // Copyright 2004-2020, The Board of Regents of The University of Texas System 22 // 23 //============================================================================== 24 25 //============================================================================== 26 // 27 // This software was developed by Applied Research Laboratories at the 28 // University of Texas at Austin, under contract to an agency or agencies 29 // within the U.S. Department of Defense. The U.S. Government retains all 30 // rights to use, duplicate, distribute, disclose, or release this software. 31 // 32 // Pursuant to DoD Directive 523024 33 // 34 // DISTRIBUTION STATEMENT A: This software has been approved for public 35 // release, distribution is unlimited. 36 // 37 //============================================================================== 38 39 /** 40 * @file SRI.cpp 41 * Implementation of class SRI. 42 * class SRI implements the square root information methods, used for least squares 43 * estimation and the SRI form of the Kalman filter. 44 * 45 * Reference: "Factorization Methods for Discrete Sequential Estimation," 46 * by G.J. Bierman, Academic Press, 1977. 47 */ 48 49 // ----------------------------------------------------------------------------------- 50 // system 51 #include <string> 52 #include <vector> 53 #include <algorithm> 54 #include <ostream> 55 // geomatics 56 #include "SRI.hpp" 57 #include "Namelist.hpp" 58 // GPSTk 59 #include "StringUtils.hpp" 60 61 using namespace std; 62 63 namespace gpstk 64 { 65 using namespace StringUtils; 66 67 // -------------------------------------------------------------------------------- 68 // used to mark optional input 69 const Matrix<double> SRINullMatrix; 70 const SparseMatrix<double> SRINullSparseMatrix; 71 72 //--------------------------------------------------------------------------------- 73 // constructor given the dimension N. SRI(const unsigned int N)74 SRI::SRI(const unsigned int N) 75 throw() 76 { 77 R = Matrix<double>(N,N,0.0); 78 Z = Vector<double>(N,0.0); 79 names = Namelist(N); 80 } 81 82 // -------------------------------------------------------------------------------- 83 // constructor given a Namelist, its dimension determines the SRI dimension. SRI(const Namelist & nl)84 SRI::SRI(const Namelist& nl) 85 throw() 86 { 87 if(nl.size() <= 0) return; 88 R = Matrix<double>(nl.size(),nl.size(),0.0); 89 Z = Vector<double>(nl.size(),0.0); 90 names = nl; 91 } 92 93 // -------------------------------------------------------------------------------- 94 // explicit constructor - throw if the dimensions are inconsistent. SRI(const Matrix<double> & r,const Vector<double> & z,const Namelist & nl)95 SRI::SRI(const Matrix<double>& r, 96 const Vector<double>& z, 97 const Namelist& nl) 98 { 99 if(r.rows() != r.cols() || r.rows() != z.size() || r.rows() != nl.size()) { 100 MatrixException me("Invalid dimensions in explicit SRI constructor:\n R is " 101 + asString<int>(r.rows()) + "x" 102 + asString<int>(r.cols()) + ", Z has length " 103 + asString<int>(z.size()) + " and NL has length " 104 + asString<int>(nl.size())); 105 GPSTK_THROW(me); 106 } 107 if(r.rows() <= 0) return; 108 R = r; 109 Z = z; 110 names = nl; 111 } 112 113 // -------------------------------------------------------------------------------- 114 // define from covariance and state setFromCovState(const Matrix<double> & Cov,const Vector<double> & State,const Namelist & NL)115 void SRI::setFromCovState(const Matrix<double>& Cov, const Vector<double>& State, 116 const Namelist& NL) 117 { 118 if(Cov.rows()!=Cov.cols() || Cov.rows()!=State.size() || Cov.rows()!=NL.size()) 119 { 120 MatrixException me("Invalid dimensions in SRI constructor from Cov,State:\n" 121 " Cov is " + asString<int>(Cov.rows()) + "x" + asString<int>(Cov.cols()) 122 + ", State has length " + asString<int>(State.size()) 123 + " and NL has length " + asString<int>(NL.size())); 124 GPSTK_THROW(me); 125 } 126 R = inverseUT(upperCholesky(Cov)); 127 Z = R * State; 128 names = NL; 129 } 130 131 // -------------------------------------------------------------------------------- 132 // copy constructor SRI(const SRI & s)133 SRI::SRI(const SRI& s) 134 throw() 135 { 136 R = s.R; 137 Z = s.Z; 138 names = s.names; 139 } 140 141 // -------------------------------------------------------------------------------- 142 // operator= operator =(const SRI & right)143 SRI& SRI::operator=(const SRI& right) 144 throw() 145 { 146 R = right.R; 147 Z = right.Z; 148 names = right.names; 149 return *this; 150 } 151 152 // --------------------------------------------------------------------------- 153 // modify SRIs 154 // -------------------------------------------------------------------------------- 155 // Permute the SRI elements to match the input Namelist, which may differ with 156 // the SRI Namelist by AT MOST A PERMUTATION, throw if this is not true. permute(const Namelist & nl)157 void SRI::permute(const Namelist& nl) 158 { 159 if(identical(names,nl)) return; 160 if(names != nl) { 161 MatrixException me("Invalid input: Namelists must be == to w/in permute"); 162 GPSTK_THROW(me); 163 } 164 165 try { 166 const unsigned int n(R.rows()); 167 unsigned int i,j; 168 // build a permutation matrix 169 Matrix<double> P(n,n,0.0); 170 for(i=0; i<n; i++) { 171 j = nl.index(names.getName(i)); 172 P(j,i) = 1; 173 } 174 175 // inverse of P is transpose of P 176 retriangularize(R*transpose(P), Z); 177 names = nl; 178 } 179 catch(MatrixException& me) { 180 GPSTK_RETHROW(me); 181 } 182 catch(VectorException& ve) { 183 GPSTK_RETHROW(ve); 184 } 185 } 186 187 // -------------------------------------------------------------------------------- 188 // Split this SRI (call it S) into two others, S1 and Sleft, where S1 has 189 // a Namelist identical to the input Namelist (NL); set *this = S1 at the 190 // end. NL must be a non-empty subset of names, and (names ^ NL) also must 191 // be non-empty; throw MatrixException if this is not true. The second 192 // output SRI, Sleft, will have the same names as S, but perhaps permuted. 193 // 194 // The routine works by first permuting S so that its Namelist if of the 195 // form {N2,NL}, where N2 = (names ^ NL); this is possible only if NL is 196 // a non-trivial subset of names. Then, the rows of S (rows of R and elements 197 // of Z) naturally separate into the two component SRIs, with zeros in the 198 // elements of the first SRI which correspond to N2, and those in Sleft 199 // which correspond to NL. 200 // 201 // Example: S.name = A B C D E F G and NL = D E F G. 202 // (Obviously, S may be permuted into such an order whenever this is needed.) 203 // Note that here the R,Z pair is written in a format reminiscent of the 204 // set of equations implied by R*X=Z, i.e. 1A+2B+3C+4D+5E+6F+7G=a, etc. 205 // 206 // S (R Z) = S1 + Sleft 207 // with names NL names 208 // A B C D E F G . . . D E F G A B C D E F G 209 // - - - - - - - - - - - - - - - - - - - - - - - - 210 // 1 2 3 4 5 6 7 a = . . . . . . . . + 1 2 3 4 5 6 7 a 211 // 8 9 1 2 3 4 b . . . . . . . 8 9 1 2 3 4 b 212 // 5 6 7 8 9 c . . . . . . 5 6 7 8 9 c 213 // 1 2 3 4 d 1 2 3 4 d . . . . d 214 // 5 6 7 e 5 6 7 e . . . e 215 // 8 9 f 8 9 f . . f 216 // 1 g 1 g . g 217 // 218 // where "." denotes a zero. The split is simply separating the linear 219 // equations which make up R*X=Z into two groups; because of the ordering, 220 // one of the groups of equations (S1) depends only on a particular subset 221 // of the elements of the state vector, i.e. the elements labeled by the 222 // Namelist NL. 223 // 224 // The equation shown here is an information equation; if the two SRIs S1 225 // and Sleft were merged again, none of the information would be lost. 226 // Note that S1 has no dependence on A B C (hence the .'s), and therefore 227 // its size can be reduced. However Sleft still depends on the full names 228 // Namelist. Sleft is necessarily singular, but S1 is not. 229 // 230 // Note that the SRI contains information about both the solution and 231 // the covariance, i.e. state and noise, and therefore one must be very careful 232 // in interpreting the results of split and merge (operator+=). [Be especially 233 // careful about the idea that a merge might be reversible with a split() or 234 // vice-versa - strictly this is never possible unless the Namelists are 235 // mutually exclusive - two separate problems.] 236 // 237 // For example, suppose two different SRI's, which have some elements in common, 238 // are merged. The combined SRI will have more information (it can't have less) 239 // about the common elements, and therefore the solution will be 'better' 240 // (assuming the underlying model equations for those elements are identical). 241 // However the noises will also be combined, and the results you get might be 242 // surprising. Also, note that if you then split the combined SRI again, the 243 // solution won't change but the noises will be very different; in particular 244 // the new split part will take all the information with it, so the common states 245 // will have lower noise than they did in the original SRI. 246 // See the test program tsri.cpp 247 // split(const Namelist & NL,SRI & Sleft)248 void SRI::split(const Namelist& NL, SRI& Sleft) 249 { 250 try { 251 Sleft = SRI(0); 252 unsigned int n,m; 253 n = NL.size(); 254 m = names.size(); 255 if(n >= m) { 256 MatrixException me("split: Input Namelist must be a subset of this one"); 257 GPSTK_THROW(me); 258 } 259 260 unsigned int i,j; 261 // copy names and permute it so that its end matches NL 262 Namelist N0(names); 263 for(i=1; i<=n; i++) { // loop (backwards) over names in NL 264 for(j=1; j<=m; j++) { // search (backwards) in NO for a match 265 if(NL.labels[n-i] == N0.labels[m-j]) { // if found a match 266 N0.swap(m-i,m-j); // then move matching name to end 267 break; // and go on to next name in NL 268 } 269 } 270 if(j > m) { 271 MatrixException me("split: Input Namelist is not non-trivial subset"); 272 GPSTK_THROW(me); 273 } 274 } 275 276 // copy *this into Sleft, then do the permutation 277 Sleft = *this; 278 Sleft.permute(N0); 279 280 // copy parts of Sleft into S1, and then zero out those parts of Sleft 281 SRI S1(NL); 282 S1.R = Matrix<double>(Sleft.R,m-n,m-n,n,n); 283 S1.Z.resize(n); 284 for(i=0; i<n; i++) S1.Z(i) = Sleft.Z(m-n+i); 285 for(i=m-n; i<m; i++) Sleft.zeroOne(i); 286 287 *this = S1; 288 } 289 catch(MatrixException& me) { 290 GPSTK_RETHROW(me); 291 } 292 catch(VectorException& ve) { 293 GPSTK_RETHROW(ve); 294 } 295 } 296 297 // -------------------------------------------------------------------------------- 298 // extend this SRI to include the given Namelist, with no added information; 299 // names in the input namelist which are not unique are ignored. operator +=(const Namelist & NL)300 SRI& SRI::operator+=(const Namelist& NL) 301 { 302 try { 303 Namelist B(names); 304 // NB assume that Namelist::operator|=() adds at the _end_ 305 // NB if there are duplicate names, |= will not add them 306 B |= NL; 307 // NB assume that this zeros A.R and A.Z 308 SRI A(B); 309 // should do this with slices.. 310 // copy into the new SRI 311 for(unsigned int i=0; i<R.rows(); i++) { 312 A.Z(i) = Z(i); 313 for(unsigned int j=0; j<R.cols(); j++) A.R(i,j) = R(i,j); 314 } 315 *this = A; 316 return *this; 317 } 318 catch(MatrixException& me) { 319 GPSTK_RETHROW(me); 320 } 321 catch(VectorException& ve) { 322 GPSTK_RETHROW(ve); 323 } 324 } 325 326 // -------------------------------------------------------------------------------- 327 // reshape this SRI to match the input Namelist, by calling other member 328 // functions, including split(), operator+() and permute() 329 // Given this SRI and a new Namelist NL, if NL does not match names, 330 // transform names to match it, using (1) drop elements (this is probably 331 // optional - you can always keep 'dead' elements), (2) add new elements 332 // (with zero information), and (3) permute to match NL. reshape(const Namelist & NL)333 void SRI::reshape(const Namelist& NL) 334 { 335 try { 336 if(names == NL) return; 337 Namelist keep(names); 338 keep &= NL; // keep only those in both names and NL 339 //Namelist drop(names); // (drop is unneeded - split would do it) 340 //drop ^= keep; // lose those in names but not in keep 341 Namelist add(NL); 342 add ^= keep; // add those in NL but not in keep 343 SRI Sdrop; // make a new SRI to hold the losers 344 // would like to allow caller access to Sdrop.. 345 split(keep,Sdrop); // split off only the losers 346 // NB names = drop | keep; drop & keep is empty 347 *this += add; // add the new ones 348 this->permute(NL); // permute it to match NL 349 } 350 catch(MatrixException& me) { 351 GPSTK_RETHROW(me); 352 } 353 catch(VectorException& ve) { 354 GPSTK_RETHROW(ve); 355 } 356 } 357 358 // -------------------------------------------------------------------------------- 359 // append an SRI onto this SRI. Similar to opertor+= but simpler; input SRI is 360 // simply appended, first using operator+=(Namelist), then filling the new portions 361 // of R and Z, all without final Householder transformation of result. 362 // Do not allow a name that is already present to be added: throw. append(const SRI & S)363 SRI& SRI::append(const SRI& S) 364 { 365 try { 366 // do not allow duplicates 367 if((names & S.names).size() > 0) { 368 Exception e("Cannot append duplicate names"); 369 GPSTK_THROW(e); 370 } 371 372 // append to names at the end, and to R Z, zero filling 373 const size_t I(names.size()); 374 *this += S.names; 375 376 // just in case...to avoid overflow in loop below 377 if(I+S.names.size() != names.size()) { 378 Exception e("Append failed"); 379 GPSTK_THROW(e); 380 } 381 382 // loop over new names, copying data from input into the new SRI 383 for(size_t i=0; i<S.names.size(); i++) { 384 Z(I+i) = S.Z(i); 385 for(size_t j=0; j<S.names.size(); j++) 386 R(I+i,I+j) = S.R(i,j); 387 } 388 389 return *this; 390 } 391 catch(MatrixException& me) { 392 GPSTK_RETHROW(me); 393 } 394 catch(VectorException& ve) { 395 GPSTK_RETHROW(ve); 396 } 397 } 398 399 // -------------------------------------------------------------------------------- 400 // merge this SRI with the given input SRI. ? should this be operator&=() ? 401 // NB may reorder the names in the resulting Namelist. operator +=(const SRI & S)402 SRI& SRI::operator+=(const SRI& S) 403 { 404 try { 405 Namelist all(names); 406 all |= S.names; // assumes Namelist::op|= adds unique S.names to _end_ 407 408 //all.sort(); // TEMP - for testing with old version 409 410 // stack the (R|Z)'s from both in one matrix; 411 // all determines the columns, plus last column is for Z 412 unsigned int i,j,n,m,sm; 413 n = all.labels.size(); 414 m = R.rows(); 415 sm = S.R.rows(); 416 Matrix<double> A(m+sm,n+1,0.0); 417 418 // copy R into A, permuting columns as names differs from all 419 // loop over columns of R; do Z at the same time using j=row 420 for(j=0; j<m; j++) { 421 // find where this column of R goes in A 422 // (should never throw..) 423 int k = all.index(names.labels[j]); 424 if(k == -1) { 425 MatrixException me("Algorithm error 1"); 426 GPSTK_THROW(me); 427 } 428 429 // copy this col of R into A (R is UT) 430 for(i=0; i<=j; i++) A(i,k) = R(i,j); 431 // also the jth element of Z 432 A(j,n) = Z(j); 433 } 434 435 // now do the same for S, but put S.R|S.Z below R|Z 436 for(j=0; j<sm; j++) { 437 int k = all.index(S.names.labels[j]); 438 if(k == -1) { 439 MatrixException me("Algorithm error 2"); 440 GPSTK_THROW(me); 441 } 442 for(i=0; i<=j; i++) A(m+i,k) = S.R(i,j); 443 A(m+j,n) = S.Z(j); 444 } 445 446 // now triangularize A and pull out the new R and Z 447 //DONT - dimensions change - retriangularize(A); 448 Householder<double> HA; 449 HA(A); 450 // submatrix args are matrix,toprow,topcol,numrows,numcols 451 R = Matrix<double>(HA.A,0,0,n,n); 452 Z = Vector<double>(HA.A.colCopy(n)); 453 Z.resize(n); 454 names = all; 455 456 return *this; 457 } 458 catch(MatrixException& me) { 459 GPSTK_RETHROW(me); 460 } 461 catch(VectorException& ve) { 462 GPSTK_RETHROW(ve); 463 } 464 } 465 466 // -------------------------------------------------------------------------------- 467 // merge two SRIs to produce a third. ? should this be operator&() ? operator +(const SRI & Sleft,const SRI & Sright)468 SRI operator+(const SRI& Sleft, 469 const SRI& Sright) 470 { 471 try { 472 SRI S(Sleft); 473 S += Sright; 474 return S; 475 } 476 catch(MatrixException& me) { 477 GPSTK_RETHROW(me); 478 } 479 catch(VectorException& ve) { 480 GPSTK_RETHROW(ve); 481 } 482 } 483 484 // -------------------------------------------------------------------------------- 485 // Zero out the nth row of R and the nth element of Z, removing all 486 // information about that element. zeroOne(const unsigned int n)487 void SRI::zeroOne(const unsigned int n) 488 throw() 489 { 490 if(n >= R.rows()) 491 return; 492 493 for(unsigned int j=n; j<R.cols(); j++) 494 R(n,j) = 0.0; 495 Z(n) = 0.0; 496 } 497 498 // -------------------------------------------------------------------------------- 499 // Zero out all the first n rows of R and elements of Z, removing all 500 // information about those elements. Default value of the input is 0, 501 // meaning zero out the entire SRI. zeroAll(const unsigned int n)502 void SRI::zeroAll(const unsigned int n) 503 throw() 504 { 505 if(n <= 0) { 506 R = 0.0; 507 Z = 0.0; 508 return; 509 } 510 511 if(n >= int(R.rows())) 512 return; 513 514 for(unsigned int i=0; i<n; i++) { 515 for(unsigned int j=i; j<R.cols(); j++) 516 R(i,j) = 0.0; 517 Z(i) = 0.0; 518 } 519 } 520 521 // -------------------------------------------------------------------------------- 522 // Shift the state vector by a constant vector X0; does not change information 523 // i.e. let R * X = Z => R * (X-X0) = Z' 524 // throw on invalid input dimension shift(const Vector<double> & X0)525 void SRI::shift(const Vector<double>& X0) 526 { 527 if(X0.size() != R.cols()) { 528 MatrixException me("Invalid input dimension: SRI has dimension " 529 + asString<int>(R.rows()) + " while input has length " 530 + asString<int>(X0.size()) 531 ); 532 GPSTK_THROW(me); 533 } 534 Z = Z - R * X0; 535 } 536 537 // -------------------------------------------------------------------------------- 538 // Shift the SRI state vector (Z) by a constant vector Z0; 539 // does not change information. i.e. let Z => Z-Z0 540 // throw on invalid input dimension shiftZ(const Vector<double> & Z0)541 void SRI::shiftZ(const Vector<double>& Z0) 542 { 543 if(Z0.size() != R.cols()) { 544 MatrixException me("Invalid input dimension: SRI has dimension " 545 + asString<int>(R.rows()) + " while input has length " 546 + asString<int>(Z0.size()) 547 ); 548 GPSTK_THROW(me); 549 } 550 Z = Z - Z0; 551 } 552 553 // -------------------------------------------------------------------------------- 554 // Retriangularize the SRI, when it has been modified to a non-UT matrix 555 // (e.g. by transform()). Given the matrix A=[R||Z], apply HH transforms 556 // to retriagularize it and pull out new R and Z. 557 // param A Matrix<double> which is [R || Z] to be retriangularizied. 558 // throw if dimensions are wrong. retriangularize(const Matrix<double> & A)559 void SRI::retriangularize(const Matrix<double>& A) 560 { 561 const unsigned int n(R.rows()); 562 if(A.rows() != n || A.cols() != n+1) { 563 MatrixException me("Invalid input dimension: SRI has dimension " 564 + asString<int>(n) + " while input has dimension " 565 + asString<int>(A.rows()) + "x" + asString<int>(A.cols())); 566 GPSTK_THROW(me); 567 } 568 569 Householder<double> HA; 570 HA(A); 571 // submatrix args are matrix,toprow,topcol,numrows,numcols 572 R = Matrix<double>(HA.A,0,0,n,n); 573 Z = Vector<double>(HA.A.colCopy(n)); 574 // NB names cannot be changed - caller must do it. 575 } 576 577 // Retriangularize the SRI, that is assuming R has been modified to a non-UT 578 // matrix (e.g. by transform()). Given RR and ZZ, apply HH transforms to 579 // retriangularize, and store as R,Z. 580 // @param R Matrix<double> input the modified (non-UT) R 581 // @param Z Vector<double> input the (potentially) modified Z 582 // @throw if dimensions are wrong. retriangularize(Matrix<double> RR,Vector<double> ZZ)583 void SRI::retriangularize(Matrix<double> RR, Vector<double> ZZ) 584 { 585 const unsigned int n(R.rows()); 586 if(RR.rows() != n || RR.cols() != n || ZZ.size() != n) { 587 MatrixException me("Invalid input dimension: SRI has dimension " 588 + asString<int>(n) + " while input has dimension " 589 + asString<int>(RR.rows()) + "x" + asString<int>(RR.cols()) 590 + " and " + asString<int>(ZZ.size())); 591 GPSTK_THROW(me); 592 } 593 594 Matrix<double> A(RR || ZZ); 595 retriangularize(A); 596 } 597 598 // -------------------------------------------------------------------------------- 599 // Apply transformation matrix T to the SRI; i.e. X -> T*X, Cov -> T*Cov*T^T 600 // X' = T*X, C' = T*C*T^T = (TR^-1)(TR^-1)^T = R'^-1*R'^-T; 601 // but then Z' = R'*X' = R'TX = RT^-1TX = RX = Z 602 // This implies right multiplying R by inverse(T), which is the input, and then 603 // using HH to retriangularize. 604 // Input is the _inverse_ of the transformation. 605 // SRI.names is set to input Namelist 606 // throw MatrixException if input dimensions are wrong. transform(const Matrix<double> & invT,const Namelist & NL)607 void SRI::transform(const Matrix<double>& invT, const Namelist& NL) 608 { 609 const unsigned int n(R.rows()); 610 if(invT.rows() != n || invT.cols() != n) { 611 MatrixException me("Invalid input dimension: SRI has dimension " 612 + asString<int>(n) + " while invT has dimension " 613 + asString<int>(invT.rows()) + "x" 614 + asString<int>(invT.cols())); 615 GPSTK_THROW(me); 616 } 617 618 R = R*invT; 619 retriangularize(R,Z); 620 names = NL; 621 } 622 623 // -------------------------------------------------------------------------------- 624 // Decrease the information in this SRI for, or 'Q bump', the element 625 // with the input index. This means that the uncertainty and the state 626 // element given by the index are divided by the input factor q; the 627 // default input is zero, which means zero out the information (q = infinite). 628 // A Q bump by factor q is equivalent to 'de-weighting' the element by q. 629 // No effect if input index is out of range. 630 // 631 // Use a specialized form of the time update, with Phi=unity, G(N x 1) = 0 632 // except 1 for the element (in) getting bumped, and Rw(1 x 1) = 1 / q. 633 // Note that this bump of the covariance for element k results in 634 // Cov(k,k) += q (plus, not times!). 635 // if q is 0, replace q with 1/q, i.e. lose all information, covariance 636 // goes singular; this is equivalent to (1) permute so that the 'in' 637 // element is first, (2) zero out the first row of R and the first element 638 // of Z, (3) permute the first row back to in. Qbump(const unsigned int & in,const double & q)639 void SRI::Qbump(const unsigned int& in, 640 const double& q) 641 { 642 try { 643 if(in >= R.rows()) return; 644 double factor=0.0; 645 if(q != 0.0) factor=1.0/q; 646 647 unsigned int ns=1,i,j,n=R.rows(); 648 649 Matrix<double> A(n+ns,n+ns+1,0.0), G(n,ns,0.0); 650 A(0,0) = factor; // Rw, dimension ns x ns = 1 x 1 651 G(in,0) = 1.0; 652 G = R * G; // R*Phi*G 653 for(i=0; i<n; i++) { 654 A(ns+i,0) = -G(i,0); // A = Rw 0 zw=0 655 for(j=0; j<n; j++) // -R*Phi*G R*Phi Z 656 if(i<=j) A(ns+i,ns+j) = R(i,j); // 657 A(ns+i,ns+n) = Z(i); 658 } 659 660 // triangularize and pull out the new R and Z 661 // NB do not call retriangularize() here! 662 Householder<double> HA; // A = Rw Rwx zw 663 HA(A); // 0 R z 664 R = Matrix<double>(HA.A,ns,ns,n,n); 665 Vector<double> T=HA.A.colCopy(ns+n); 666 Z = Vector<double>(T,ns,n); 667 } 668 catch(MatrixException& me) { 669 GPSTK_RETHROW(me); 670 } 671 catch(VectorException& ve) { 672 GPSTK_RETHROW(ve); 673 } 674 } 675 676 // -------------------------------------------------------------------------------- 677 // Fix one state element (with the given name) to a given value, and set the 678 // information for that element (== 1/sigma) to a given value. 679 // No effect if name is not found 680 // param name string labeling the state in Namelist names 681 // param value to which the state element is fixed 682 // param sigma (1/information) assigned to the element stateFix(const string & name,const double & value,const double & sigma,bool restore)683 void SRI::stateFix(const string& name, 684 const double& value, const double& sigma, bool restore) 685 { 686 int index = names.index(name); 687 if(index == -1) return; 688 stateFix((unsigned int)(index), value, sigma, restore); 689 } 690 691 // -------------------------------------------------------------------------------- 692 // Fix one state element (at the given index) to a given value, and set the 693 // information for that element (== 1/sigma) to a given value. 694 // No effect if index is out of range. 695 // param index of the element to fix 696 // param value to which the state element is fixed 697 // param sigma (1/information) assigned to the element stateFix(const unsigned int & index,const double & value,const double & sigma,bool restore)698 void SRI::stateFix(const unsigned int& index, 699 const double& value, const double& sigma, bool restore) 700 { 701 if(index >= names.size()) GPSTK_THROW(Exception("Index out of range")); 702 const unsigned int N(names.size()); 703 704 // make a namelist with the desired element last 705 Namelist NL(names); 706 if(index != N-1) { 707 NL.swap(index, N-1); 708 permute(NL); 709 } 710 711 // fix the element and information 712 Z(N-1) = value/sigma; 713 R(N-1,N-1) = 1.0/sigma; 714 715 // permute back, if the caller wants 716 if(restore && index != N-1) { 717 NL = names; 718 NL.swap(index, N-1); 719 permute(NL); 720 } 721 } 722 723 // -------------------------------------------------------------------------------- 724 // Fix the state element with the input index to the input value, and 725 // collapse the SRI by removing that element. 726 // No effect if index is out of range. stateFixAndRemove(const unsigned int & in,const double & bias)727 void SRI::stateFixAndRemove(const unsigned int& in, const double& bias) 728 { 729 if(in >= R.rows()) return; 730 731 try { 732 unsigned int i,j,ii,jj,n=R.rows(); 733 Vector<double> Znew(n-1,0.0); 734 Matrix<double> Rnew(n-1,n-1,0.0); 735 // move the X(in) terms to the data vector on the RHS 736 for(i=0; i<in; i++) Z(i) -= R(i,in)*bias; 737 // remove row/col in and collapse 738 for(i=0,ii=0; i<n; i++) { 739 if(i == in) continue; 740 Znew(ii) = Z(i); 741 for(j=i,jj=ii; j<n; j++) { 742 if(j == in) continue; 743 Rnew(ii,jj) = R(i,j); 744 jj++; 745 } 746 ii++; 747 } 748 R = Rnew; 749 Z = Znew; 750 names -= names.labels[in]; 751 } 752 catch(MatrixException& me) { GPSTK_RETHROW(me); } 753 catch(VectorException& ve) { GPSTK_RETHROW(ve); } 754 } 755 756 // -------------------------------------------------------------------------------- 757 // Vector version of stateFixAndRemove with several states given in a Namelist. stateFixAndRemove(const Namelist & dropNL,const Vector<double> & values_in)758 void SRI::stateFixAndRemove(const Namelist& dropNL,const Vector<double>& values_in) 759 { 760 try { 761 if(dropNL.size() != values_in.size()) { 762 VectorException e("Input has inconsistent lengths"); 763 GPSTK_THROW(e); 764 } 765 /* 766 // build a vector of indexes to keep 767 int i,j; 768 vector<int> indexes; 769 for(i=0; i<names.size(); i++) { 770 j = dropNL.index(names.getName(i)); // index in dropNL, this state 771 if(j == -1) indexes.push_back(i);// not found in dropNL, so keep 772 } 773 774 const int n=indexes.size(); // new dimension 775 if(n == 0) { 776 Exception e("Cannot drop all states"); 777 GPSTK_THROW(e); 778 } 779 780 Vector<double> X,newX(n); 781 Matrix<double> C,newC(n,n); 782 Namelist newNL; 783 784 double big,small; 785 getStateAndCovariance(X,C,&small,&big); 786 787 for(i=0; i<n; i++) { 788 newX(i) = X(indexes[i]); 789 for(j=0; j<n; j++) newC(i,j) = C(indexes[i],indexes[j]); 790 newNL += names.getName(indexes[i]); 791 } 792 793 R = Matrix<double>(inverseUT(upperCholesky(newC))); 794 Z = Vector<double>(R*newX); 795 names = newNL; 796 */ 797 size_t i,j,k; 798 // create a vector of indexes and corresponding values 799 vector<int> indx; 800 vector<double> value; 801 for(i=0; i<dropNL.size(); i++) { 802 int in = names.index(dropNL.getName(i)); // in must be allowed to be -1 803 if(in > -1) { 804 indx.push_back(in); 805 value.push_back(values_in(i)); 806 } 807 //else nothing happens 808 } 809 const unsigned int m = indx.size(); 810 const unsigned int n = R.rows(); 811 if(m == 0) return; 812 if(m == n) { 813 *this = SRI(0); 814 return; 815 } 816 // move the X(in) terms to the data vector on the RHS 817 for(k=0; k<m; k++) 818 for(i=0; i<indx[k]; i++) 819 Z(i) -= R(i,indx[k])*value[k]; 820 821 // first remove the rows in indx 822 bool skip; 823 Vector<double> Ztmp(n-m,0.0); 824 Matrix<double> Rtmp(n-m,n,0.0); 825 for(i=0,k=0; i<n; i++) { 826 skip = false; 827 for(j=0; j<m; j++) if((int)i == indx[j]) { skip=true; break; } 828 if(skip) continue; // skip row to be dropped 829 830 Ztmp(k) = Z(i); 831 for(j=i; j<n; j++) Rtmp(k,j) = R(i,j); 832 k++; 833 } 834 835 // Z is now done 836 Z = Ztmp; 837 838 // now remove columns in indx 839 R = Matrix<double>(n-m,n-m,0.0); 840 for(j=0,k=0; j<n; j++) { 841 skip = false; 842 for(i=0; i<m; i++) if((int)j == indx[i]) { skip=true; break; } 843 if(skip) continue; // skip col to be dropped 844 845 for(i=0; i<=j; i++) R(i,k) = Rtmp(i,j); 846 k++; 847 } 848 849 // remove the names 850 for(k=0; k<dropNL.size(); k++) { 851 std::string name(dropNL.getName(k)); 852 names -= name; 853 } 854 } 855 catch(MatrixException& me) { GPSTK_RETHROW(me); } 856 catch(VectorException& ve) { GPSTK_RETHROW(ve); } 857 } 858 859 //--------------------------------------------------------------------------------- 860 // Add a priori or 'constraint' information 861 // Prefer addAPrioriInformation(inverse(Cov), inverse(Cov)*X); addAPriori(const Matrix<double> & Cov,const Vector<double> & X)862 void SRI::addAPriori(const Matrix<double>& Cov, const Vector<double>& X) 863 { 864 if(Cov.rows() != Cov.cols() || Cov.rows() != R.rows() || X.size() != R.rows()) { 865 MatrixException me("Invalid input dimensions:\n SRI has dimension " 866 + asString<int>(R.rows()) + ",\n while input is Cov(" 867 + asString<int>(Cov.rows()) + "x" 868 + asString<int>(Cov.cols()) + ") and X(" 869 + asString<int>(X.size()) + ")."); 870 GPSTK_THROW(me); 871 } 872 873 try { 874 Matrix<double> InvCov = inverseLUD(Cov); 875 addAPrioriInformation(InvCov, X); 876 } 877 catch(MatrixException& me) { 878 GPSTK_THROW(me); 879 } 880 } 881 882 // -------------------------------------------------------------------------------- addAPrioriInformation(const Matrix<double> & InvCov,const Vector<double> & X)883 void SRI::addAPrioriInformation(const Matrix<double>& InvCov, 884 const Vector<double>& X) 885 { 886 if(InvCov.rows() != InvCov.cols() || InvCov.rows() != R.rows() 887 || X.size() != R.rows()) { 888 MatrixException me("Invalid input dimensions:\n SRI has dimension " 889 + asString<int>(R.rows()) + ",\n while input is InvCov(" 890 + asString<int>(InvCov.rows()) + "x" 891 + asString<int>(InvCov.cols()) + ") and X(" 892 + asString<int>(X.size()) + ")." 893 ); 894 GPSTK_THROW(me); 895 } 896 897 try { 898 Matrix<double> L(lowerCholesky(InvCov)); 899 Matrix<double> apR(transpose(L)); // R = UT(inv(Cov)) 900 Vector<double> apZ(apR*X); // Z = R*X 901 SrifMU(R, Z, apR, apZ); 902 } 903 catch(MatrixException& me) { 904 GPSTK_THROW(me); 905 } 906 } 907 908 // -------------------------------------------------------------------------------- getConditionNumber(double & small,double & big) const909 void SRI::getConditionNumber(double& small, double& big) const 910 { 911 try { 912 small = big = 0.0; 913 const int n=R.rows(); 914 if(n == 0) return; 915 SVD<double> svd; 916 svd(R); 917 svd.sort(true); // now the last s.v. is the smallest 918 small = svd.S(n-1); 919 big = svd.S(0); 920 } 921 catch(MatrixException& me) { 922 me.addText("Called by getConditionNumber"); 923 GPSTK_RETHROW(me); 924 } 925 } 926 927 // -------------------------------------------------------------------------------- 928 // Compute state without computing covariance. Use the fact that R is upper 929 // triangular. Throw if and when a zero diagonal element is found; values at larger 930 // index are still valid. On output *ptr is the largest singular index getState(Vector<double> & X,int * ptr) const931 void SRI::getState(Vector<double>& X, int *ptr) const 932 { 933 const int n=Z.size(); 934 X = Vector<double>(n,0.0); 935 if(ptr) *ptr = -1; 936 if(n == 0) return; 937 int i,j; 938 for(i=n-1; i>=0; i--) { // loop over rows, in reverse order 939 if(R(i,i) == 0.0) { 940 if(ptr) *ptr = i; 941 MatrixException me("Singular matrix; zero diagonal element at index " 942 + asString<int>(i)); 943 GPSTK_THROW(me); 944 } 945 double sum=Z(i); 946 for(j=i+1; j<n; j++) // sum over columns to right of diagonal 947 sum -= R(i,j)*X(j); 948 X(i) = sum/R(i,i); 949 } 950 } 951 952 // -------------------------------------------------------------------------------- 953 // get the state X and the covariance matrix C of the state, where 954 // C = transpose(inverse(R))*inverse(R) and X = inverse(R) * Z. 955 // Throws MatrixException if R is singular. getStateAndCovariance(Vector<double> & X,Matrix<double> & C,double * ptrSmall,double * ptrBig) const956 void SRI::getStateAndCovariance(Vector<double>& X, 957 Matrix<double>& C, 958 double *ptrSmall, 959 double *ptrBig) const 960 { 961 try { 962 double small,big; 963 Matrix<double> invR(inverseUT(R,&small,&big)); 964 if(ptrSmall) *ptrSmall = small; 965 if(ptrBig) *ptrBig = big; 966 967 //cout << " small is " << scientific << setprecision(3) << small 968 // << " and big is " << big; 969 //cout << " exponent is " << ::log(big) - ::log(small) << endl; 970 // how best to test? 971 // ::log(big) - ::log(small) + 1 >= numeric_limits<double>::max_exponent 972 if(small <= 10*numeric_limits<double>::epsilon()) { 973 MatrixException me("Singular matrix: condition number is " 974 + asString<double>(big) + " / " + asString<double>(small)); 975 GPSTK_THROW(me); 976 } 977 978 C = UTtimesTranspose(invR); 979 X = invR * Z; 980 } 981 catch(MatrixException& me) { 982 GPSTK_RETHROW(me); 983 } 984 catch(VectorException& ve) { 985 GPSTK_RETHROW(ve); 986 } 987 } 988 989 //--------------------------------------------------------------------------------- 990 // output operator operator <<(ostream & os,const SRI & S)991 ostream& operator<<(ostream& os, const SRI& S) 992 { 993 Namelist NLR(S.names); 994 Namelist NLC(S.names); 995 NLC += string("State"); 996 Matrix<double> A; 997 A = S.R || S.Z; 998 LabeledMatrix LM(NLR,NLC,A); 999 1000 ios_base::fmtflags flags = os.flags(); 1001 if(flags & ios_base::scientific) LM.scientific(); 1002 LM.setw(os.width()); 1003 LM.setprecision(os.precision()); 1004 LM.clean(); 1005 //LM.message("NL"); 1006 //LM.linetag("tag"); 1007 1008 os << LM; 1009 1010 return os; 1011 } 1012 1013 } // end namespace gpstk 1014 1015 //------------------------------------------------------------------------------------ 1016 //------------------------------------------------------------------------------------ 1017