diff --git a/state_machine/applications/flight/Tasks/gnc.py b/state_machine/applications/flight/Tasks/gnc.py
index 93a2b217..dd769a5f 100644
--- a/state_machine/applications/flight/Tasks/gnc.py
+++ b/state_machine/applications/flight/Tasks/gnc.py
@@ -1,15 +1,12 @@
 from lib.template_task import Task
-from lib.control import bcross
 import lib.pycubed as cubesat
-from lib.IGRF import igrf_eci
-from lib.sun_position import approx_sun_position_ECI
-import lib.orbital_mechanics as orbital_mechanics
-import lib.mekf as mekf
+from lib.gnc.control import bcross
+from lib.gnc.IGRF import igrf_eci
+from lib.gnc.sun_position import approx_sun_position_ECI
+import lib.gnc.orbital_mechanics as orbital_mechanics
+import lib.gnc.mekf as mekf
+import lib.gnc.gnc_state as gnc_state
 import time
-try:
-    from ulab.numpy import array
-except ImportError:
-    from numpy import array
 
 
 def toStr(arr):
@@ -21,9 +18,36 @@ class task(Task):
 
     rgb_on = False
     last = None
-    r_eci = array([6871, -6571, -7071])
+    sun_sensor_failed = False
 
     async def main_task(self):
+        failed = False
+        # start calculating time steps
+        if self.last is None:
+            self.last = time.monotonic()
+            return
+        t = time.monotonic()
+        delta_t = t - self.last
+
+        # update mekf
+        w = cubesat.gyro()
+        br_mag = cubesat.magnetic()
+        try:
+            br_sun = cubesat.sun_vector()
+        except cubesat.HardwareInitException as e:
+            if not self.sun_sensor_failed:
+                self.debug('Something went wrong trying to read from a sun sensor')
+                self.debug(f'Error: {e}')
+                self.sun_sensor_failed = True
+            failed = True
+        nr_mag = igrf_eci(t, gnc_state.eci_state[0:3])
+        nr_sun = approx_sun_position_ECI(t)
+
+        if not failed:
+            mekf.step(w, delta_t, nr_mag, nr_sun, br_mag, br_sun)
+
+        # propogate ECI position
+        gnc_state.eci_state = orbital_mechanics.propogate(gnc_state.eci_state, delta_t, integration_step=5)
 
         # compute control
         m = bcross(cubesat.magnetic(), cubesat.gyro())
diff --git a/state_machine/applications/flight/lib/IGRF.py b/state_machine/applications/flight/lib/gnc/IGRF.py
similarity index 98%
rename from state_machine/applications/flight/lib/IGRF.py
rename to state_machine/applications/flight/lib/gnc/IGRF.py
index fc1d531f..19bcfded 100644
--- a/state_machine/applications/flight/lib/IGRF.py
+++ b/state_machine/applications/flight/lib/gnc/IGRF.py
@@ -3,12 +3,12 @@
 # Fifth order approximatin.
 
 try:
-    import ulab as np
+    import ulab.numpy as np
 except ImportError:
     import numpy as np
 import math
 
-import lib.frames as frames
+import lib.gnc.frames as frames
 
 def reset_array(input_array):
     for i in range(len(input_array)):
diff --git a/state_machine/applications/flight/lib/control.py b/state_machine/applications/flight/lib/gnc/control.py
similarity index 96%
rename from state_machine/applications/flight/lib/control.py
rename to state_machine/applications/flight/lib/gnc/control.py
index afb924dc..83be26bc 100644
--- a/state_machine/applications/flight/lib/control.py
+++ b/state_machine/applications/flight/lib/gnc/control.py
@@ -1,6 +1,6 @@
 try:
     from ulab.numpy import eye as identity, array, linalg, cross, dot as matmul, isfinite, all
-except Exception:
+except ImportError:
     from numpy import identity, array, linalg, cross, matmul, isfinite, all
 
 def bcross(b, ω, k=7e-4):
diff --git a/state_machine/applications/flight/lib/frames.py b/state_machine/applications/flight/lib/gnc/frames.py
similarity index 96%
rename from state_machine/applications/flight/lib/frames.py
rename to state_machine/applications/flight/lib/gnc/frames.py
index 3416f114..ca23417f 100644
--- a/state_machine/applications/flight/lib/frames.py
+++ b/state_machine/applications/flight/lib/gnc/frames.py
@@ -1,9 +1,10 @@
 try:
     import ulab.numpy as np
-    from ulab.numpy import cos, sin, pi, arctan2, sqrt, array
+    from ulab.numpy import cos, sin, pi, sqrt, array
 except ImportError:
     import numpy as np
-    from numpy import cos, sin, pi, arctan2, sqrt, array
+    from numpy import cos, sin, pi, sqrt, array
+from math import atan2 as arctan2
 
 J2000 = 946684800  # unix timestamp for the Julian date 2000-01-01
 MJD_ZERO = 2400000.5  # Offset of Modified Julian Days representation with respect to Julian Days.
