KalmanFilter.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 KALMAN_FILTER_INCLUDE
#define KALMAN_FILTER_INCLUDE
 
#include "Exception.hpp"
#include "Matrix.hpp"
#include "Namelist.hpp"
#include "SRIFilter.hpp"
#include "Vector.hpp"
#include "logstream.hpp"
#include <map>
#include <sstream>
#include <string>
 
namespace gnsstk
{
   class KalmanFilter
   {
   public:
         // enum to define the current filter operation, mostly for output()
      typedef enum FilterStageEnum
      {
         Unknown = 0,
         Init,
         IB1,
         IB2,
         IB3, // for "in between" meaning Interim
         TU,
         MU,
         SU,
         StageCount
      } FilterStage;
 
         // enum to define the return values for defineMeasurements()
      typedef enum KalmanMUReturnValuesEnum
      {
         Process = 0,
         ProcessThenQuit, // 1
         SkipThisEpoch,   // 2
         SkipThenQuit,    // 3
         QuitImmediately, // 4
         ReturnCount      // 5
      } KalmanReturn;
 
      // ----------------------------------------------------------------------------
      // data
   protected:
      bool doOutput;
      bool doInversions;
      bool singular;
      bool doSRISU;
      bool extended;
      bool smoother;
      bool inverted;
      bool timeReversed;
      bool dryRun;
 
      int NTU;    
      int NMU;    
      int NSU;    
      int Nstate; 
      int Nnoise; 
 
      FilterStage stage; 
      double time;       
      double nominalDT;  
      double big, small; 
      std::string KFtag; 
 
      gnsstk::Vector<double> State;   
      gnsstk::Matrix<double> Cov;     
      gnsstk::SRIFilter srif;         
                             // MU
      gnsstk::Vector<double> PFResid; 
      gnsstk::Matrix<double>
         Partials;                   
      gnsstk::Vector<double>
         Data;                       
      gnsstk::Matrix<double> MCov;    
                            // TU
      gnsstk::Vector<double> Zw;      
      gnsstk::Vector<double> Control; 
      gnsstk::Matrix<double>
         PhiInv;                     
      gnsstk::Matrix<double> G;       
      gnsstk::Matrix<double> Rw;      
                            // SU
      gnsstk::Vector<double>
         SMResid;                    
 
      typedef struct Smoother_storage_record
      {
         gnsstk::Matrix<double> Rw;
         gnsstk::Matrix<double> Rwx;
         gnsstk::Matrix<double> PhiInv;
         gnsstk::Matrix<double> G;
         gnsstk::Vector<double> Zw;
         gnsstk::Vector<double> Control;
         double Time;
      } SmootherStoreRec;
      std::map<int, SmootherStoreRec> SmootherStore;
 
   public:
      // functions
      // -----------------------------------------------------------------------------
      /* 1. Define all the 'define' functions; this constitutes the filter
         design; these get information from the user and MUST be implemented in
         the derived class. */
 
      virtual int defineInitial(double& T0, gnsstk::Vector<double>& X,
                                gnsstk::Matrix<double>& Cov) = 0;
 
      virtual KalmanReturn defineMeasurements(double& T,
                                              const gnsstk::Vector<double>& X,
                                              const gnsstk::Matrix<double>& C,
                                              bool useFlag) = 0;
 
      virtual void defineTimestep(double T, double DT,
                                  const gnsstk::Vector<double>& State,
                                  const gnsstk::Matrix<double>& Cov,
                                  bool useFlag) = 0;
 
      virtual int defineInterim(int which, double Time) { return -1; }
 
      // -----------------------------------------------------------------------------
      KalmanFilter()
         : NTU(0), NMU(0), NSU(0), Nstate(0), stage(Unknown), Nnoise(0),
           extended(false), smoother(false), doSRISU(true), doOutput(true),
           doInversions(true), singular(true), timeReversed(false),
           dryRun(false)
      {
      }
 
      KalmanFilter(const gnsstk::Namelist& NL) { Reset(NL); }
 
      void Reset(const gnsstk::Namelist& NL) { initialize(NL); }
 
