gz/math/Filter.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2014 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef GZ_MATH_FILTER_HH_
#define GZ_MATH_FILTER_HH_
 
#include <gz/math/Helpers.hh>
#include <gz/math/Vector3.hh>
#include <gz/math/Quaternion.hh>
#include <gz/math/config.hh>
 
namespace gz::math
{
  // Inline bracket to help doxygen filtering.
  inline namespace GZ_MATH_VERSION_NAMESPACE {
  //
  template <class T>
  class Filter
  {
    public: virtual ~Filter() {}
 
    public: virtual void Set(const T &_val)
    {
      y0 = _val;
    }
 
    public: virtual void Fc(double _fc, double _fs) = 0;
 
    public: virtual const T &Value() const
    {
      return y0;
    }
 
    protected: T y0{};
  };
 
  template <class T>
  class OnePole : public Filter<T>
  {
    public: OnePole() = default;
 
    public: OnePole(double _fc, double _fs)
    {
      this->Fc(_fc, _fs);
    }
 
    // Documentation Inherited.
    public: virtual void Fc(double _fc, double _fs) override
    {
      b1 = exp(-2.0 * GZ_PI * _fc / _fs);
      a0 = 1.0 - b1;
    }
 
    public: const T& Process(const T &_x)
    {
      this->y0 = a0 * _x + b1 * this->y0;
      return this->y0;
    }
 
    protected: double a0 = 0;
 
    protected: double b1 = 0;
  };
 
  class OnePoleQuaternion : public OnePole<math::Quaterniond>
  {
    public: OnePoleQuaternion()
    {
      this->Set(math::Quaterniond(1, 0, 0, 0));
    }
 
    public: OnePoleQuaternion(double _fc, double _fs)
      : OnePole<math::Quaterniond>(_fc, _fs)
    {
      this->Set(math::Quaterniond(1, 0, 0, 0));
    }
 
    public: const math::Quaterniond& Process(
                const math::Quaterniond &_x)
    {
      y0 = math::Quaterniond::Slerp(a0, y0, _x);
      return y0;
    }
  };
 
  class OnePoleVector3 : public OnePole<math::Vector3d>
  {
    public: OnePoleVector3()
    {
      this->Set(math::Vector3d(0, 0, 0));
    }
 
    public: OnePoleVector3(double _fc, double _fs)
      : OnePole<math::Vector3d>(_fc, _fs)
    {
      this->Set(math::Vector3d(0, 0, 0));
    }
  };
 
  template <class T>
  class BiQuad : public Filter<T>
  {
    public: BiQuad() = default;
 
    public: BiQuad(double _fc, double _fs)
    {
      this->Fc(_fc, _fs);
    }
 
    // Documentation Inherited.
    public: void Fc(double _fc, double _fs) override
    {
      this->Fc(_fc, _fs, 0.5);
    }
 
    public: void Fc(double _fc, double _fs, double _q)
    {
      double k = tan(GZ_PI * _fc / _fs);
      double kQuadDenom = k * k + k / _q + 1.0;
      this->a0 = k * k/ kQuadDenom;
      this->a1 = 2 * this->a0;
      this->a2 = this->a0;
      this->b0 = 1.0;
      this->b1 = 2 * (k * k - 1.0) / kQuadDenom;
      this->b2 = (k * k - k / _q + 1.0) / kQuadDenom;
    }
 
    public: virtual void Set(const T &_val) override
    {
      this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
    }
 
    public: virtual const T& Process(const T &_x)
    {
      this->y0 = this->a0 * _x +
                 this->a1 * this->x1 +
                 this->a2 * this->x2 -
                 this->b1 * this->y1 -
                 this->b2 * this->y2;
 
      this->x2 = this->x1;
      this->x1 = _x;
      this->y2 = this->y1;
      this->y1 = this->y0;
      return this->y0;
    }
 
    protected: double a0 = 0,
                      a1 = 0,
                      a2 = 0,
                      b0 = 0,
                      b1 = 0,
                      b2 = 0;
 
    protected: T x1{}, x2{}, y1{}, y2{};
  };
 
  class BiQuadVector3 : public BiQuad<math::Vector3d>
  {
    public: BiQuadVector3()
    {
      this->Set(math::Vector3d(0, 0, 0));
    }
 
    public: BiQuadVector3(double _fc, double _fs)
      : BiQuad<math::Vector3d>(_fc, _fs)
    {
      this->Set(math::Vector3d(0, 0, 0));
    }
  };
  }  // namespace GZ_MATH_VERSION_NAMESPACE
}  // namespace gz::math
#endif  // GZ_MATH_FILTER_HH_