Reset velocity, position and height during filter reset - this will make the filter converge much faster.
Change zero() function call of Vector3f to zero the vector its called on instead of returning a zero vector.