diff --git a/Core/Src/MadgwickAHRS.c b/Core/Src/MadgwickAHRS.c
index 135cc2a..a69e305 100644
--- a/Core/Src/MadgwickAHRS.c
+++ b/Core/Src/MadgwickAHRS.c
@@ -148,6 +148,14 @@ void MadgwickAHRS_update(MadgwickAHRS_Filter* filter, float gx, float gy, float
 	q[2] *= recipNorm;
 	q[3] *= recipNorm;
 
+	// enforce hemisphere continuity (because of the positive and negative doubling of output)
+	float dot = filter->q[0] * q[0] + filter->q[1] * q[1] + filter->q[2] * q[2] + filter->q[3] * q[3];
+	if (dot < 0.0f){
+		q[0] = -q[0];
+		q[1] = -q[1];
+		q[2] = -q[2];
+		q[3] = -q[3];
+	}
     // Update filter state
     filter->q[0] = q[0];
     filter->q[1] = q[1];
@@ -226,6 +234,15 @@ void MadgwickAHRS_update_IMU(MadgwickAHRS_Filter* filter, float gx, float gy, fl
 	q[2] *= recipNorm;
 	q[3] *= recipNorm;
 
+	// enforce hemisphere continuity (because of the positive and negative doubling of output)
+	float dot = filter->q[0] * q[0] + filter->q[1] * q[1] + filter->q[2] * q[2] + filter->q[3] * q[3];
+	if (dot < 0.0f){
+		q[0] = -q[0];
+		q[1] = -q[1];
+		q[2] = -q[2];
+		q[3] = -q[3];
+	}
+
     // Update filter state
     filter->q[0] = q[0];
     filter->q[1] = q[1];
diff --git a/Debug/Core/Src/MadgwickAHRS.cyclo b/Debug/Core/Src/MadgwickAHRS.cyclo
index ffa83a2..5b47ebe 100644
--- a/Debug/Core/Src/MadgwickAHRS.cyclo
+++ b/Debug/Core/Src/MadgwickAHRS.cyclo
@@ -1,4 +1,4 @@
 ../Core/Src/MadgwickAHRS.c:42:6:MadgwickAHRS_init	1
-../Core/Src/MadgwickAHRS.c:55:6:MadgwickAHRS_update	7
-../Core/Src/MadgwickAHRS.c:161:6:MadgwickAHRS_update_IMU	4
-../Core/Src/MadgwickAHRS.c:240:7:invSqrt	1
+../Core/Src/MadgwickAHRS.c:55:6:MadgwickAHRS_update	8
+../Core/Src/MadgwickAHRS.c:169:6:MadgwickAHRS_update_IMU	5
+../Core/Src/MadgwickAHRS.c:257:7:invSqrt	1
diff --git a/Debug/Core/Src/MadgwickAHRS.o b/Debug/Core/Src/MadgwickAHRS.o
index 1839445..ae23e5d 100644
Binary files a/Debug/Core/Src/MadgwickAHRS.o and b/Debug/Core/Src/MadgwickAHRS.o differ
diff --git a/Debug/Core/Src/MadgwickAHRS.su b/Debug/Core/Src/MadgwickAHRS.su
index 8cb7084..a9de0f1 100644
--- a/Debug/Core/Src/MadgwickAHRS.su
+++ b/Debug/Core/Src/MadgwickAHRS.su
@@ -1,4 +1,4 @@
 ../Core/Src/MadgwickAHRS.c:42:6:MadgwickAHRS_init	24	static
 ../Core/Src/MadgwickAHRS.c:55:6:MadgwickAHRS_update	208	static
-../Core/Src/MadgwickAHRS.c:161:6:MadgwickAHRS_update_IMU	144	static
-../Core/Src/MadgwickAHRS.c:240:7:invSqrt	32	static
+../Core/Src/MadgwickAHRS.c:169:6:MadgwickAHRS_update_IMU	152	static
+../Core/Src/MadgwickAHRS.c:257:7:invSqrt	32	static