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

[nyxrobotics]

概要
下記の部分でfitness scoreを考慮してukfをすれば安定するのではないかという提案です

hdl_localization/src/hdl_localization/pose_estimator.cpp

Line 197 in d571373

ukf->correct(observation);

hdl_localization/include/kkl/alg/unscented_kalman_filter.hpp

Line 141 in d571373

void correct(const VectorXt& measurement) {

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

目的
提案内容
タスク

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

[nyxrobotics]

おそらく下記が測定値のcovarianceに相当する場所

hdl_localization/include/kkl/alg/unscented_kalman_filter.hpp

Line 147 in d571373

ext_cov_pred.bottomRightCorner(K, K) = measurement_noise;

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

  /**
   * @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]

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