      virtual ~KalmanFilter(){};
 
      // -----------------------------------------------------------------------------
      virtual void initializeFilter()
      {
         try
         {
            double T;
 
            int isInfo;
            gnsstk::Vector<double> initX;
            gnsstk::Matrix<double> initCov; // may be info or cov
 
            // call derived class to get initial time, apriori state
            // and covariance
            isInfo = defineInitial(T, initX, initCov);
            time   = T;
            stage  = Init;
 
            try
            {
               if (isInfo == -1)
               {
                  srif.addAPrioriInformation(initCov, initX);
                  Invert(std::string("Invert after adding a priori info"));
               }
               else if (isInfo == 1)
               {
                  srif.addAPriori(initCov, initX);
                  Invert(std::string("Invert after adding a priori info"));
                  // Cov = initCov;
                  // State = initX;
                  // inverted = true;
               }
               else
               { // returned zero
                  State    = initX;
                  Cov      = initCov;
                  inverted = false;
               }
            }
            catch (gnsstk::Exception& e)
            {
               e.addText("Failed to add apriori");
               GNSSTK_RETHROW(e);
            }
 
            if (inverted)
            {
               output(NTU);
            }
         }
         catch (gnsstk::Exception& e)
         {
            e.addText("initializeFilter");
            GNSSTK_RETHROW(e);
         }
      }
 
      // -------------------------------------------------------------
      virtual void ForwardFilter(double finalT, double DT)
      {
         int iret;
         try
         {
            // don't allow a non-positive timestep
            if ((!timeReversed && DT <= 0.0) || (timeReversed && DT >= 0.0))
            {
               gnsstk::Exception e(
                  std::string("Filter time step must be ") +
                  (timeReversed ? std::string("< 0") : std::string("> 0")));
               GNSSTK_THROW(e);
            }
 
            // save filter timestep, which is the timestep of one TU,
            // and ~timestep between consecutive defineMeasurements()
            nominalDT = DT;
 
            // to avoid round-off problems, make time comparisons only to within
            // tol
            const double tol(nominalDT / 10.0);
 
            // forward filter: loop over time
            double tmax(finalT + nominalDT);
            while (
               (timeReversed ? (time - tmax >= tol) : (time - tmax <= -tol)))
            {
 
               // ------------------------------------------------------------
               // interim #1
               iret = KalmanInterim(1, time);
 
               if (iret)
               {
                  stage = IB1;
                  Invert(std::string("Invert after interim 1"));
                  output(NTU);
               }
 
               // ------------------------------------------------------------
               /* MU: returns 0: Process         : ok, process normally
                *             1: ProcessThenQuit : quit after this data,
                *             2: SkipThisEpoch   : don't process this data, but
                *             proceed 3: SkipThenQuit    : skip this data, and
                *             then quit 4: QuitImmediately : stop now
                * This defines nexttime as the next available data epoch. */
               double nexttime    = time;
               KalmanReturn kfret = KalmanMeasurementUpdate(nexttime);
               if (kfret == QuitImmediately || kfret == SkipThenQuit)
               {
                  break;
               }
               // else SkipThisEpoch
               else if (kfret == Process || kfret == ProcessThenQuit)
               {
                  stage = MU;
 
                  if (doInversions)
                  {
                     Invert(std::string("Invert after MU"));
                     output(NMU);
                  }
               }
               // TD would you ever want several TUs before the first good MU?
               else if (kfret == SkipThisEpoch && NTU == 0)
               {
                  time = nexttime;
                  continue;
               }
 
               // ------------------------------------------------------------
               // interim #2
               iret = KalmanInterim(2, time);
 
               if (iret)
               {
                  stage = IB2;
                  Invert(std::string("Invert after interim 2"));
                  output(NTU);
               }
 
               // ------------------------------------------------------------
               // compute next timestep
               double deltaT = nexttime - time;
               // why the 1.5? why not? it must be >1 and <=2
               if (::fabs(deltaT) > 1.5 * ::fabs(nominalDT))
               {
                  deltaT = nominalDT;
               }
 
               // TU. this will update time by deltaT
               KalmanTimeUpdate(time, deltaT);
               stage = TU;
 
               if (doInversions)
               {
                  Invert(std::string("Invert after TU"));
                  output(NTU);
               }
 
               // ------------------------------------------------------------
               // interim #3
               iret = KalmanInterim(3, time);
 
               if (iret)
               {
                  stage = IB3;
                  Invert(std::string("Invert after interim 3"));
                  output(NTU);
               }
 
               if (kfret == ProcessThenQuit || kfret == SkipThenQuit)
               {
                  break;
               }
 
            } // end loop over forward filter
         }
         catch (gnsstk::Exception& e)
         {
            e.addText("ForwardFilter");
            GNSSTK_RETHROW(e);
         }
      }
 
