fix: change QuaternionEKF UpdateCount (uint64_t) to accel_lpf_inited (uint8_t)

UpdateCount was only used as a first-time-initialization flag for the
accel low-pass filter, not as a real counter. Replaced with a simple
uint8_t flag and moved the set to the first-update path.
This commit is contained in:
2026-05-30 00:35:54 +08:00
parent ea493af11f
commit 075495791a
2 changed files with 4 additions and 4 deletions
+3 -3
View File
@@ -56,7 +56,7 @@ void IMU_QuaternionEKF_Init(float* init_quaternion,float process_noise1, float p
QEKF_INS.ChiSquareTestThreshold = 1e-8;
QEKF_INS.ConvergeFlag = 0;
QEKF_INS.ErrorCount = 0;
QEKF_INS.UpdateCount = 0;
QEKF_INS.accel_lpf_inited = 0;
if (lambda > 1)
{
lambda = 1;
@@ -140,11 +140,12 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
QEKF_INS.IMU_QuaternionEKF.F_data[20] = -halfgxdt;
// accel low pass filter,加速度过一下低通滤波平滑数据,降低撞击和异常的影响
if (QEKF_INS.UpdateCount == 0) // 如果是第一次进入,需要初始化低通滤波
if (!QEKF_INS.accel_lpf_inited)
{
QEKF_INS.Accel[0] = ax;
QEKF_INS.Accel[1] = ay;
QEKF_INS.Accel[2] = az;
QEKF_INS.accel_lpf_inited = 1;
}
QEKF_INS.Accel[0] = QEKF_INS.Accel[0] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ax * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
QEKF_INS.Accel[1] = QEKF_INS.Accel[1] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ay * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
@@ -213,7 +214,6 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
}
QEKF_INS.YawTotalAngle = 360.0f * QEKF_INS.YawRoundCount + QEKF_INS.Yaw;
QEKF_INS.YawAngleLast = QEKF_INS.Yaw;
QEKF_INS.UpdateCount++; // 初始化低通滤波用,计数测试用
}
/**
+1 -1
View File
@@ -30,7 +30,7 @@ typedef struct
uint8_t ConvergeFlag;
uint8_t StableFlag;
uint64_t ErrorCount;
uint64_t UpdateCount;
uint8_t accel_lpf_inited;
float q[4]; // 四元数估计值
float GyroBias[3]; // 陀螺仪零偏估计值