MWC互补滤波代码有问题 (amobbs.com 阿莫电子技术论坛) -

Gosling 发表于 2014-11-5 23:36:24

MWC互补滤波代码有问题

EstG.A = (EstG.A * (float)cfg.gyro_cmpf_factor + accSmooth) * INV_GYR_CMPF_FACTOR;

转化一下

ACC历史 = （ACC历史 * 400 + ACC当前）/401;这个只是对加速度历史数据和当前数据融合一下，根本没用到陀螺仪，最后求欧拉角度时也没用到这个ACC。
这是计算角度代码
static void getEstimatedAttitude(void)
{
uint32_t axis;
int32_t accMag = 0;
static t_fp_vector EstG;
static t_fp_vector EstM;
#if defined(MG_LPF_FACTOR)
static int16_t mgSmooth;
#endif
static float accLPF;
static uint32_t previousT;
uint32_t currentT = micros();
float scale, deltaGyroAngle;

scale = (currentT - previousT) * GYRO_SCALE;
previousT = currentT;

// Initialization
for (axis = 0; axis < 3; axis++) {
   deltaGyroAngle = gyroADC * scale;
   if (cfg.acc_lpf_factor > 0) {
         accLPF = accLPF * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC * (1.0f / cfg.acc_lpf_factor);
         accSmooth = accLPF;
   } else {
         accSmooth = accADC;
   }
   accMag += (int32_t)accSmooth * accSmooth;

   if (sensors(SENSOR_MAG)) {
#if defined(MG_LPF_FACTOR)
         mgSmooth = (mgSmooth * (MG_LPF_FACTOR - 1) + magADC) / MG_LPF_FACTOR; // LPF for Magnetometer values
#define MAG_VALUE mgSmooth
#else
#define MAG_VALUE magADC
#endif
   }
}
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);

rotateV(&EstG.V, deltaGyroAngle);
if (sensors(SENSOR_MAG))
   rotateV(&EstM.V, deltaGyroAngle);

if (abs(accSmooth) < acc_25deg && abs(accSmooth) < acc_25deg && accSmooth > 0)
   f.SMALL_ANGLES_25 = 1;
else
   f.SMALL_ANGLES_25 = 0;

// 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) || f.SMALL_ANGLES_25) {
   for (axis = 0; axis < 3; axis++)
         EstG.A = (EstG.A * (float)cfg.gyro_cmpf_factor + accSmooth) * INV_GYR_CMPF_FACTOR;
}

if (sensors(SENSOR_MAG)) {
   for (axis = 0; axis < 3; axis++)
         EstM.A = (EstM.A * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
}

// Attitude of the estimated vector
angle = _atan2f(EstG.V.X, EstG.V.Z);
angle = _atan2f(EstG.V.Y, EstG.V.Z);

#ifdef MAG
if (sensors(SENSOR_MAG)) {
   // Attitude of the cross product vector GxM
   heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z);
   heading = heading + magneticDeclination;
   heading = heading / 10;

   if (heading > 180)
         heading = heading - 360;
   else if (heading < -180)
         heading = heading + 360;
}
#endif
}

// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v, float *delta)
{
struct fp_vector v_tmp = *v;
v->Z -= delta * v_tmp.X + delta * v_tmp.Y;
v->X += delta * v_tmp.Z - delta * v_tmp.Y;
v->Y += delta * v_tmp.Z + delta * v_tmp.X;
}

EstG.A数据跟EstG.V数据都是分开的，根本没有一点联系，EstG.V这个是通过陀螺仪角度经过旋转矩阵求得的角度，EstG.A完全就是融合一下加速度值，然后就什么也不做了，求角度什么的都是用的EstG.V，
那么对加速度融合计算到底是干什么的，互补不应该是（EstG.V*400 + EstG.A）/401=（陀螺仪角度*400+加速度角度）/401吗？

qwe2231695 发表于 2014-11-6 01:32:34

楼主啊你要认真看啊 {:sweat:}

EstG.A = (EstG.A * (float)cfg.gyro_cmpf_factor + accSmooth) * INV_GYR_CMPF_FACTOR;
这句话就是融合，是很巧妙的使用共用体融合。EstG.A是和EstG.V为共用体。EstG.V为这次的角度，在上面已经使用旋转矩阵法将陀螺仪积分进去了。

