Passing through UKF covariance?

[jsupratman13]

これってUKFの共分散を渡さずにGICPのfitness scoreを渡しているのは何か理由がありますか？

_Originally posted by @Tacha-S in https://github.com/sbgisen/hdl_localization/issues/2#issuecomment-1302909737_

[jsupratman13]

Yes, you can retrieve the covariance using getCov(). But, I expect that the estimated covariance doesn't well represent the true uncertainty of the localization result because the UKF doesn't take scan matching errors into account.

_Originally posted by @koide3 in https://github.com/koide3/hdl_localization/issues/40#issuecomment-609516107_

[Tacha-S]

スキャンマッチングの結果を考慮していない(定数の観測共分散を使っている)のは確かですが、 それでも予測値と観測値の誤差からUKFの共分散は更新されていくはずなので、 その値が厳密には正しくなくても我々が使う範囲では許容できる程度の誤差に収まる可能性は十分にあります。

それを差し置いてfitness score(スキャンマッチングの精度であり共分散ではないもの)を時系列処理することもなく毎回与えるのはどう妥当なのかがわらかない

[jsupratman13]

メモ： ros メッセージの角度の分散がオイラー角であり、hdlの角度の分散がquaternionで出しているため、一度変更する必要がある。 https://stats.stackexchange.com/questions/119780/what-does-the-covariance-of-a-quaternion-mean

[YS-KID]

ひとまず7x7(poseとquaternion)の分散共分散行列はもってこれそうだったため、上で言及されているquatarnionをオイラー角に変換するところができるか試してみます

[Tacha-S]

https://github.com/sbgisen/hdl_localization/blob/master/apps/hdl_localization_nodelet.cpp

init_pose受け取ってリセットしてるよ

[jsupratman13]

https://github.com/sbgisen/hdl_localization/blob/master/apps/hdl_localization_nodelet.cpp

init_pose受け取ってリセットしてるよ

初期化以外はrvizでinitial poseを設定している時とrelocalize ROS serviceを呼び出した時だと思うけど、それ以外にリセットしている場所がある？

[Tacha-S]

それだけじゃない？ 共分散が小さくなるのはrvizでinitial poseを設定したからではなくて？

[jsupratman13]

それだけじゃない？ 共分散が小さくなるのはrvizでinitial poseを設定したからではなくて？

rvizではなくgazebo側でロボットを誘拐した上で共分散が変動していないのが気になる。ただ検証がまだ不充分なので調べている途中です。

[jsupratman13]

メモ: getCov() はこれのこと

https://github.com/sbgisen/hdl_localization/blob/d57137356dc7629fd40fcca509779366f20014df/include/kkl/alg/unscented_kalman_filter.hpp#L189

[jsupratman13]

メモ：ukfに渡す前のマッチング https://github.com/sbgisen/hdl_localization/blob/d57137356dc7629fd40fcca509779366f20014df/src/hdl_localization/pose_estimator.cpp#L177-L197

[nyxrobotics]

今の状況だとこのissueが無視されてPRマージされたように見えます。 なんの作業をして、どこがうまく行かなかったのか残してもらえると嬉しいです。 作業中は時間がないかもしれなせんが、その後時間取れたタイミングでもコメント残すようにしてください。

[nyxrobotics]

おそらく下記のようにukf->getCov()を所得した際に、スキャンマッチング結果の良し悪しに関係なさそうな0に近い値が出ていたのだと推測します。その現象自体はこちらでも再現できました。

https://github.com/sbgisen/hdl_localization/blob/d57137356dc7629fd40fcca509779366f20014df/src/hdl_localization/pose_estimator.cpp#L197

  ukf->correct(observation);
  Eigen::MatrixXf ukf_cov = ukf->getCov();
  Eigen::MatrixXf ukf_cov_7x7 = Eigen::MatrixXf::Identity(7, 7);
  ukf_cov_7x7.block<3, 3>(0, 0) = ukf_cov.block<3, 3>(0, 0);
  ukf_cov_7x7.block<3, 4>(0, 3) = ukf_cov.block<3, 4>(0, 6);
  ukf_cov_7x7.block<4, 3>(3, 0) = ukf_cov.block<4, 3>(6, 0);
  ukf_cov_7x7.block<4, 4>(3, 3) = ukf_cov.block<4, 4>(6, 6);

[nyxrobotics]

UnscentedKalmanFilteのcorrect関数内の処理は位置の結果に対してcovarianceを定数で入れてukfかけてるので、点群マッチングのスコアなどに影響受けない。 ジャンプした瞬間などは収束計算に影響が出るためcovarianceの値が変わりそうだが、時間が経つと収束してしまい望んだ動作ではないようです。

[nyxrobotics]

本件解決したと思うのでcloseします。質問あればコメントください。