关于IMU中滤波算法的请教
void updateACC(){uint8_t axis;
float accMagSum = 0;
for (axis = 0; axis < 3; axis++) {
accLPF = accADC;
accMagSum += accLPF*accLPF;
}
// 24 us
accMagSum = accMagSum*100.0/(ACC_1G*ACC_1G);
utilLP_float(&accMag, accMagSum, (1.0f/ACC_LPF_FACTOR));
}
void updateACCAttitude(){
uint8_t axis;
// 80 us
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if (( 36 < accMag && accMag < 196 ) || disableAccGtest) {
for (axis = 0; axis < 3; axis++) {
//utilLP_float(&EstG.A, accLPF, AccComplFilterConst);
EstG.A = EstG.A * (1.0 - AccComplFilterConst) + accLPF * AccComplFilterConst;
}
}
}
第二段代码可以理解,对加速度值做一阶低通滤波。但第一段代码完全没弄懂是用来做什么的。。。
这段代码中accMAg是什么, 36 < accMag && accMag < 196又是怎么得来的。
页:
[1]