#include "UWBFilter.h"
#include "math.h"

UWBFilter::UWBFilter() {
	outfile.open("/home/firefly/pibot_ros/ros_ws/src/upbot_mapping/data_fliter.txt");
	if (outfile.is_open()) {
            // std::cout << "start saving data" << std::endl;
			outfile << "lastX,lastY,lastXf,lastYf,lastV"\
					<< "\n";
        } else {
            // std::cerr << "Unable to open file " << filename << std::endl;
        }
}

void UWBFilter::update(float& x, float& y) {
	if (initFlag == false) {
		lastX = x;
		lastY = y;
		lastXf = x;
		lastYf = y;
		initFlag = true;
	}

	float d2 = (x - lastX) * (x - lastX) + (y - lastY) * (y - lastY);
	lastX = x;
	lastY = y;
	// �ж��ٶ��Ƿ���Ч�������ظ�ֵ���ٶ�Ϊ0
	float v = d2 == 0 ? lastV : sqrtf(d2);
	// �����ٶȲ���ѡ��ͬ�˲�������
	float alpha = v < 50 ? 0.9f : 0.98f;
	x = lastXf * alpha + (1 - alpha) * x;
	y = lastYf * alpha + (1 - alpha) * y;
	// ��¼��һ�ε��˲�ֵ

	lastXf = x;
	lastYf = y;
	lastV = v;
	outfile << lastX <<","<< lastY <<"," << lastXf <<"," << lastYf <<"," << lastV << "\n";
}