      // -------------------------------------------------------------
      void BackwardFilter(double M)
      {
         GNSSTK_THROW(
            gnsstk::Exception("BackwardFilter must be called with integer NTU"));
      }
 
      // -------------------------------------------------------------
      virtual void BackwardFilter(int M)
      {
         try
         {
            if (!isSmoother())
            {
               gnsstk::Exception e("Use setSmoother(true) to turn on smoothing");
               GNSSTK_THROW(e);
            }
            if (singular)
            {
               gnsstk::Exception e("Cannot smooth singular filter");
               GNSSTK_THROW(e);
            }
 
            stage = SU;
            if (M < 0)
            {
               M = 0;
            }
 
            while (NTU > M)
            {
 
               // Do the SU. Decrements time by timestep, and decrements NTU
               // (first)
               KalmanSmootherUpdate();
 
               // get state after SU -- only if using SRISU, not SRIS_DM
               if (doSRISU)
               {
                  Invert(std::string("Invert after SRISU"));
               }
 
               // ------------------------------------------------------------
               // interim #4
               // Calls defineInterim(4,time);     NB ignore return value
               KalmanInterim(4, time);
 
               // output - do it here so names agree forward/backward
               output(NTU);
 
            } // end loop
         }
         catch (gnsstk::Exception& e)
         {
            e.addText("BackwardFilter");
            GNSSTK_RETHROW(e);
         }
      }
 
      // -------------------------------------------------------------
      virtual void output(int N)
      {
         if (!doOutput)
         {
            return;
         }
 
         unsigned int i;
         std::ostringstream oss;
 
         if (stage == Unknown)
         {
            LOG(ERROR) << "Kalman stage not defined in output().";
            return;
         }
         LOG(DEBUG) << "Enter KalmanFilter::output(" << N << ")";
 
         // if MU or SU, output the namelist first
         // TD make verbose
         if (stage == Init || stage == MU || stage == SU)
         {
            oss << ((stage == MU || stage == Init) ? "KNL" : "KSL") << KFtag
                << " " << std::fixed << N << " " << std::setprecision(3)
                << time;
            gnsstk::Namelist NL = srif.getNames();
            for (i = 0; i < NL.size(); i++)
               oss << std::setw(10) << NL.getName(i);
 
            LOG(INFO) << oss.str();
            oss.str("");
         }
 
         // output a label
         switch (stage)
         {
            case Init:
               oss << "KIN";
               break;
            case IB1:
            case IB2:
            case IB3:
               oss << "KIB";
               break;
            case TU:
               oss << "KTU";
               break;
            case MU:
               oss << "KMU";
               break;
            case SU:
               oss << "KSU";
               break;
            default:
            case Unknown:
               LOG(INFO) << "Kalman stage not defined." << std::endl;
               return;
         }
         oss << KFtag << " ";
 
         // output the time
         oss << std::fixed << N << " " << std::setprecision(3) << time;
 
         // output the state
         for (i = 0; i < State.size(); i++)
            oss << " " << std::setw(9) << State(i);
 
         // output sqrt of diagonal covariance elements
         oss << std::scientific << std::setprecision(2);
         for (i = 0; i < State.size(); i++)
            oss << " " << std::setw(10) << (singular ? 0.0 : ::sqrt(Cov(i, i)));
 
         LOG(INFO) << oss.str();
      }
 
