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

UWBFilter::UWBFilter() {
}

void UWBFilter::update(float& x, float& y,float& vo) {
	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.98f : 0.999f;
	x = lastXf * alpha + (1 - alpha) * x;
	y = lastYf * alpha + (1 - alpha) * y;
	// 记录上一次的滤波值
	lastXf = x;
	lastYf = y;
	lastV = v;
	vo = v;
;}