Обновлена IRS

2026-04-16 14:06:03 +03:00
parent da4dfbfae5
commit 273398ba16
7 changed files with 435 additions and 111 deletions
--- a/Source/INS/IRS.c
+++ b/Source/INS/IRS.c
@@ -1,111 +1,264 @@
-#include "irs.h"
+#include "IRS.h"
 #include <math.h>

-void IRS_init(IRS* irs)
+
+static constexpr float PI = 3.14159265359f;
+static constexpr float TO_DEG = 180.0f / PI;
+static constexpr float TO_RAD = PI / 180.0f;
+static constexpr float G = 9.80665f; // m/c^2
+
+IRS::IRS():
+  RecordGyro(RecGyr.Rec),
+  RecordSpeed(RecSpd.Rec),
+  RecordPosit(RecPos.Rec),
+  RecordQuat(RecQua.Rec)
 {
-    irs->q.w = 1.0f;
-    irs->q.x = 0.0f;
-    irs->q.y = 0.0f;
-    irs->q.z = 0.0f;
+  RecordGyro.Init(RecGyr.Buffer, Freq, RecGyr.Past);
+  RecordSpeed.Init(RecSpd.Buffer, Freq, RecSpd.Past);
+  RecordPosit.Init(RecPos.Buffer, Freq, RecPos.Past);
+  RecordQuat.Init(RecQua.Buffer, Freq, RecQua.Past);
 }

-void IRS_update(IRS* irs, float dt)
+
+// ======================Orientation======================
+void IRS::UpdateGyro(const Vector3& Gyr)
 {
-	// gyro update
-	Vector3 gyro = 
-	{
-		irs->gyro.x * dt * DEG2RAD,
-		irs->gyro.y * dt * DEG2RAD,
-		irs->gyro.z * dt * DEG2RAD
-	};
-	
-	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);
-	
-  // /gyro update
-	
-	// accel update
-	// restoreQuat(irs);
-	
-	// /accel update
+  if(!Gyr.IsFinite()) return;
+  
+  IMU.Gyr = Gyr;
+
+  Vector3 gyr = Gyr * (Period * TO_RAD);
+
+  RecordGyro.Add(Gyr * TO_RAD);
+
+  Quaternion g = gyr;
+  g = (OriQuat * g);
+
+  OriQuat += g * 0.5f;
+  OriQuat = OriQuat.Norm();
+
+  RecordQuat.Add(OriQuat);
+
+  OriPRT = OriQuat.Conjugate() * Quaternion(0, 0, 1, 0) * OriQuat;
 }

-void restoreQuat(IRS* irs)
+void IRS::UpdateAccel(const Vector3& Acc)
 {
-	float len = lengthV3(&irs->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(&irs->accel, 1.0f);
-	
-	Vector3 est = IRS_getGravity(&irs->q);
-	
-	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);
+  if(!Acc.IsFinite()) return;
+  
+  Vector3 accel = ShiftAccelPRY * Acc * ShiftAccelPRY.Conjugate();
+  
+  IMU.Acc = accel;
+
+  Quaternion ine = OriQuat * accel * OriQuat.Conjugate();
+
+  Vector3 acc = { ine.X, ine.Y, ine.Z - 1.0f }; // без гравитации
+  
+  Inertial.Acc = acc;//OriQuat.RotateAroundZ(acc, true);
+
+  Inertial.Spd += acc * (G * Period); // интегрирование скорости
+  
+  Vector3 spd = Inertial.Spd * Period;
+  
+  Inertial.Pos += spd; // интегрирование позиции
+  
+  RecordSpeed.Add(Inertial.Spd);
+  RecordPosit.Add(Inertial.Pos);
+
+  RestoreQuat(accel);
 }

-void setAccelShift(IRS* irs, const float roll, const float pitch, const float yaw)
+void IRS::RestoreQuat(const Vector3& Acc)
 {
-	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;
+  float len = Acc.Length();
+
+  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 = Acc.Norm();
+  
+  Vector3 est;
+  est.X = 2.0f * (OriQuat.X * OriQuat.Z - OriQuat.W * OriQuat.Y);
+  est.Y = 2.0f * (OriQuat.W * OriQuat.X + OriQuat.Y * OriQuat.Z);
+  est.Z = OriQuat.W * OriQuat.W - OriQuat.X * OriQuat.X - OriQuat.Y * OriQuat.Y + OriQuat.Z * OriQuat.Z;
+
+  Vector3 cross = acc.Cross(est); 
+  float dot = acc.Dot(est);
+  
+  if (dot < 0.0f) 
+  {
+    float error_len = cross.Length();
+    if (error_len < 0.001f) cross = {1.0f, 0.0f, 0.0f}; 
+    else cross = cross * (1.0f / error_len);
+  }
+  
+  Vector3 axis = cross * (gain * 0.5f);
+  
+  Quaternion correction 
+  {
+    axis.X,
+    axis.Y,
+    axis.Z,
+    1.0f // Для малых углов cos(0) = 1. При нормализации это сработает корректно.
+  };
+
+  OriQuat = OriQuat * correction;
+  
+  OriQuat = OriQuat.Norm();
 }