         // ---------------------------------------------------------------------
         // The support routines
         // -------------------------------------------------------------------------
      virtual int KalmanInterim(int which, double Time)
      {
         try
         {
            int iret = defineInterim(which, Time);
            if (iret < 0)
            {
               return Process;
            }
            return iret;
         }
         catch (gnsstk::Exception& e)
         {
            e.addText("KINT");
            GNSSTK_RETHROW(e);
         }
      }
 
      // -------------------------------------------------------------
      virtual KalmanReturn KalmanMeasurementUpdate(double& T)
      {
         try
         {
            // Pass in T=current, return T=next data epoch;
            // if next > curr+nominalDT, should return SkipThisEpoch so TU will
            // catch up
            KalmanReturn ret =
               defineMeasurements(T, State, Cov, !singular && inverted);
            PFResid = gnsstk::Vector<double>(0);
            if (ret == Process || ret == ProcessThenQuit)
            {
               if (extended)
               {
                  srif.zeroState();
               }
               // NB. derived class must update reference trajectory
 
               if (!dryRun)
               {
                  // this func whitens before update, then unwhitens resid
                  // (PFResid)
                  PFResid = Data; // MU will replace with post-fit residuals
                  srif.measurementUpdate(Partials, PFResid, MCov);
               }
 
               inverted = false;
               NMU++;
            }
 
            return ret;
         }
         catch (gnsstk::Exception& e)
         {
            e.addText("KMU");
            GNSSTK_RETHROW(e);
         }
      }
 
      // -------------------------------------------------------------
      virtual void KalmanTimeUpdate(double T, double DT)
      {
         try
         {
            // LOG(INFO) << "KTU with NTU NMU " << NTU << " " << NMU;
 
            double timesave(time);
 
            time += DT;
            defineTimestep(time, DT, State, Cov, !singular && inverted);
 
            Nnoise = Rw.rows(); // Nnoise is member, but temporary
            Zw     = gnsstk::Vector<double>(Nnoise);
 
            // control
            if (Control.size() > 0)
            {
               srif.shift(-PhiInv * Control); // not tested
            }
 
            // create a new smoother storage record
            if (isSmoother())
            { // save for smoother
               SmootherStore[NTU]    = SmootherStoreRec();
               SmootherStoreRec &rec = SmootherStore[NTU];
               // timeUpdate will trash these
               rec.PhiInv = PhiInv;
               rec.G      = G;
               if (Control.size() > 0)
               {
                  rec.Control = Control;
               }
            }
 
            Zw = 0.0; // yes this is necessary
            gnsstk::Matrix<double> Rwx(Nnoise, Nstate, 0.0);
            if (!dryRun)
            {
               srif.timeUpdate(PhiInv, Rw, G, Zw, Rwx);
            }
            inverted = false;
 
            if (isSmoother())
            { // save for smoother
               SmootherStoreRec &rec = SmootherStore[NTU];
               // indexing is 0...NTU-1
               rec.Rw   = Rw;
               rec.Rwx  = Rwx;
               rec.Zw   = Zw;
               rec.Time = timesave;
            }
 
            NTU++;
         }
         catch (gnsstk::Exception& e)
         {
            e.addText("KTU");
            GNSSTK_RETHROW(e);
         }
      }
 
