search

sbgisen / hdl_localization

Real-time 3D localization using a (velodyne) 3D LIDAR
BSD 2-Clause "Simplified" License
2 stars 2 forks source link

内部処理のukfにfitness scoreを反映した場合の挙動検証 #12

Open nyxrobotics opened 1 year ago

nyxrobotics commented 1 year ago

概要 下記の部分でfitness scoreを考慮してukfをすれば安定するのではないかという提案です https://github.com/sbgisen/hdl_localization/blob/d57137356dc7629fd40fcca509779366f20014df/src/hdl_localization/pose_estimator.cpp#L197

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

測定値のcovarianceがどの変数か調査中

目的 提案内容 タスク

nyxrobotics commented 1 year ago

おそらく下記が測定値のcovarianceに相当する場所 https://github.com/sbgisen/hdl_localization/blob/d57137356dc7629fd40fcca509779366f20014df/include/kkl/alg/unscented_kalman_filter.hpp#L147

下記のように書き換えたら多少挙動は変化したが良くなったとは言えない。もう少し調査が必要そうです。

  /**
   * @brief correct
   * @param measurement  measurement vector
   * @param fitness_score  scam matching fitness score (The better the accuracy of the matching, the smaller the value.)
   */
  void correct(const VectorXt& measurement, double fitness_score) {
    // create extended state space which includes error variances
    VectorXt ext_mean_pred = VectorXt::Zero(N + K, 1);
    MatrixXt ext_cov_pred = MatrixXt::Zero(N + K, N + K);
    ext_mean_pred.topLeftCorner(N, 1) = VectorXt(mean);
    ext_cov_pred.topLeftCorner(N, N) = MatrixXt(cov);
    MatrixXt measurement_covariance_matrix = fitness_score * MatrixXt::Identity(measurement_noise.cols(), measurement_noise.rows());
    ext_cov_pred.bottomRightCorner(K, K) = measurement_covariance_matrix;
nyxrobotics commented 1 year ago

UKFの計算手順などは下記を参照しています https://arx.appi.keio.ac.jp/wp-content/uploads/2011/05/10102.pdf