diff --git a/state_machine/applications/flight/lib/gnc/gnc_state.py b/state_machine/applications/flight/lib/gnc/gnc_state.py
new file mode 100644
index 00000000..d7eff7b3
--- /dev/null
+++ b/state_machine/applications/flight/lib/gnc/gnc_state.py
@@ -0,0 +1,6 @@
+try:
+    from ulab.numpy import array
+except ImportError:
+    from numpy import array
+
+eci_state = array([6871, -6571, -7071, 2, -10, 3])  # [x, y, z, vx, vy, vz]
diff --git a/state_machine/applications/flight/lib/mathutils.py b/state_machine/applications/flight/lib/gnc/mathutils.py
similarity index 99%
rename from state_machine/applications/flight/lib/mathutils.py
rename to state_machine/applications/flight/lib/gnc/mathutils.py
index bbe2c3eb..dc29346f 100644
--- a/state_machine/applications/flight/lib/mathutils.py
+++ b/state_machine/applications/flight/lib/gnc/mathutils.py
@@ -1,6 +1,6 @@
 try:
     from ulab.numpy import array, ndarray, zeros, eye as I, dot as matmul  # noqa: E741 (I is not ambiguous)
-except Exception:
+except ImportError:
     from numpy import array, ndarray, zeros, eye as I, matmul  # noqa: E741 (I is not ambiguous)
 
 def block(S):