-Vector3 IRS_getGravity(const Quaternion* q)
+void IRS::UpdateMagnet(const Vector3& Mag)
 {
-  Vector3 g = 
-	{
-		2.0f * (q->x*q->z - q->w*q->y),
-		2.0f * (q->w*q->x + q->y*q->z),
-		q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z
-	};
+  if(!Mag.IsFinite()) return;
+  
+  IMU.Mag = Mag; // Сохраняем сырые данные (если нужно для отладки)
+  
+  static int init_count = 100;
+  
+  float alpha = 0.001f; 
+  

-  return g;
+  if(init_count > 0) 
+  {
+    alpha = 0.05f;
+    init_count--;
+  }
+
+  Vector3 mag_norm = Mag.Norm();
+  
+  Quaternion mW = OriQuat * mag_norm * OriQuat.Conjugate();
+  
+  Vector3 mW_horizontal = { mW.X, mW.Y, 0.0f };
+  
+  if (mW_horizontal.LengthSquared() < 0.0001f) return;
+  mW_horizontal = mW_horizontal.Norm();
+
+  float error_z = mW_horizontal.Y * NorthDeclination.X - mW_horizontal.X * NorthDeclination.Y;
+  
+  float half_error_step = error_z * alpha * 0.5f;
+  
+  Quaternion corr
+  {
+      0.0f,
+      0.0f,
+      half_error_step, 
+      1.0f 
+  };
+  
+  OriQuat = corr * OriQuat;
+  OriQuat = OriQuat.Norm();
+}
+// ======================/Orientation======================
+
+
+void IRS::UpdatePositionSpeed(const Vector3& ShiftPosition, const Vector3& ShiftSpeed)
+{
+  Shift.Pos += ShiftPosition;
+  Shift.Spd += ShiftSpeed;
+}
+
+void IRS::UpdateQuaternion(const Quaternion& ShiftQuaternion, float Alpha)
+{
+  Shift.Pos = Shift.Qua * (1.0f - Alpha) + ShiftQuaternion * Alpha;
+}
+
+void IRS::RestoreAllShift(Vector3& ShiftPos)
+{
+  Vector3 spd = Shift.Spd;
+  Shift.Spd = 0;
+  RecordSpeed.Mix(spd);
+  Inertial.Spd += spd;
+
+  Vector3 pos = Shift.Pos;
+  Shift.Pos = 0;
+  
+  RecordPosit.Mix(pos);
+  Inertial.Pos += pos;
+  ShiftPos=pos;
+
+  Quaternion qua = Shift.Qua;
+  Shift.Qua = 0;
+  RecordQuat.Mix(qua);
+  OriQuat += qua;
+  OriQuat = OriQuat.Norm();
+}
+
+void IRS::SetAccelShift(float Pitch, float Roll, float Yaw)
+{
+  float h_roll  = (Roll * TO_RAD)  / 2.0f;
+  float h_pitch = (Pitch * TO_RAD) / 2.0f;
+  float h_yaw   = (Yaw * TO_RAD)   / 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)};
+  //   Как найти поворот: arcsin(ось/макс_ось) * TO_GRAD
+  Quaternion q_yaw = {0.0f, 0.0f, sinf(h_yaw), cosf(h_yaw)}; 
+  
+  ShiftAccelPRY = q_pitch * q_roll * q_yaw;
+}
+
+void IRS::SetNorthDeclination(const float Yaw)
+{
+  float dec = Yaw * TO_RAD; // Укажите ваше склонение
+  Vector3 mag_north_target;
+  NorthDeclination = { sinf(dec), cosf(dec) };
+}
+
+bool IRS::SetGroundHeight(float* Height, float MaxHeight, float Alpha)
+{
+  if(Height)
+  {
+    float shift = Inertial.Pos.Z - *Height;
+    GroundShift = GroundShift*(1.0f-Alpha) + shift*Alpha;
+  }
+  else
+  {
+    float height = Inertial.Pos.Z - GroundShift;
+    
+    if(height<MaxHeight)
+    {
+      float shift = Inertial.Pos.Z - MaxHeight;
+      GroundShift = GroundShift*(1.0f-Alpha) + shift*Alpha;
+    }
+  }
+  
+  float height = Inertial.Pos.Z - GroundShift;
+  if (height < 0.0f) GroundShift+=height;
+}
+
+void IRS::GetSinXYCosZ(Vector3& SinXYCosZ)
+{
+  SinXYCosZ = OriPRT;
+}
+
+void IRS::GetPitchRollYaw(Vector3& PitchRollYaw, bool& Reversed)
+{
+  Reversed = OriPRT.Z < 0;
+  
+  float yaw = atan2f(2.0f * (OriQuat.X * OriQuat.Y - OriQuat.W * OriQuat.Z), 1.0f - 2.0f * (OriQuat.Y * OriQuat.Y + OriQuat.Z * OriQuat.Z)) * TO_DEG;
+  if (yaw < 0.0f) yaw += 360.0f;
+
+  PitchRollYaw =
+  {
+    GetAngle(OriPRT.Y, OriPRT.X, OriPRT.Z),  // Pitch
+    GetAngle(-OriPRT.X, OriPRT.Y, OriPRT.Z), // Roll
+    yaw                                      // Yaw
+  };
+}
+
+void IRS::GetInertial(Vector3* Pos, Vector3* Spd, Vector3* Acc, float* Gnd)
+{
+  if (Acc) *Acc = Inertial.Acc;
+  if (Spd) *Spd = Inertial.Spd;
+  if (Pos) *Pos = Inertial.Pos;
+  if (Gnd) *Gnd = GroundShift;
 }


