first_order_filter.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2018 The urg_stamped Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  * http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #ifndef FIRST_ORDER_FILTER_H
00018 #define FIRST_ORDER_FILTER_H
00019 
00020 template <typename FLT>
00021 class FirstOrderFilter
00022 {
00023 protected:
00024   FLT x_;
00025   FLT k_[4];
00026 
00027 public:
00028   FirstOrderFilter()
00029     : x_(0)
00030   {
00031     // Pass through
00032     k_[0] = k_[1] = k_[3] = 0;
00033     k_[2] = 1;
00034   }
00035   FLT update(const FLT &in)
00036   {
00037     x_ = k_[0] * in + k_[1] * x_;
00038     const auto out = k_[2] * in + k_[3] * x_;
00039 
00040     return out;
00041   }
00042 };
00043 
00044 template <typename FLT>
00045 class FirstOrderLPF : public FirstOrderFilter<FLT>
00046 {
00047 public:
00048   explicit FirstOrderLPF(const FLT &time_constant)
00049   {
00050     this->k_[3] = -1 / (1.0 + 2 * time_constant);
00051     this->k_[2] = -this->k_[3];
00052     this->k_[1] = (1.0 - 2 * time_constant) * this->k_[3];
00053     this->k_[0] = -this->k_[1] - 1.0;
00054   }
00055 };
00056 
00057 template <typename FLT>
00058 class FirstOrderHPF : public FirstOrderFilter<FLT>
00059 {
00060 public:
00061   explicit FirstOrderHPF(const FLT &time_constant)
00062   {
00063     this->k_[3] = -1 / (1.0 + 2 * time_constant);
00064     this->k_[2] = -this->k_[3] * 2 * time_constant;
00065     this->k_[1] = (1.0 - 2 * time_constant) * this->k_[3];
00066     this->k_[0] = 2 * time_constant * (-this->k_[1] + 1.0);
00067   }
00068 };
00069 
00070 #endif  // FIRST_ORDER_FILTER_H


urg_stamped
Author(s): Atsushi Watanabe
autogenerated on Thu Jun 6 2019 18:59:51