diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c
index 28dafd8cc..a4d9c24bb 100644
--- a/flight/modules/StateEstimation/filtermag.c
+++ b/flight/modules/StateEstimation/filtermag.c
@@ -63,7 +63,7 @@ struct data {
 static int32_t init(stateFilter *self);
 static filterResult filter(stateFilter *self, stateEstimation *state);
 static bool checkMagValidity(struct data *this, float error, bool setAlarms);
-// static void magOffsetEstimation(struct data *this, float mag[3]);
+static void magOffsetEstimation(struct data *this, float mag[3]);
 static float getMagError(struct data *this, float mag[3]);
 
 int32_t filterMagInitialize(stateFilter *handle)
@@ -118,10 +118,10 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
 
     if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_AUXONLY) &&
         IS_SET(state->updated, SENSORUPDATES_boardMag)) {
-        // TODO!
-        /*if (this->revoCalibration.MagBiasNullingRate > 0) {
+        // TODO:mag Offset estimation works with onboard mag only right now.
+        if (this->revoCalibration.MagBiasNullingRate > 0) {
             magOffsetEstimation(this, state->boardMag);
-           }*/
+        }
         boardMagError = getMagError(this, state->boardMag);
         // sets warning only if no mag data are available (aux is invalid or missing)
         bool boardMagValid = checkMagValidity(this, boardMagError, (temp_status == MAGSTATUS_INVALID));
@@ -204,7 +204,7 @@ static float getMagError(struct data *this, float mag[3])
 
     return error;
 }
-#if 0
+
 /**
  * Perform an update of the @ref MagBias based on
  * Magmeter Offset Cancellation: Theory and Implementation,
@@ -301,7 +301,6 @@ void magOffsetEstimation(struct data *this, float mag[3])
 #endif // if 0
 }
 
-#endif /* if 0 */
 /**
  * @}
  * @}