#include "Filter.h"

namespace uwb_slam{

    Filter::Filter(){}

    void Filter::LPF(vector<float>& data, int threshold){
        if (data.empty()) return;
        if(init_flag){
            init_flag=false;
            predata =data;
        }
        for(size_t i = 2;i<5;++i){
            if(fabs(data[i]-predata[i])>threshold){
                data = predata;
            }
        }
        predata = data;
    }
};