mc_pos_control: new velocity low pass and notch filter (optional, disabled by default)

- MPC_VEL_LP: new velocity first order low pass filter (off by default)
 - MPC_VEL_NF_FRQ/MPC_VEL_NF_BW: new velocity notch filter (off by default)
 - MPC_VELD_LP: existing velocity derivative low pass filter, but I've dropped the remaining controllib usage

src/lib/mathlib/math/Functions.hpp

@@ -290,6 +290,11 @@ inline bool isFinite(const float &value)
 	return PX4_ISFINITE(value);
 }
 
+inline bool isFinite(const matrix::Vector2f &value)
+{
+	return value.isAllFinite();
+}
+
 inline bool isFinite(const matrix::Vector3f &value)
 {
 	return value.isAllFinite();