重心座標での制御に統一する

[tongtybj]

今は静的変形を過程で、
FCU_frameのyaw, position = CoG_frameのyaw.position
になっているけど、厳密ではない。

特にsimulation/実機でo -> lに変形中、最後の時、yawは大きくズレル。

たぶんここの問題だが、
そもそも1. COG_frameの向きを慣性主軸方向に合わせる必要はないうえに、
2. 非線形なジャイロ項が補償されていないから

この問題がおきているのかも。

[tongtybj]

#73 にて実装し、実機で確認済み