compareブランチでの実験結果を踏まえて、計算を書きなおした

剛体系で角速度やその時間微分を求めてから最後に静止系に持っていく
やり方を採用。