FIRDifferentiator5thOrder.cpp

Go to the documentation of this file.

#pragma ident "$Id$"

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk 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 2.1 of the License, or
//  any later version.
//
//  The GPSTk 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 GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
//  Dagoberto Salazar - gAGE ( http://www.gage.es ). 2011
//
//============================================================================


#include "FIRDifferentiator5thOrder.hpp"



namespace gpstk
{


      /* Return result.
       *
       * @param input      Input data.
       */
   double FIRDifferentiator5thOrder::Compute( double input )
   {

         // Be default, the filter is invalid
      valid = false;

         // Default result
      double result( 0.0 );


         // Check if there are enough stored data values
      if( X.size() == 10 )
      {

            // Compute the result. This implements the equation:
            // y[k] = k1*x[k+5] - k2*x[k+4] + k3*x[k+3] - k4*x[k+2] + k5*x[k+1]
            //      - k5*x[k-1] + k4*x[k-2] - k3*x[k-3] + k2*x[k-4] - k1*x[k-5]

         result = k1*(input - X[9]) + k2*(X[8] - X[0]) + k3*(X[1] - X[7])
                                    + k4*(X[6] - X[2]) + k5*(X[3] - X[5]);

            // Filter result is valid
         valid = true;
      }

         // Update filter state

         // Insert input in input vector
      X.push_front( input );

         // Delete old inputs
      if(X.size() > 10) X.pop_back();

         // Return result
      return result;

   }  // End of constructor 'FIRDifferentiator5thOrder::Compute()'



      // Resets filter, cleaning its internal state.
   void FIRDifferentiator5thOrder::Reset(void)
   {
         // Clear stored input values
      X.clear();

         // Filter state is invalid
      valid = false;

         // Return
      return;

   }  // End of constructor 'FIRDifferentiator5thOrder::Reset()'



      /* Set the sampling period, in seconds.
       *
       * @param[in] period      Sampling period, in seconds.
       *
       * @warning Only values higher that zero are allowed. Other values will
       * be ignored.
       *
       * @warning This operation resets the filter.
       */
   FIRDifferentiator5thOrder& FIRDifferentiator5thOrder::setT( double period )
   {
         // Check period
      if( period > 0.0 )
      {
         T = period;
         Init();
         Reset();
      }

      return (*this);

   }  // End of method 'FIRDifferentiator5thOrder::setT()'



      // Initialization method
   void FIRDifferentiator5thOrder::Init( void )
   {

         // Generate filter parameters
      k1 = 1.0/(1260.0*T);
      k2 = k1*(12.5);
      k3 = k1*(75.0);
      k4 = k1*(300.0);
      k5 = k1*(1050.0);

         // Filter parameters are set. Let's return
      return;

   }  // End of method 'FIRDifferentiator5thOrder::Init()'



}  // End of namespace gpstk

---