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:
@@ -56,7 +56,7 @@ void IMU_QuaternionEKF_Init(float* init_quaternion,float process_noise1, float p
|
|||||||
QEKF_INS.ChiSquareTestThreshold = 1e-8;
|
QEKF_INS.ChiSquareTestThreshold = 1e-8;
|
||||||
QEKF_INS.ConvergeFlag = 0;
|
QEKF_INS.ConvergeFlag = 0;
|
||||||
QEKF_INS.ErrorCount = 0;
|
QEKF_INS.ErrorCount = 0;
|
||||||
QEKF_INS.UpdateCount = 0;
|
QEKF_INS.accel_lpf_inited = 0;
|
||||||
if (lambda > 1)
|
if (lambda > 1)
|
||||||
{
|
{
|
||||||
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;
|
QEKF_INS.IMU_QuaternionEKF.F_data[20] = -halfgxdt;
|
||||||
|
|
||||||
// accel low pass filter,加速度过一下低通滤波平滑数据,降低撞击和异常的影响
|
// accel low pass filter,加速度过一下低通滤波平滑数据,降低撞击和异常的影响
|
||||||
if (QEKF_INS.UpdateCount == 0) // 如果是第一次进入,需要初始化低通滤波
|
if (!QEKF_INS.accel_lpf_inited)
|
||||||
{
|
{
|
||||||
QEKF_INS.Accel[0] = ax;
|
QEKF_INS.Accel[0] = ax;
|
||||||
QEKF_INS.Accel[1] = ay;
|
QEKF_INS.Accel[1] = ay;
|
||||||
QEKF_INS.Accel[2] = az;
|
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[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);
|
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.YawTotalAngle = 360.0f * QEKF_INS.YawRoundCount + QEKF_INS.Yaw;
|
||||||
QEKF_INS.YawAngleLast = QEKF_INS.Yaw;
|
QEKF_INS.YawAngleLast = QEKF_INS.Yaw;
|
||||||
QEKF_INS.UpdateCount++; // 初始化低通滤波用,计数测试用
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -30,7 +30,7 @@ typedef struct
|
|||||||
uint8_t ConvergeFlag;
|
uint8_t ConvergeFlag;
|
||||||
uint8_t StableFlag;
|
uint8_t StableFlag;
|
||||||
uint64_t ErrorCount;
|
uint64_t ErrorCount;
|
||||||
uint64_t UpdateCount;
|
uint8_t accel_lpf_inited;
|
||||||
|
|
||||||
float q[4]; // 四元数估计值
|
float q[4]; // 四元数估计值
|
||||||
float GyroBias[3]; // 陀螺仪零偏估计值
|
float GyroBias[3]; // 陀螺仪零偏估计值
|
||||||
|
|||||||
Reference in New Issue
Block a user