diff --git a/libraries/AP_Compass/AP_Compass_SITL.cpp b/libraries/AP_Compass/AP_Compass_SITL.cpp
index 7540c643b5..046a43c9b1 100644
--- a/libraries/AP_Compass/AP_Compass_SITL.cpp
+++ b/libraries/AP_Compass/AP_Compass_SITL.cpp
@@ -25,25 +25,38 @@ AP_Compass_SITL::AP_Compass_SITL(Compass &compass):
         
         // make first compass external
         set_external(_compass_instance[0], true);
-        
-        hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void));
 
-        // create correction matrix for diagnonals and off-diagonals
-        Vector3f diag = _sitl->mag_diag.get();
-        if (diag.is_zero()) {
-            diag(1,1,1);
-        }
-        const Vector3f &diagonals = diag;
-        const Vector3f &offdiagonals = _sitl->mag_offdiag;
-        _eliptical_corr = Matrix3f(diagonals.x,    offdiagonals.x, offdiagonals.y,
-                                   offdiagonals.x, diagonals.y,    offdiagonals.z,
-                                   offdiagonals.y, offdiagonals.z, diagonals.z);
-        if (!_eliptical_corr.invert()) {
-            _eliptical_corr.identity();
-        }
+        hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void));
     }
 }
 
+
+/*
+  create correction matrix for diagnonals and off-diagonals
+*/
+void AP_Compass_SITL::_setup_eliptical_correcion(void)
+{
+    Vector3f diag = _sitl->mag_diag.get();
+    if (diag.is_zero()) {
+        diag(1,1,1);
+    }
+    const Vector3f &diagonals = diag;
+    const Vector3f &offdiagonals = _sitl->mag_offdiag;
+    
+    if (diagonals == _last_dia && offdiagonals == _last_odi) {
+        return;
+    }
+    
+    _eliptical_corr = Matrix3f(diagonals.x,    offdiagonals.x, offdiagonals.y,
+                               offdiagonals.x, diagonals.y,    offdiagonals.z,
+                               offdiagonals.y, offdiagonals.z, diagonals.z);
+    if (!_eliptical_corr.invert()) {
+        _eliptical_corr.identity();
+    }
+    _last_dia = diag;
+    _last_odi = offdiagonals;
+}
+
 void AP_Compass_SITL::_timer()
 {
     // TODO: Refactor delay buffer with AP_Baro_SITL.
@@ -91,6 +104,8 @@ void AP_Compass_SITL::_timer()
         new_mag_data = buffer[best_index].data;
     }
 
+    _setup_eliptical_correcion();        
+    
     new_mag_data = _eliptical_corr * new_mag_data;
     new_mag_data -= _sitl->mag_ofs.get();
 
diff --git a/libraries/AP_Compass/AP_Compass_SITL.h b/libraries/AP_Compass/AP_Compass_SITL.h
index 360c6aabda..e5e0c4c650 100644
--- a/libraries/AP_Compass/AP_Compass_SITL.h
+++ b/libraries/AP_Compass/AP_Compass_SITL.h
@@ -37,8 +37,11 @@ private:
 
     Vector3f _mag_accum[SITL_NUM_COMPASSES];
     uint32_t _accum_count;
+
+    void _setup_eliptical_correcion();
+    
     Matrix3f _eliptical_corr;
-
-
+    Vector3f _last_dia;
+    Vector3f _last_odi;
 };
 #endif // CONFIG_HAL_BOARD