@@ -114,6 +267,3 @@ Vector3 IRS_getGravity(const Quaternion* q)



-
-
-
--- a/Source/INS/IRS.h
+++ b/Source/INS/IRS.h
@@ -1,30 +1,93 @@
+#pragma once
+
 #ifndef IRS_H
 #define IRS_H

 #include "Quaternion.h"
 #include "Vector.h"
+#include "util/Record.h"

-#define PI			3.14159265359f
-#define DEG2RAD		PI / 180.0f
-
-typedef struct
+class IRS // Inertial Reference System
 {
-  Quaternion q;   			// ориентация
-	Vector3 oriPRT; 			// orientation pitch roll tilts
-  Vector3 gyro;   			// deg/s
-  Vector3 accel;  			// g
-	
-	Quaternion shiftAccelPRY; 	// смещение акселерометра
+public:
+  IRS();
+  
+  static constexpr float Freq = 100.0f;
+  static constexpr float Period = 1.0f / Freq;
+  
+  Quaternion OriQuat; // Главный кватернион ориентации
+  Vector3 OriPRT;     // Локальные наклоны
+  
+  struct { Vector3 Gyr, Acc, Mag; } IMU;      // Последние значения датчиков
+  struct { Vector3 Acc, Spd, Pos; } Inertial; // Инерциальные значения движения
+  
+  float GroundShift = 0.0f; // Разница между высотой и поверхностью
+  
+  Quaternion ShiftAccelPRY; // Смещение акселерометра
+  Vector2 NorthDeclination; // Смещение магнитометра
+  
+private:
+  // Запись значений для расчётов в прошлом
+  struct StructRecGyr
+  {
+    static constexpr float Past = 0.1f; // Максимальная запись прошлых данных
+    static constexpr unsigned long Count = Freq * Past + 1;
+    Vector3 Buffer[Count];
+    Record<Vector3> Rec;
+  } RecGyr;

-} IRS;
+  struct StructRecAccSpdPos
+  {
+    static constexpr float Past = 0.6f; // Максимальная запись прошлых данных
+    static constexpr unsigned long Count = Freq * Past + 1;
+    Vector3 Buffer[Count];
+    Record<Vector3> Rec;
+  } RecAcc, RecSpd, RecPos;

-void IRS_init(IRS* irs);
+  struct StructRecQua
+  {
+    static constexpr float Past = 0.6f; // Максимальная запись прошлых данных
+    static constexpr unsigned long Count = Freq * Past + 1;
+    Quaternion Buffer[Count];
+    Record<Quaternion> Rec;
+  } RecQua;
+  
+  //---
+  // Сдвиг данных
+  struct { Vector3 Pos, Spd; Quaternion Qua = 0.0f; } Shift; // Значение смещения позиции, скорости и кватерниона
+  //---
+  
+public:
+  Record<Vector3>& RecordGyro;
+  Record<Vector3>& RecordSpeed;
+  Record<Vector3>& RecordPosit;
+  Record<Quaternion>& RecordQuat;

