Некоторая реализация логики перенесена в main

Source/INS/IRS.c

@@ -1,8 +1,11 @@
 #include "irs.h"
 #include <math.h>
 
-#define MAHONY_KP 2.5f
-#define MAHONY_KI 0.01f
+#define MAHONY_KP 		2.5f
+#define MAHONY_KI 		0.01f
+
+#define ACC_ALPHA		0.01f
+#define ACC_MAX_ERROR	0.2f
 
 static Vector3 gyro_bias = {0};
 
@@ -14,14 +17,14 @@ void IRS_init(IRS* irs)
     irs->q.z = 0.0f;
 }
 
-Vector3 IRS_getGravity(const IRS* irs)
+Vector3 IRS_getGravity(const Quaternion* q)
 {
-    const Quaternion* q = &irs->q;
-
-    Vector3 g;
-    g.x = 2 * (q->x*q->z - q->w*q->y);
-    g.y = 2 * (q->w*q->x + q->y*q->z);
-    g.z = q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z;
+    Vector3 g = 
+	{
+		2 * (q->x*q->z - q->w*q->y),
+		2 * (q->w*q->x + q->y*q->z),
+		q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z
+	};
 
     return g;
 }
@@ -29,54 +32,72 @@ Vector3 IRS_getGravity(const IRS* irs)
 void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float dt)
 {
     Vector3 gyro = *gyro_in;
-    Vector3 accel = *accel_in;
+	Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z};
 
-    // --- нормализация акселя
-    float norm = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
-
-    if (norm > 1e-6f)
-    {
-        accel.x /= norm;
-        accel.y /= norm;
-        accel.z /= norm;
-
-        Vector3 g = IRS_getGravity(irs);
-
-        Vector3 error =
-        {
-            accel.y*g.z - accel.z*g.y,
-            accel.z*g.x - accel.x*g.z,
-            accel.x*g.y - accel.y*g.x
-        };
-
-        gyro_bias.x += MAHONY_KI * error.x * dt;
-        gyro_bias.y += MAHONY_KI * error.y * dt;
-        gyro_bias.z += MAHONY_KI * error.z * dt;
-
-        gyro.x += MAHONY_KP * error.x + gyro_bias.x;
-        gyro.y += MAHONY_KP * error.y + gyro_bias.y;
-        gyro.z += MAHONY_KP * error.z + gyro_bias.z;
-    }
-
-    irs->gyro = gyro;
-    irs->accel = accel;
-
-    // --- интеграция кватерниона
-    Quaternion q = irs->q;
-
-    Quaternion omega =
-    {
-        gyro.x * DEG2RAD,
-        gyro.y * DEG2RAD,
-        gyro.z * DEG2RAD,
-        0
-    };
-
-    Quaternion dq = QuatProd(&q, &omega);
-    dq = QuatConstProd(&dq, 0.5f * dt);
-
-    q = QuatSum(&q, &dq);
-    q = QuatNormalize(&q, 1.0f);
-
-    irs->q = q;
+    // gyro intergate
+	Quaternion q = irs->q;
+	
+	Quaternion omega = 
+	{
+		gyro.x * DEG2RAD,
+		gyro.y * DEG2RAD,
+		gyro.z * DEG2RAD,
+		0
+	};
+	
+	Quaternion dq = QuatProd(&q, &omega);
+	dq = QuatConstProd(&dq, 0.5f * dt);
+	
+	q = QuatSum(&q, &dq);
+	q = QuatNormalize(&q, 1.0f);
+	
+	// accel correction
+	float acc_len = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
+	
+	if (acc_len > 1e-6f)
+	{
+		Vector3 acc_norm =
+		{
+			accel.x / acc_len,
+			accel.y / acc_len,
+			accel.z / acc_len
+		};
+		
+		float dyn = fabsf(acc_len - 1.0f);
+		
+		float trust;
+		if (dyn > ACC_MAX_ERROR)
+			trust = 0.0f;
+		else
+			trust = 1.0f - (dyn / ACC_MAX_ERROR);
+		
+		float gain = ACC_ALPHA * trust;
+		
+		if (gain > 1e-5f)
+		{
+			Vector3 est = IRS_getGravity(&q);
+			
+			Vector3 error = 
+			{
+				acc_norm.y*est.z - acc_norm.z*est.y,
+				acc_norm.z*est.x - acc_norm.x*est.z,
+				acc_norm.x*est.y - acc_norm.y*est.x
+			};
+			
+			Quaternion corretrion = 
+			{
+				error.x * gain * 0.5f,
+				error.y * gain * 0.5f,
+				error.z * gain * 0.5f,
+				1.0f
+			};
+			
+			q = QuatProd(&corretrion, &q);
+			q = QuatNormalize(&q, 1.0f);
+		}
+	}
+	
+	irs->q = q;
+	irs->gyro = gyro;
+	irs->accel = accel;
 }