diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h
index f586d65ea..dcd70e5df 100644
--- a/flight/libraries/math/mathmisc.h
+++ b/flight/libraries/math/mathmisc.h
@@ -107,7 +107,7 @@ static inline void vector_normalizef(float *vector, const uint8_t dim)
 {
     float length = vector_lengthf(vector, dim);
 
-    if (length <= 0.0f || isnan(length)) {
+    if (length <= 0.0f || !IS_REAL(length)) {
         return;
     }
     for (int t = 0; t < dim; t++) {
diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c
index 8a9719efc..561bc03a7 100644
--- a/flight/modules/Attitude/attitude.c
+++ b/flight/modules/Attitude/attitude.c
@@ -538,7 +538,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
     accels[2] *= accel_scale.Z * invcount;
     temp *= invcount;
 
-    if (isnan(temperature)) {
+    if (!IS_REAL(temperature)) {
         temperature = temp;
     }
     temperature = temp_alpha * (temp - temperature) + temperature;
@@ -740,7 +740,7 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
 
     // If quaternion has become inappropriately short or is nan reinit.
     // THIS SHOULD NEVER ACTUALLY HAPPEN
-    if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) {
+    if ((fabsf(inv_qmag) > 1e3f) || !IS_REAL(inv_qmag)) {
         q[0] = 1;
         q[1] = 0;
         q[2] = 0;
diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c
index 2d16307e4..7f3fa3f48 100644
--- a/flight/modules/Sensors/sensors.c
+++ b/flight/modules/Sensors/sensors.c
@@ -518,7 +518,7 @@ static void handleBaro(float sample, float temperature)
 
     float altitude = 44330.0f * (1.0f - powf((sample) / PIOS_CONST_MKS_STD_ATMOSPHERE_F, (1.0f / 5.255f)));
 
-    if (!isnan(altitude)) {
+    if (IS_REAL(altitude)) {
         BaroSensorData data;
         data.Altitude    = altitude;
         data.Temperature = temperature;
@@ -530,7 +530,7 @@ static void handleBaro(float sample, float temperature)
 
 static void updateAccelTempBias(float temperature)
 {
-    if (isnan(accel_temperature)) {
+    if (!IS_REAL(accel_temperature)) {
         accel_temperature = temperature;
     }
     accel_temperature = temp_alpha_gyro_accel * (temperature - accel_temperature) + accel_temperature;
@@ -552,7 +552,7 @@ static void updateAccelTempBias(float temperature)
 
 static void updateGyroTempBias(float temperature)
 {
-    if (isnan(gyro_temperature)) {
+    if (!IS_REAL(gyro_temperature)) {
         gyro_temperature = temperature;
     }
 
@@ -573,7 +573,7 @@ static void updateGyroTempBias(float temperature)
 
 static void updateBaroTempBias(float temperature)
 {
-    if (isnan(baro_temperature)) {
+    if (!IS_REAL(baro_temperature)) {
         baro_temperature = temperature;
     }
 
diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c
index 0e67fb99a..f17a610a6 100644
--- a/flight/modules/StateEstimation/filtercf.c
+++ b/flight/modules/StateEstimation/filtercf.c
@@ -535,7 +535,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
 
     // If quaternion has become inappropriately short or is nan reinit.
     // THIS SHOULD NEVER ACTUALLY HAPPEN
-    if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) {
+    if ((fabsf(inv_qmag) > 1e3f) || !IS_REAL(inv_qmag)) {
         this->first_run = 1;
         return FILTERRESULT_WARNING;
     }
diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c
index 92f3f7364..08f0ef32a 100644
--- a/flight/modules/StateEstimation/filterekf.c
+++ b/flight/modules/StateEstimation/filterekf.c
@@ -481,7 +481,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
 // check for invalid variance values
 static inline bool invalid_var(float data)
 {
-    if (isnan(data) || isinf(data)) {
+    if (!IS_REAL(data)) {
         return true;
     }
     if (data < 1e-15f) { // var should not be close to zero. And not negative either.
diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c
index 299af3d30..5fe73d20e 100644
--- a/flight/modules/StateEstimation/filtermag.c
+++ b/flight/modules/StateEstimation/filtermag.c
@@ -282,9 +282,9 @@ void magOffsetEstimation(struct data *this, float mag[3])
     delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
     delta[2] = -rate * (Rz - B_e[2]);
 
-    if (!isnan(delta[0]) && !isinf(delta[0]) &&
-        !isnan(delta[1]) && !isinf(delta[1]) &&
-        !isnan(delta[2]) && !isinf(delta[2])) {
+    if (IS_REAL(delta[0]) &&
+        IS_REAL(delta[1]) &&
+        IS_REAL(delta[2])) {
         this->magBias[0] += delta[0];
         this->magBias[1] += delta[1];
         this->magBias[2] += delta[2];
diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h
index 8c89b13c5..f165282ca 100644
--- a/flight/pios/inc/pios_math.h
+++ b/flight/pios/inc/pios_math.h
@@ -66,20 +66,26 @@
 #define M_EULER_D    0.57721566490153286060651209008d      /* Euler constant */
 
 // Conversion macro
-#define RAD2DEG(rad)            ((rad) * (180.0f / M_PI_F))
-#define DEG2RAD(deg)            ((deg) * (M_PI_F / 180.0f))
+#define RAD2DEG(rad)      ((rad) * (180.0f / M_PI_F))
+#define DEG2RAD(deg)      ((deg) * (M_PI_F / 180.0f))
 
-#define RAD2DEG_D(rad)          ((rad) * (180.0d / M_PI_D))
-#define DEG2RAD_D(deg)          ((deg) * (M_PI_D / 180.0d))
+#define RAD2DEG_D(rad)    ((rad) * (180.0d / M_PI_D))
+#define DEG2RAD_D(deg)    ((deg) * (M_PI_D / 180.0d))
 
 // helper macros for LPFs
-#define LPF_ALPHA(dt, fc)       (dt / (dt + 1.0f / (2.0f * M_PI_F * fc)))
+#define LPF_ALPHA(dt, fc) (dt / (dt + 1.0f / (2.0f * M_PI_F * fc)))
 
 // Useful math macros
-#define MAX(a, b)               ((a) > (b) ? (a) : (b))
-#define MIN(a, b)               ((a) < (b) ? (a) : (b))
+#define MAX(a, b)         ((a) > (b) ? (a) : (b))
+#define MIN(a, b)         ((a) < (b) ? (a) : (b))
 
-#define IS_REAL(f)              (isfinite(f))
+#ifndef isfinite
+    #include <math.h>
+#endif
+__attribute__((optimize("no-fast-math"))) static inline int IS_REAL(float f)
+{
+    return isfinite(f);
+}
 
 // Bitfield access
 #define IS_SET(field, mask)     (((field) & (mask)) == (mask))