private void computeDerivativeTerm( FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity) { desiredAngularVelocity.changeFrame(bodyFrame); currentAngularVelocity.changeFrame(bodyFrame); derivativeTerm.sub(desiredAngularVelocity, currentAngularVelocity); // Limit the maximum velocity error considered for control action double maximumVelocityError = gains.getMaximumDerivativeError(); double velocityErrorMagnitude = derivativeTerm.length(); if (velocityErrorMagnitude > maximumVelocityError) { derivativeTerm.scale(maximumVelocityError / velocityErrorMagnitude); } velocityError.set(derivativeTerm); derivativeGainMatrix.transform(derivativeTerm.getVector()); }