-void IRS_update(IRS* irs, float dt);
-void restoreQuat(IRS* irs);
+private:
+  void RestoreQuat(const Vector3& Acc);
+  static float GetAngle(float X, float Y, float Z);
+  
+public:
+  void UpdateGyro(const Vector3& Gyr);    // deg/s
+  void UpdateAccel(const Vector3& Acc);   // g
+  void UpdateMagnet(const Vector3& Mag);  // gaus
+  
+  // Коэффициенты восстановления позиции и скорости в секунду
+  void SetShiftAlpha(const Vector3* Pos, const Vector3* Spd, const float* Qua); 

-void setAccelShift(IRS* irs, const float pitch, const float roll, const float yaw);
+  void UpdatePositionSpeed(const Vector3& ShiftPosition, const Vector3& ShiftSpeed);
+  void UpdateQuaternion(const Quaternion& ShiftQuaternion, float Alpha);

-Vector3 IRS_getGravity(const Quaternion* q);
+  void RestoreAllShift(Vector3& ShiftPos);
+  
+  void SetAccelShift(float Pitch, float Roll, float Yaw);
+  void SetNorthDeclination(const float Yaw);
+  
+  bool SetGroundHeight(float* Height, float MaxHeight, float Alpha);
+  
+  void GetSinXYCosZ(Vector3& SinXYCos);
+  void GetPitchRollYaw(Vector3& PitchRollYaw, bool& Reversed);
+  void GetInertial(Vector3* Pos, Vector3* Spd, Vector3* Acc, float* Gnd);
+};

 #endif
--- a/Source/INS/util/Record.cpp
+++ b/Source/INS/util/Record.cpp
@@ -0,0 +1,47 @@
+#include "Record.h"
+#include <math.h>
+
+template class Record<float>;
+template class Record<Vector2>;
+template class Record<Vector3>;
+template class Record<Quaternion>;
+
+template <class T>
+Record<T>::Record(T* RecordBuffer, float Frequency, float Period)
+{
+  Init(RecordBuffer, Frequency, Period);
+}
+
+template <class T>
+void Record<T>::Init(T* RecordBuffer, float Frequency, float Period)
+{
+  Freq = Frequency;
+  Buffer = RecordBuffer;
+  Count = (float)(Frequency * Period + 1.0f);
+}
+
+template <class T>
+T Record<T>::Past(float Time) const
+{
+  unsigned long move = (float)(Time * Freq);
+  if (move >= Count) move = Count - 1;
+  
+  if (Index >= move) move = Index - move;
+  else move = Index + (Count - move);
+
+  return Buffer[move];
+}
+
+template <class T>
+void Record<T>::Add(const T& Value)
+{
+  Index++;
+  if (Index >= Count) Index = 0;
+  Buffer[Index] = Value;
+}
+
+template <class T>
+void Record<T>::Mix(const T& Shift)
+{
+  for (unsigned int a = 0; a < Count; a++) Buffer[a] += Shift;
+}
--- a/Source/INS/util/Record.h
+++ b/Source/INS/util/Record.h
@@ -0,0 +1,45 @@
+#pragma once
+
+#ifndef RECORD_H
+#define RECORD_H
+
+#include "Vector.h"
+#include "Quaternion.h"
+
+template <typename T>
+class Record
+{
+public:
+  Record() {};
+  Record(T* RecordBuffer, float Frequency, float Period); // BufferSize = (Frequency * Period) + 1;
+  
+private:
+  float Freq;
+  unsigned long Count;
+  T* Buffer;
+  T Shift;
+  unsigned long Index = 0;
+
+public:
+  void Init(T* RecordBuffer, float Frequency, float Period); // BufferSize = (Frequency * Period) + 1;
+
+  T Past(float Time) const;
+
+  void Add(const T& Value);
+
+  void Mix(const T& Shift);
+};
+
+template <typename T>
+class Average
+{
+private:
+  T Value;
+  float Count = 0;
+
+public:
+  void Set(const T& Val) { Value += Val; Count++; }
+  T Get() { if (Count == 0.0f) return T(); T ave = Value / Count; Count = 0; Value = 0.0f; return ave; }
+};
+
+#endif