回転と並進でそれぞれ補正値にゲインを掛ける

[nyxrobotics]

概要
点群マッチングで補正する移動量に１未満の係数を掛けてゲインを下げることで発散を防ぐパラメータを追加したい

目的
現状でもカルマンフィルタの定数で応答性を調整できるが以下の課題がある

• 点群マッチングについてはcovarianceが正しくはわからない
• waypoint follower等で使用する際は収束が離散的になると蛇行して走行経路をはみ出す危険がある
• IMUやodometryを使う場合、局所的な相対角度（＋odometryのときは相対位置）はある程度連続かつ信頼できる状態なので点群マッチングは大域的で良い

上記の課題を解決するため大域的な収束位置は同じになるように（時刻の同期ズレ・位相遅れがないように）補正のゲインを下げる機能を実装したい

提案内容
カルマンフィルタによって導出した「推定位置」とスキャンマッチングによって求まる「測定位置」を比較し、ゲインを掛けて測定位置を推定位置に寄せる
もともとカルマンフィルタの実装が怪しい今の状態でこれを実装すると通常のカルマンフィルタではなくなるので
カルマンフィルタをリファクタリングするならそれを先にやったほうが良い（#26）

タスク

• 細かいタスクに分解できているなら書き出す