SRIFilter.hpp

Go to the documentation of this file.

//==============================================================================
//
//  This file is part of GNSSTk, the ARL:UT GNSS Toolkit.
//
//  The GNSSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 3.0 of the License, or
//  any later version.
//
//  The GNSSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GNSSTk; if not, write to the Free Software Foundation,
//  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
//
//  This software was developed by Applied Research Laboratories at the
//  University of Texas at Austin.
//  Copyright 2004-2022, The Board of Regents of The University of Texas System
//
//==============================================================================
 
//==============================================================================
//
//  This software was developed by Applied Research Laboratories at the
//  University of Texas at Austin, under contract to an agency or agencies
//  within the U.S. Department of Defense. The U.S. Government retains all
//  rights to use, duplicate, distribute, disclose, or release this software.
//
//  Pursuant to DoD Directive 523024
//
//  DISTRIBUTION STATEMENT A: This software has been approved for public
//                            release, distribution is unlimited.
//
//==============================================================================
 
//------------------------------------------------------------------------------------
#ifndef CLASS_SQUAREROOT_INFORMATION_FILTER_INCLUDE
#define CLASS_SQUAREROOT_INFORMATION_FILTER_INCLUDE
 
//------------------------------------------------------------------------------------
// system
#include <ostream>
// GNSSTk
#include "Matrix.hpp"
#include "Vector.hpp"
// geomatics
#include "SRI.hpp"
#include "SparseMatrix.hpp"
 
namespace gnsstk
{
   class SRIFilter : public SRI
   {
   public:
      SRIFilter();
 
      SRIFilter(const unsigned int N);
 
      SRIFilter(const Namelist& NL);
 
      SRIFilter(const Matrix<double>& R, const Vector<double>& Z,
                const Namelist& NL);
 
      SRIFilter(const SRIFilter& right) { *this = right; }
 
      SRIFilter& operator=(const SRIFilter& right);
 
      void measurementUpdate(const Matrix<double>& H, Vector<double>& D,
                             const Matrix<double>& CM = SRINullMatrix);
 
      void measurementUpdate(const SparseMatrix<double>& H, Vector<double>& D,
                        const SparseMatrix<double>& CM = SRINullSparseMatrix);
 
      void timeUpdate(Matrix<double>& PhiInv, Matrix<double>& Rw,
                      Matrix<double>& G, Vector<double>& Zw,
                      Matrix<double>& Rwx);
 
      void smootherUpdate(Matrix<double>& Phi, Matrix<double>& Rw,
                          Matrix<double>& G, Vector<double>& Zw,
                          Matrix<double>& Rwx);
 
      static void DMsmootherUpdate(Matrix<double>& P, Vector<double>& X,
                                   Matrix<double>& Phinv, Matrix<double>& Rw,
                                   Matrix<double>& G, Vector<double>& Zw,
                                   Matrix<double>& Rwx);
 
      static void
      DMsmootherUpdateWithControl(Matrix<double>& P, Vector<double>& X,
                                  Matrix<double>& Phinv, Matrix<double>& Rw,
                                  Matrix<double>& G, Vector<double>& Zw,
                                  Matrix<double>& Rwx, Vector<double>& U);
 
      void zeroAll();
 
      void reset(const int N = 0);
 
   private:
      template <class T>
      static void SrifTU(Matrix<T>& R, Vector<T>& Z, Matrix<T>& Phi,
                         Matrix<T>& Rw, Matrix<T>& G, Vector<T>& Zw,
                         Matrix<T>& Rwx);
 
      template <class T>
      static void SrifSU(Matrix<T>& R, Vector<T>& Z, Matrix<T>& Phi,
                         Matrix<T>& Rw, Matrix<T>& G, Vector<T>& Zw,
                         Matrix<T>& Rwx);
 
      template <class T>
      static void SrifSU_DM(Matrix<T>& P, Vector<T>& X, Matrix<T>& Phinv,
                            Matrix<T>& Rw, Matrix<T>& G, Vector<T>& Zw,
                            Matrix<T>& Rwx);
 
      void defaults()
      {
         // valid = false;
      }
 
      // private member data - inherits from SRI
      // inherit SRI Information matrix, an upper triangular (square) matrix
      // Matrix<double> R;
      // inherit SRI state vector, of length equal to dimension (row and col) of
      // R.
      // Vector<double> Z;
      // inherit SRI Namelist parallel to R and Z, labelling elements of state
      // vector.
      // Namelist names;
 
      // --------- private member data ------------
      // TD how to implement valid?
      // indicates if filter is valid - set false when inversion finds
      // singularity.
      // bool valid;
 
   }; // end class SRIFilter
 
} // end namespace gnsstk
 
//------------------------------------------------------------------------------------
#endif