calculate

Dependencies:   mbed X_NUCLEO_IKS01A3 Mahony_Algorithm