AP_Math: Moved rounding function for float vector to AP_Math and used standard C++ rounding function

Signed-off-by: Daniel Frenzel <dgdanielf@gmail.com>