public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) { if (!hasBeenCalled.getBooleanValue() || containsNaN()) { hasBeenCalled.set(true); set(xUnfiltered, yUnfiltered, zUnfiltered); } if (maxRateVariable.getValue() < 0) throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); differenceVector.setToZero(getReferenceFrame()); differenceVector.set(xUnfiltered, yUnfiltered, zUnfiltered); differenceVector.sub(getX(), getY(), getZ()); limited.set(differenceVector.clipToMaxLength(maxRateVariable.getValue() * dt)); add(differenceVector); } }