ukfのシステムをrobot_localizationに合わせる

[nyxrobotics]

概要
下記に記載されているシステムのメンバーを変更したい

hdl_localization/include/kkl/alg/unscented_kalman_filter.hpp

Line 108 in 62a8828

* @note state = [px, py, pz, vx, vy, vz, qw, qx, qy, qz, acc_bias_x, acc_bias_y, acc_bias_z, gyro_bias_x, gyro_bias_y, gyro_bias_z]

robot_localizationは下記
https://github.com/cra-ros-pkg/robot_localization/blob/ea976f9e65eac505f5cc7d3197ae080de4e5c10b/include/robot_localization/filter_common.hpp#L41-L58

目的

• IMUの加速度・角速度のバイアスをUKFで推定したいようだがcorrect処理での入力がNDTで補正した位置・姿勢で、時刻や距離の分解能・精度的に達成できる見込みがない
• Quaternionの四つの成分をシステムとして持つ場合covarianceの次元が角速度と噛み合わないためカルマンフィルタのシステム内ではオイラー角に変換したい
• Odometry型でパブリッシュする場合も姿勢のcovarianceはオイラー角なのでROS的にはオイラー角のほうが見やすい

方針概要

• 当初はhdl_localization内でukfをかけずにrobot_localizationでukfを掛けるようにしたかった
• しかしhdl_localizationは内部のukfで推定した位置を原点としてndtを掛ける処理となっており、上記を達成しようとするとhdl_localizationはrobot_localizationと交互にデータをやり取りする形式になってしまう
• この場合hdl_localizationはrobot_localizationがないと動作しなくなってしまいhdl_localization単体のデバッグができなくなる
• 仕方がないのでhdl_localization内のukfは単に離散値を適当に収束させるためのものとして残しパラメータはそれらしくなるよう調整する
• パラメータを調整しやすいようにhdl_localizationと次元を揃えたい

提案内容
タスク

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