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 /// @file SRIleastSquares.hpp 40 /// Include file defining class SRIleastSquares, which inherits class SRI and 41 /// implements a general least squares algorithm that includes linear or linearized 42 /// problems, weighting, robust estimation, and sequential estimation. 43 44 //------------------------------------------------------------------------------------ 45 #ifndef CLASS_SRI_LEAST_SQUARES_INCLUDE 46 #define CLASS_SRI_LEAST_SQUARES_INCLUDE 47 48 //------------------------------------------------------------------------------------ 49 // system 50 #include <ostream> 51 // GPSTk 52 #include "Vector.hpp" 53 #include "Matrix.hpp" 54 #include "SRI.hpp" 55 56 namespace gpstk { 57 58 //------------------------------------------------------------------------------------ 59 /// class SRIleastSquares inherits SRI and implements a general least squares 60 /// algorithm using SRI, including weighted, linear or linearized, robust and/or 61 /// sequential algorithms. 62 /// 63 /// At any point the state X and covariance P are related to the SRI by 64 /// X = inverse(R) * z , P = inverse(R) * inverse(transpose(R)), or 65 /// R = upper triangular square root (Cholesky decomposition) of the inverse of P, 66 /// and z = R * X. 67 class SRIleastSquares : public SRI { 68 public: 69 /// empty constructor 70 SRIleastSquares(void) throw(); 71 72 /// constructor given the dimension N. 73 /// @param N dimension of the SRIleastSquares. 74 SRIleastSquares(const unsigned int N) 75 throw(); 76 77 /// constructor given a Namelist; its dimension determines the SRI dimension. 78 /// @param NL Namelist for the SRIleastSquares. 79 SRIleastSquares(const Namelist& NL) 80 throw(); 81 82 /// explicit constructor - throw if the dimensions are inconsistent. 83 /// @param R Initial information matrix, an upper triangular matrix of dim N. 84 /// @param Z Initial information data vector, of length N. 85 /// @param NL Namelist for the SRIleastSquares, also of length N. 86 /// @throw MatrixException if dimensions are not consistent. 87 SRIleastSquares(const Matrix<double>& R, 88 const Vector<double>& Z, 89 const Namelist& NL); 90 91 /// copy constructor 92 /// @param right SRIleastSquares to be copied SRIleastSquares(const SRIleastSquares & right)93 SRIleastSquares(const SRIleastSquares& right) 94 throw() 95 { *this = right; } 96 97 /// operator= 98 /// @param right SRIleastSquares to be copied 99 SRIleastSquares& operator=(const SRIleastSquares& right) 100 throw(); 101 102 /// A general least squares update, NOT the SRIF (Kalman) measurement update. 103 /// Given data and measurement covariance, compute a solution and 104 /// covariance using the appropriate least squares algorithm. 105 /// @param D Data vector, length M 106 /// Input: raw data 107 /// Output: post-fit residuals 108 /// @param X Solution vector, length N 109 /// Input: nominal solution X0 (zero when doLinearized is false) 110 /// Output: final solution 111 /// @param Cov Covariance matrix, dimension (N,N) 112 /// Input: (If doWeight is true) inverse measurement covariance 113 /// or weight matrix(M,M) 114 /// Output: Solution covariance matrix (N,N) 115 /// @param LSF Pointer to a function which is used to define the equation 116 /// to be solved. 117 /// LSF arguments are: 118 /// X Nominal solution (input) 119 /// f Values of the equation f(X) (length M) (output) 120 /// P Partials matrix df/dX evaluated at X (dimension M,N) (output) 121 /// When doLinearize is false, LSF ignores X and returns the (constant) 122 /// partials matrix P and zero for f(X). 123 /// @throw MatrixException if the input is inconsistent 124 /// Return values: 0 ok 125 /// -1 Problem is underdetermined (M<N) // TD -- naturalized sol? 126 /// -2 Problem is singular 127 /// -3 Algorithm failed to converge 128 /// -4 Algorithm diverged 129 int dataUpdate(Vector<double>& D, 130 Vector<double>& X, 131 Matrix<double>& Cov, 132 void (LSF)(Vector<double>& X, 133 Vector<double>& f, 134 Matrix<double>& P) 135 ); 136 137 /// output operator 138 friend std::ostream& operator<<(std::ostream& s, 139 const SRIleastSquares& srif); 140 141 /// remove all stored information by setting the SRI to zero 142 /// (does not re-dimension). 143 void zeroAll(void); 144 145 /// Return true if the solution is valid, i.e. if the problem is non-singular. isValid()146 bool isValid() { return valid; } 147 148 /// reset the computation, i.e. remove all stored information, and 149 /// optionally change the dimension. If N is not input, the 150 /// dimension is not changed. 151 /// @param N new SRIleastSquares dimension (optional). 152 /// @throw Exception 153 void Reset(const int N=0); 154 155 // ------------- member functions --------------- 156 /// Get the current solution vector 157 /// @return current solution vector Solution(void)158 Vector<double> Solution(void) { return Xsave; } 159 160 /// Get the number of iterations used in last call to leastSquaresEstimation() 161 /// @return the number of iterations Iterations()162 int Iterations() { return number_iterations; } 163 164 /// Get the convergence value found in last call to leastSquaresEstimation() 165 /// @return the convergence value Convergence()166 double Convergence() { return rms_convergence; } 167 168 /// Get the condition number of the covariance matrix from last calls 169 /// to leastSquaresEstimation() (Larger means 'closer to singular' except 170 /// zero means condition number is infinite) ConditionNumber()171 double ConditionNumber() { return condition_number; } 172 173 // ------------- member data --------------- 174 /// limit on the number of iterations 175 int iterationsLimit; 176 177 /// limit on the RSS change in solution which produces success 178 double convergenceLimit; 179 180 /// upper limit on the RSS change in solution which produces an abort 181 double divergenceLimit; 182 183 /// if true, weight the equation using the inverse of covariance matrix 184 /// on input - default is false 185 bool doWeight; 186 187 /// if true, weight the equation using robust statistical techniques 188 /// - default is false 189 /// (NB robust & sequential doesn't make much sense) 190 bool doRobust; 191 192 /// if true, save information for a sequential solution - default is false 193 /// (NB robust & sequential doesn't make much sense) 194 bool doSequential; 195 196 /// if true, equation F(X)=D is non-linear, the algorithm will be iterated, 197 /// and LSF must return partials matrix and F(X). - default is false 198 bool doLinearize; 199 200 /// if true, output intermediate results 201 bool doVerbose; 202 203 private: 204 /// initialization used by constructors - leastSquaresEstimation() only defaults(void)205 void defaults(void) throw() 206 { 207 iterationsLimit = 10; 208 convergenceLimit = 1.e-8; 209 divergenceLimit = 1.e10; 210 doWeight = false; 211 doRobust = false; 212 doLinearize = false; 213 doSequential = false; 214 doVerbose = false; 215 number_iterations = number_batches = 0; 216 rms_convergence = condition_number = 0.0; 217 valid = false; 218 } 219 220 // private member data - inherits from SRI 221 // inherit SRI Information matrix, an upper triangular (square) matrix 222 //Matrix<double> R; 223 // inherit SRI state vector, of length equal to dimension (row and col) of R. 224 //Vector<double> Z; 225 // inherit SRI Namelist parallel to R and Z, labelling elements of state vector. 226 //Namelist names; 227 228 // --------- private member data ------------ 229 /// indicates if the filter is valid - set false when singular 230 bool valid; 231 232 /// current number of iterations 233 int number_iterations; 234 235 /// current number of batches seen 236 int number_batches; 237 238 /// RMS change in state, used for convergence test 239 double rms_convergence; 240 241 /// condition number, defined in inversion to get state and covariance 242 double condition_number; 243 244 /// solution X consistent with current information RX=z 245 Vector<double> Xsave; 246 247 }; // end class SRIleastSquares 248 249 } // end namespace gpstk 250 251 //------------------------------------------------------------------------------------ 252 #endif 253