diff --git a/state_machine/applications/flight/lib/mekf.py b/state_machine/applications/flight/lib/gnc/mekf.py
similarity index 96%
rename from state_machine/applications/flight/lib/mekf.py
rename to state_machine/applications/flight/lib/gnc/mekf.py
index c87ab56e..97e09302 100644
--- a/state_machine/applications/flight/lib/mekf.py
+++ b/state_machine/applications/flight/lib/gnc/mekf.py
@@ -12,9 +12,9 @@
 """
 try:
     from ulab.numpy import dot as matmul, eye as I, zeros, array, linalg, concatenate as concat  # noqa: E741
-except Exception:
+except ImportError:
     from numpy import linalg, matmul, eye as I, zeros, array, concatenate as concat  # noqa: E741
-from lib.mathutils import quaternion_mul, quaternion_to_left_matrix, hat, block, quaternion_to_rotation_matrix
+from lib.gnc.mathutils import quaternion_mul, quaternion_to_left_matrix, hat, block, quaternion_to_rotation_matrix
 from math import cos, sin
 
 q = array([0., 0., 0., 0.])  # Quaternion attitude vector
diff --git a/state_machine/applications/flight/lib/orbital_mechanics.py b/state_machine/applications/flight/lib/gnc/orbital_mechanics.py
similarity index 100%
rename from state_machine/applications/flight/lib/orbital_mechanics.py
rename to state_machine/applications/flight/lib/gnc/orbital_mechanics.py
diff --git a/state_machine/applications/flight/lib/sun_position.py b/state_machine/applications/flight/lib/gnc/sun_position.py
similarity index 100%
rename from state_machine/applications/flight/lib/sun_position.py
rename to state_machine/applications/flight/lib/gnc/sun_position.py
diff --git a/state_machine/drivers/emulation/lib/pycubed.py b/state_machine/drivers/emulation/lib/pycubed.py
index c9370e73..fb99bf43 100644
--- a/state_machine/drivers/emulation/lib/pycubed.py
+++ b/state_machine/drivers/emulation/lib/pycubed.py
@@ -1,3 +1,8 @@
+# force numpy to use one thread
+import os
+os.environ["OPENBLAS_NUM_THREADS"] = "1"
+os.environ["MKL_NUM_THREADS"] = "1"
+
 import time
 import tasko
 
@@ -55,7 +60,7 @@ def __init__(self):
         # to provide more interesting output from the b-cross controller.
         self._accel = [1.0, 2.0, 3.0]
         self._mag = [4.0, 3.0, 1.0]
-        self._gyro = [0.0, 0.0, 0.0]
+        self._gyro = [3.0, 5.0, -0.3]
         self._torque = [0, 0, 0]
         self.sim = False
 
@@ -124,6 +129,13 @@ def sun_vector():
     return array([0, 0, 0])
 
 
+"""
+Define HardwareInitException
+"""
+class HardwareInitException(Exception):
+    pass
+
+
 """
 Radio related functions
 """
diff --git a/state_machine/drivers/pycubedmini/lib/pycubed.py b/state_machine/drivers/pycubedmini/lib/pycubed.py
index 9819fd44..33ced20b 100644
--- a/state_machine/drivers/pycubedmini/lib/pycubed.py
+++ b/state_machine/drivers/pycubedmini/lib/pycubed.py
@@ -238,6 +238,13 @@ class _Satellite:
     c_downlink = multiBitFlag(register=_DWNLINK, lowest_bit=0, num_bits=8)
     c_logfail = multiBitFlag(register=_LOGFAIL, lowest_bit=0, num_bits=8)
 
+    # Set hardware attributes to None
+    # Needed for things not to crash
+    _i2c1, _i2c2, _i2c3, _spi, _sd, _neopixel = None, None, None, None, None, None
+    _imu, _radio, _sun_yn, _sun_zn, _sun_xn = None, None, None, None, None
+    _sun_yp, _sun_zp, _sun_xp, _drv_x, _drv_y = None, None, None, None, None
+    _drv_z, _burnwire1, _burnwire2 = None, None, None
+
     UHF_FREQ = 433.0
 
     instance = None
diff --git a/state_machine/unit_tests/test_control.py b/state_machine/unit_tests/test_control.py
index 822089c9..34e0474b 100644
--- a/state_machine/unit_tests/test_control.py
+++ b/state_machine/unit_tests/test_control.py
@@ -6,7 +6,7 @@
 sys.path.insert(0, './state_machine/applications/flight')
 sys.path.insert(0, './state_machine/frame')
 
-from lib.control import bcross
+from lib.gnc.control import bcross
 
 
 class BCrossTest(unittest.TestCase):
diff --git a/state_machine/unit_tests/test_frames.py b/state_machine/unit_tests/test_frames.py
index a8b61841..1e8098a1 100644
--- a/state_machine/unit_tests/test_frames.py
+++ b/state_machine/unit_tests/test_frames.py
@@ -6,7 +6,7 @@
 
 sys.path.insert(0, './state_machine/applications/flight')
 
-import lib.frames as frames
+import lib.gnc.frames as frames
 
 EARTH_RADIUS = 6371  # Earth radius (km)
 LEO = 2000  # Low Earth Orbit Altitude Limit (km)
diff --git a/state_machine/unit_tests/test_igrf.py b/state_machine/unit_tests/test_igrf.py
index 9d0c757f..ca05995c 100644
--- a/state_machine/unit_tests/test_igrf.py
+++ b/state_machine/unit_tests/test_igrf.py
@@ -5,7 +5,7 @@
 
 sys.path.insert(0, './state_machine/applications/flight')
 
-from lib.IGRF import igrf, igrf_eci
+from lib.gnc.IGRF import igrf, igrf_eci
 
 EARTH_RADIUS = 6.378136300e3  # km
 
diff --git a/state_machine/unit_tests/test_mathutils.py b/state_machine/unit_tests/test_mathutils.py
index a3bd347f..31abbb93 100644
--- a/state_machine/unit_tests/test_mathutils.py
+++ b/state_machine/unit_tests/test_mathutils.py
@@ -4,7 +4,7 @@
 
 sys.path.insert(0, './state_machine/applications/flight')
 
-from lib.mathutils import hat, quaternion_to_left_matrix, block, quaternion_to_rotation_matrix, quaternion_mul
+from lib.gnc.mathutils import hat, quaternion_to_left_matrix, block, quaternion_to_rotation_matrix, quaternion_mul
 
 class HatTests(unittest.TestCase):
 
diff --git a/state_machine/unit_tests/test_mekf.py b/state_machine/unit_tests/test_mekf.py
index 8e26feb0..39549329 100644
--- a/state_machine/unit_tests/test_mekf.py
+++ b/state_machine/unit_tests/test_mekf.py
@@ -4,7 +4,7 @@
 
 sys.path.insert(0, './state_machine/applications/flight')
 
-import lib.mekf as mekf
+import lib.gnc.mekf as mekf
 
 class PropogationTest(unittest.TestCase):
 
diff --git a/state_machine/unit_tests/test_orbital_mechanics.py b/state_machine/unit_tests/test_orbital_mechanics.py
index 5667339b..7ded0870 100644
--- a/state_machine/unit_tests/test_orbital_mechanics.py
+++ b/state_machine/unit_tests/test_orbital_mechanics.py
@@ -6,7 +6,7 @@
 
 sys.path.insert(0, './state_machine/applications/flight')
 
-from lib.orbital_mechanics import rk4, propogate, d_state, MEAN_RADIUS
+from lib.gnc.orbital_mechanics import rk4, propogate, d_state, MEAN_RADIUS
 
 class TestRK4(unittest.TestCase):
 
diff --git a/state_machine/unit_tests/test_sun_position.py b/state_machine/unit_tests/test_sun_position.py
index 1644917f..950ff207 100644
--- a/state_machine/unit_tests/test_sun_position.py
+++ b/state_machine/unit_tests/test_sun_position.py
@@ -4,7 +4,7 @@
 
 sys.path.insert(0, './state_machine/applications/flight')
 
-from lib.sun_position import approx_sun_position_ECI
+from lib.gnc.sun_position import approx_sun_position_ECI
 
 class TestSunPosition(unittest.TestCase):