飞控位置和速度校正算法问题

boshao

看minifly源码，有两个问题不太明白的，有那位大神能解答一下，谢谢了
1、为什么Z方向需要进行W*EWDT，而X,Y方向就不用呢，这个不太明白，源码如下：
/*位置校正*/
static void inavFilterCorrectPos(int axis, float dt, float e, float w)
{
    float ewdt = e * w * dt;
    estimator.pos[axis] += ewdt;
    estimator.vel[axis] += w * ewdt;
}
/*速度校正*/
static void inavFilterCorrectVel(int axis, float dt, float e, float w)
{
   estimator.vel[axis] += e * w * dt;
}
2、使用气压校正Z方向的加速度，权重也为什么需要是平方的呢？
/*加速度偏置校正*/
Axis3f accelBiasCorr = {{ 0, 0, 0}};

accelBiasCorr.z -= errPosZ  * sq(wBaro);
float accelBiasCorrMagnitudeSq = sq(accelBiasCorr.x) + sq(accelBiasCorr.y) + sq(accelBiasCorr.z);

电子之旅

Z轴高度的预估是由加速度计和气压两个来预估的，所以加速度计占一部分权重，气压计占一部分权重，W就是权重的意思

boshao

版主，你好，还是不太理解，例如： estimator.vel[axis] += w * ewdt; 其实这个estimator.vel[aixs] = estimator.vel[aixs] + w * w * e * dt，这里需要把W权重平方，这里有具体的推导吗？我看源码里面有很多地方计算速度或加速度都需要将W权重进行平方，而估算位置时不需要，是不是将W权重平方后，加快了估算速度或加速度迫近真实的速度或加速度？麻烦版主，继续解答一下，谢谢

Ronniehu

boshao 发表于 2019-5-13 22:07
版主，你好，还是不太理解，例如： estimator.vel[axis] += w * ewdt; 其实这个estimator.vel[aixs] = esti ...

这个貌似没看到详细的推导，很多说的都很含糊。