Update FuseMagnetometer.cpp

Added sq function