public VectorFloat Filter(VectorFloat v, float dt, float hz)
{
float alpha = FilterUtil.Alpha(hz, dt);
VectorFloat last = v * alpha + current * (1.0f - alpha);
current.x = last.x;
current.y = last.y;
current.z = last.z;
return(last);
}
public VectorFloat Filter(VectorFloat v, float dt, float hz_x, float hz_y, float hz_z)
{
VectorFloat alpha = new VectorFloat(FilterUtil.Alpha(hz_x, dt), FilterUtil.Alpha(hz_y, dt), FilterUtil.Alpha(hz_z, dt));
VectorFloat last = v * alpha + current * (new VectorFloat()
{
x = 1, y = 1, z = 1
} -alpha);
current.x = last.x;
current.y = last.y;
current.z = last.z;
return(last);
}
public VectorFloat Filter(VectorFloat v, float dt, float hz_x, float hz_y, float hz_z)
{
if (first)
{
first = false;
last = v;
}
VectorFloat d = (v - last) / dt;
last = v;
VectorFloat alpha = new VectorFloat(FilterUtil.Alpha(hz_x, dt), FilterUtil.Alpha(hz_y, dt), FilterUtil.Alpha(hz_z, dt));
current = alpha * (current + d);
return(current);
}