      // -------------------------------------------------------------
      virtual void KalmanSmootherUpdate()
      {
         try
         {
            NTU--;
            NSU++;
 
            // LOG(DEBUG) << " SU at " << NTU << " with state " <<
            // srif.getNames();
            SmootherStoreRec &rec(SmootherStore[NTU]);
            gnsstk::Matrix<double> Rw     = rec.Rw;
            gnsstk::Matrix<double> Rwx    = rec.Rwx;
            gnsstk::Matrix<double> PhiInv = rec.PhiInv;
            gnsstk::Matrix<double> G      = rec.G;
            gnsstk::Vector<double> Zw     = rec.Zw;
            // SU knows nothing about time; this is just for output purposes
            time = rec.Time;
 
            // TD should Control vector correction be here???
 
            if (!dryRun)
            {
               if (doSRISU)
               {
                  gnsstk::Matrix<double> Phi;
                  Phi = inverse(PhiInv);
                  srif.smootherUpdate(Phi, Rw, G, Zw, Rwx);
                  inverted = false;
               }
               else
               {
                  srif.DMsmootherUpdate(Cov, State, PhiInv, Rw, G, Zw, Rwx);
               }
            }
 
            // correct for Control vector
            if (rec.Control.size() > 0)
            {
               gnsstk::Vector<double> Control = rec.Control;
               if (doSRISU)
               {
                  srif.shift(PhiInv * Control);
               }
               else
               {
                  State -= PhiInv * Control;
               }
            }
         }
         catch (gnsstk::Exception& e)
         {
            e.addText("KSU");
            GNSSTK_RETHROW(e);
         }
      }
 
      // ----------------------------------------------------------------------------
      // Utilities
 
      bool getDoInvert() { return doInversions; }
      void setDoInvert(bool on) { doInversions = on; }
 
      bool getDoOutput() { return doOutput; }
      void setDoOutput(bool on) { doOutput = on; }
 
      bool isExtended() { return extended; }
      void setExtended(bool ext) { extended = ext; }
 
      void setSmoother(bool ext) { smoother = ext; }
      bool isSmoother() { return smoother; }
 
      void setSRISU(bool ext) { doSRISU = ext; }
      bool isSRISU() { return doSRISU; }
 
      bool isSingular() { return singular; }
 
      void setTimeReverse(bool tr = true) { timeReversed = tr; }
      bool isTimeReversed() { return timeReversed; }
 
      void setDryRun(bool t = true) { dryRun = t; }
      bool isDryRun() { return dryRun; }
 
      std::string getTag() { return KFtag; }
      void setTag(std::string tag) { KFtag = tag; }
 
      void setSRI(gnsstk::SRI& sri)
      {
         srif = static_cast<gnsstk::SRIFilter&>(sri);
      }
      gnsstk::SRI getSRI() { return static_cast<gnsstk::SRI>(srif); }
 
      gnsstk::Namelist getNames() { return srif.getNames(); }
      gnsstk::Vector<double> getState() { return State; }
      gnsstk::Matrix<double> getCovariance() { return Cov; }
 
      int getNMU() { return NMU; }
 
   private:
         // --------------------------------------------------------------------------
      void initialize(const gnsstk::Namelist& NL)
      {
         Nstate = NL.size();
         // Nnoise = Ns;    // Nnoise is for the user only
         NTU = NMU = NSU = 0;
 
         stage = Unknown;
 
         // initialize the SRIF
         srif     = gnsstk::SRIFilter(NL);
         inverted = false;
 
         State = gnsstk::Vector<double>(Nstate, 0.0);
         Cov   = gnsstk::Matrix<double>(Nstate, Nstate, 0.0);
 
         // clear smoother store
         SmootherStore.clear();
      }
 
      void Invert(const std::string& msg = std::string())
      {
         if (dryRun)
         {
            LOG(INFO) << "Dry invert" << (msg.empty() ? "" : " " + msg);
            return;
         }
         if (!doInversions)
         {
            LOG(DEBUG) << msg << " (doInversions false)";
            return;
         }
 
         // get state and covariance
         try
         {
            srif.getStateAndCovariance(State, Cov, &small, &big);
            singular = false;
            inverted = true;
            Nstate   = srif.size();
            LOG(DEBUG) << msg << " (non-singular)";
         }
         catch (gnsstk::Exception& e)
         {
            singular = true;
            inverted = false;
            LOG(DEBUG) << msg << " (singular)";
            e.addText(msg);
            GNSSTK_RETHROW(e);
         }
         catch (std::exception& e)
         {
            gnsstk::Exception E(std::string("std exception: ") + e.what());
            GNSSTK_THROW(E);
         }
      }
 
   }; // end class KalmanFilter
 
} // namespace gnsstk
#endif