Gosling 发表于 2014-11-6 08:19:35

本帖最后由 Gosling 于 2014-11-6 08:21 编辑

qwe2231695 发表于 2014-11-6 01:32
楼主啊你要认真看啊

EstG.A = (EstG.A * (float)cfg.gyro_cmpf_factor + accSmo ...

一语点醒梦中人，EstG是个共用体，惯性思维直接看成结构体了，其中V跟A是一个东西，在这里绕了一天死循环了。非常谢谢

Gosling 发表于 2014-11-6 10:44:14

qwe2231695 发表于 2014-11-6 01:32
楼主啊你要认真看啊

EstG.A = (EstG.A * (float)cfg.gyro_cmpf_factor + accSmo ...

厚脸皮再请教个问题，
// Attitude of the estimated vector
sqGX_sqGZ = sq(EstG32.V.X) + sq(EstG32.V.Z);
invG = InvSqrt(sqGX_sqGZ + sq(EstG32.V.Y));
angle= _atan2(EstG32.V.X , EstG32.V.Z);
angle = _atan2(EstG32.V.Y , InvSqrt(sqGX_sqGZ)*sqGX_sqGZ);

mwc有的求PITCH用的arctan(y,z),有的是_atan2(EstG32.V.Y , InvSqrt(sqGX_sqGZ)*sqGX_sqGZ)这样，哪个是正确的？InvSqrt(sqGX_sqGZ)*sqGX_sqGZ什么意思额，还有sq函数是干什么用的？

qwe2231695 发表于 2014-11-6 18:32:44

InvSqrt是开方的倒数，两种都可以

Gosling 发表于 2014-11-6 23:45:29

qwe2231695 发表于 2014-11-6 18:32
InvSqrt是开方的倒数，两种都可以

恩，跟公式对了一下，对求角度还不是很清晰，再看看

whq5234970 发表于 2014-11-7 12:20:24

Gosling 发表于 2014-11-6 23:45
恩，跟公式对了一下，对求角度还不是很清晰，再看看

楼主和什么角度的公式对比的？

Gosling 发表于 2014-11-7 17:01:04

whq5234970 发表于 2014-11-7 12:20
楼主和什么角度的公式对比的？

网上有对MWC算法注释的，上面有小角处理后的公式，知网上硕士论文也有相应的公式推导，两者有些出入（应该是我没看懂）

whq5234970 发表于 2014-11-7 17:38:45

Gosling 发表于 2014-11-7 17:01
网上有对MWC算法注释的，上面有小角处理后的公式，知网上硕士论文也有相应的公式推导，两者有些出入（应 ...

楼主给个注释的链接哇。我也在看这个代码

Gosling 发表于 2014-11-7 17:56:19

whq5234970 发表于 2014-11-7 17:38
楼主给个注释的链接哇。我也在看这个代码

http://www.amobbs.com/forum.php?mod=viewthread&tid=5557397&highlight=mwc

Gosling 发表于 2014-11-8 12:33:00

qwe2231695 发表于 2014-11-6 18:32
InvSqrt是开方的倒数，两种都可以

两种方法都是可以是因为小角度近似吗？

chopininoctober 发表于 2015-5-27 15:40:31

qwe2231695 发表于 2014-11-6 01:32
楼主啊你要认真看啊

EstG.A = (EstG.A * (float)cfg.gyro_cmpf_factor + accSmo ...

您好，我最近正在研究mwc 2.2 的IMU程序，遇到了一些问题，请您帮忙，定有酬谢。 {:tongue:}

chopininoctober 发表于 2015-5-27 16:41:36

请问rotateV(&EstG.V, deltaGyroAngle) 中的 EstG.V 是怎么来的？

forwinry 发表于 2015-8-1 16:51:23

请问，如果要求YAW的角度，这种方法是不是就不能用了

forwinry 发表于 2015-8-13 13:56:20

chopininoctober 发表于 2015-5-27 16:41
请问rotateV(&EstG.V, deltaGyroAngle) 中的 EstG.V 是怎么来的？

兄弟，EstG.V那地方你弄懂了吗

页: [1]

amobbs.com 阿莫电子技术论坛's Archiver

MWC互补滤波代码有问题