原子四轴 V1.2 姿态估算程序疑问

曾超fzu

        estimator.accZ = accZRemovalDead;
        estimator.accZ -= 0.02f * errorZ * weight * weight * dt;        /*补偿加速度*/               


        /*位置和速度估测*/
        estimator.positonZ += estimator.velocityZ * dt + estimator.accZ * dt * dt / 2.0f;
        estimator.velocityZ += estimator.accZ * dt;

        /*高度误差*/
        errorZ = fusedHeight - estimator.positonZ;

以上是原子四轴V1.2版本中的程序，errorZ 为实际高度-估算高度，如果errorZ>0，则说明估算高度偏小，由估算高度的计算公式可知：estimator.positonZ += estimator.velocityZ * dt + estimator.accZ * dt * dt / 2.0f;是估算加速度偏小造成的，所以补偿加速度应该让估算的加速度值变大一点，而补偿加速度的公式为estimator.accZ -= 0.02f * errorZ * weight * weight * dt;这样计算不是会让估算加速度的值更小吗？是我哪里理解错了呢？请教各位大佬

曾超fzu

lycreturn 发表于 2018-8-13 15:19
更简单的验证办法，你觉得符号有问题，你改一下，然后把代码刷到MiniFly上，你看还能不能正常定高

estimator.accZ -= 0.02f * errorZ * weight * weight * dt;        /*补偿加速度*/  
我说的是这个公式  我觉得应该把“-”变成“+”
昨天试了一下，把-换成+或者把这句屏蔽掉小四轴都能正常定高，后来仔细看了高度估算这部分的程序，发现estimator.accZ 这个数据是直接从mpu6500加速度计里面读取Z轴加速度后经过低通滤波处理后赋值给estimator.accZ，和上一次的补偿没关系，也就是说每次加速度补偿并没有累加到下一次，而这个加速度补偿的参数为0.02*weight*weight*dt，这个值是非常小的，几乎不起作用，所以这里换成+或者-或者把这句去掉影响都不大，不过从逻辑上，我个人认为换成“+”应该更准确一点。

peng1554

帮顶！

lycreturn

注意  estimator.positonZ += estimator.velocityZ * dt + estimator.accZ * dt * dt / 2.0f;     是 estimator.positonZ +=，是一个增量， 而不是 estimator.positonZ =简单赋值

lycreturn

更简单的验证办法，你觉得符号有问题，你改一下，然后把代码刷到MiniFly上，你看还能不能正常定高