IMUの加速度積分におけるtimestamp問題

[tongtybj]

IMUのキャリブ値をspinalに保存する場合、Flash Memoryに書き込むため、
1秒間ほど、spinalのtickは動かない。とまり、spinal内の時間は止まる。
ただし, rosserialは2.5秒おきに、spinalとrosとの時間の同期を行うため、
2.5秒後には時間のズレは補正される。

しかし、その補正により、/imuのstampは一気に1秒進む。
これにより、前後トピックのstampの差で積分を行う加速度積分計算が大きくずれる。
https://github.com/JSKAerialRobot/aerial_robot/blob/master/aerial_robot_estimation/src/sensor/imu.cpp#L119

なので、stampの差が大きすぎる(e.g. 通常の10倍以上)場合は、積分を行わないようにする.