消除重力问题

[Jxpenggf]

在代码中Rwb.transpose() * ( ddp - gn ),有点迷惑，我觉得应该是加号啊，难道这个z轴向下？

[TouchDeeper]

@Jxpenggf 当加速度计各轴和东北天坐标系对齐，根据受力平衡，f+mg=ma. 加速度计通过测量 f 得到加速度测量，即imu_acc，imu_acc = f/m = a - g. 当加速度计各轴和东北天坐标系不对齐，Rwb*f + mg = ma，得出 imu_acc = Rbw(a-g)。以上推导与g的正负无关。东北天下g=(0,0,-9.81)。