From b6945b83f45570a7c4003c9f2a53d099fdd7daa2 Mon Sep 17 00:00:00 2001 From: Radzhab Bisultanov Date: Thu, 26 Mar 2026 18:01:04 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9B=D0=BE=D0=B3=D0=B8=D0=BA=D0=B0=20=D0=BE?= =?UTF-8?q?=D0=B1=D1=80=D0=B0=D0=B1=D0=BE=D1=82=D0=BA=D0=B8=20=D0=B4=D0=B0?= =?UTF-8?q?=D0=BD=D0=BD=D1=8B=D1=85=20=D0=B8=D0=BC=D1=83=20=D1=80=D0=B5?= =?UTF-8?q?=D0=B0=D0=BB=D0=B8=D0=B7=D0=BE=D0=B2=D0=B0=D0=BD=D0=B0=20=D0=BA?= =?UTF-8?q?=D0=B0=D0=BA=20=D0=B2=20=D1=80=D0=B0=D0=B1=D0=BE=D1=87=D0=B5?= =?UTF-8?q?=D0=B9=20=D0=BF=D1=80=D0=BE=D1=88=D0=B8=D0=B2=D0=BA=D0=B5.=20?= =?UTF-8?q?=D0=94=D0=BE=D0=BF=D0=BE=D0=BB=D0=BD=D0=B5=D0=BD=D1=8B=20=D1=84?= =?UTF-8?q?=D1=83=D0=BD=D0=BA=D1=86=D0=B8=D0=B8=20=D0=B2=D0=B5=D0=BA=D1=82?= =?UTF-8?q?=D0=BE=D1=80=D0=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Source/INS/IRS.c | 160 +++++++++++++++++++++++++++++------ Source/INS/IRS.h | 12 ++- Source/INS/geometry/vector.c | 40 ++++++++- Source/INS/geometry/vector.h | 5 +- 4 files changed, 183 insertions(+), 34 deletions(-) diff --git a/Source/INS/IRS.c b/Source/INS/IRS.c index fe09c06..a287fe9 100644 --- a/Source/INS/IRS.c +++ b/Source/INS/IRS.c @@ -17,41 +17,55 @@ void IRS_init(IRS* irs) irs->q.z = 0.0f; } -Vector3 IRS_getGravity(const Quaternion* q) -{ - 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; -} void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float dt) { - Vector3 gyro = *gyro_in; - Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z}; - - // gyro intergate - Quaternion q = irs->q; + Quaternion qConjugate = QuatConjugate(&irs->q); - Quaternion omega = + // gyro update + + irs->gyro = *gyro_in; + + Vector3 gyro = { - gyro.x * DEG2RAD, - gyro.y * DEG2RAD, - gyro.z * DEG2RAD, - 0 + gyro_in->x * dt * DEG2RAD, + gyro_in->y * dt * DEG2RAD, + gyro_in->z * dt * DEG2RAD }; - Quaternion dq = QuatProd(&q, &omega); - dq = QuatConstProd(&dq, 0.5f * dt); + Quaternion g = {gyro.x, gyro.y, gyro.z, 0}; + g = QuatProd(&irs->q, &g); + g = QuatConstProd(&g, 0.5f); + irs->q = QuatSum(&irs->q, &g); + irs->q = QuatNormalize(&irs->q, 1.0f); - q = QuatSum(&q, &dq); - q = QuatNormalize(&q, 1.0f); + Quaternion q0010 = {0.0f, 0.0f, 1.0f, 0.0f}; + Quaternion conj0010prod = QuatProd(&qConjugate, &q0010); + Quaternion prtilts = QuatProd(&conj0010prod, &irs->q); + + irs->oriPRT.x = prtilts.x; + irs->oriPRT.y = prtilts.y; + irs->oriPRT.z = prtilts.z; + + // /gyro update + + // accel update + + Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z}; + + irs->accel = accel; + + restoreQuat(irs, &accel); + + + + + // /accel update + + + + /*Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z}; - // accel correction float acc_len = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z); if (acc_len > 1e-6f) @@ -99,5 +113,95 @@ void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float irs->q = q; irs->gyro = gyro; - irs->accel = accel; -} \ No newline at end of file + irs->accel = accel;*/ +} + +void restoreQuat(IRS* irs, const Vector3* accel) +{ + float len = lengthV3(accel); + + static float quat_acc_alpha = 0.03f; + static float quat_acc_max = 0.02f; + + float dyn = fabsf(len - 1.0f); + if (dyn > quat_acc_max) dyn = 1.0f; else dyn /= quat_acc_max; + + float gain = quat_acc_alpha * (1.0f - dyn); + + if (gain < 0.0001f) return; + + Vector3 acc = normalizeV3(accel, 1.0f); + + Vector3 est; + est.x = 2.0f * (irs->q.x * irs->q.z - irs->q.w * irs->q.y); + est.y = 2.0f * (irs->q.w * irs->q.x - irs->q.y * irs->q.z); + est.z = irs->q.w * irs->q.w - irs->q.x * irs->q.x - irs->q.y * irs->q.y + irs->q.z * irs->q.z; + + Vector3 cross = Cross(&acc, &est); + float dot = DotV3(&acc, &est); + + if (dot < 0.0f) + { + float error_len = lengthV3(&cross); + if (error_len < 0.001f) { + cross.x = 1.0f; + cross.y = 0.0f; + cross.z = 0.0f; + } + else + { + cross = constProdV3(&cross, 1.0f / error_len); + } + } + + Vector3 axis = constProdV3(&cross, gain * 0.5f); + + Quaternion correction = + { + axis.x, + axis.y, + axis.z, + 1.0f + }; + + irs->q = QuatProd(&irs->q, &correction); + irs->q = QuatNormalize(&irs->q, 1.0f); +} + +void setAccelShift(IRS* irs, const float roll, const float pitch, const float yaw) +{ + float h_roll = (roll * DEG2RAD) / 2.0f; + float h_pitch = (pitch * DEG2RAD) / 2.0f; + float h_yaw = (yaw * DEG2RAD) / 2.0f; + + Quaternion q_roll = {0.0f, sinf(h_roll), 0.0f, cosf(h_roll)}; + Quaternion q_pitch = {sinf(h_pitch), 0.0f, 0.0f, cosf(h_pitch)}; + Quaternion q_yaw = {0.0f, 0.0f, sinf(h_yaw), cosf(h_yaw)}; + + Quaternion prProd = QuatProd(&q_pitch, &q_roll); + Quaternion pryProd = QuatProd(&prProd, &q_yaw); + + irs->shiftAccelPRY = pryProd; +} + +Vector3 IRS_getGravity(const Quaternion* q) +{ + 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; +} + + + + + + + + + + diff --git a/Source/INS/IRS.h b/Source/INS/IRS.h index 261e18a..ad59307 100644 --- a/Source/INS/IRS.h +++ b/Source/INS/IRS.h @@ -9,15 +9,21 @@ typedef struct { - Quaternion q; // ориентация - Vector3 gyro; // deg/s - Vector3 accel; // g + Quaternion q; // ориентация + Vector3 oriPRT; // orientation pitch roll tilts + Vector3 gyro; // deg/s + Vector3 accel; // g + + Quaternion shiftAccelPRY; // смещение акселерометра } IRS; void IRS_init(IRS* irs); void IRS_update(IRS* irs, const Vector3* gyro, const Vector3* accel, float dt); +void restoreQuat(IRS* irs, const Vector3* accel); + +void setAccelShift(IRS* irs, const float pitch, const float roll, const float yaw); Vector3 IRS_getGravity(const Quaternion* q); diff --git a/Source/INS/geometry/vector.c b/Source/INS/geometry/vector.c index 1fb265f..3ce6d17 100644 --- a/Source/INS/geometry/vector.c +++ b/Source/INS/geometry/vector.c @@ -10,8 +10,29 @@ Vector2 normalizeV2(const Vector2* v, float gain) Vector3 normalizeV3(const Vector3* v, float gain) { - float len = lengthV3(v); - Vector3 res = {.x = v->x / len, .y = v->y / len, .z = v->z}; + Vector3 res = {0}; + float n = lengthV3(v); + + if (n > 1e-12f) + { + n = gain / n; + res.x = v->x * n; + res.y = v->y * n; + res.z = v->z * n; + } + + return res; +} + +float DotV2(const Vector2* v1, const Vector2* v2) +{ + float res = v1->x * v2->x + v1->y * v2->y; + return res; +} + +float DotV3(const Vector3* v1, const Vector3* v2) +{ + float res = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z; return res; } @@ -129,6 +150,21 @@ float scalarProdV3(const Vector3* v1, const Vector3* v2) } +Vector3 Cross(const Vector3* v1, const Vector3* v2) +{ + Vector3 res = + { + v1->x * v2->z - v1->z * v2->y, + v1->z * v2->x - v1->x * v2->z, + v1->x * v2->y - v1->y * v2->x + }; + + return res; +} + + + + diff --git a/Source/INS/geometry/vector.h b/Source/INS/geometry/vector.h index 0ac0024..eebf4ad 100644 --- a/Source/INS/geometry/vector.h +++ b/Source/INS/geometry/vector.h @@ -17,6 +17,9 @@ typedef struct Vector2 normalizeV2(const Vector2* v, float gain); Vector3 normalizeV3(const Vector3* v, float gain); +float DotV2(const Vector2* v1, const Vector2* v2); +float DotV3(const Vector3* v1, const Vector3* v2); + Vector2 absV2(const Vector2* v); Vector3 absV3(const Vector3* v); @@ -45,6 +48,6 @@ float scalarProdV2(const Vector2* v1, const Vector2* v2); float scalarProdV3(const Vector3* v1, const Vector3* v2); Vector2 vectorProdV2(const Vector2* v1, const Vector2* v2); -Vector3 vectorProdV3(const Vector3* v1, const Vector3* v2); +Vector3 Cross(const Vector3* v1, const Vector3* v2); #endif \ No newline at end of file