An problem with UKF

[Freeskylover]

void predict(const VectorXt& control) { // calculate sigma points ensurePositiveFinite(cov); computeSigmaPoints(mean, cov, sigma_points); for (int i = 0; i < S; i++) { sigma_points.row(i) = system.f(sigma_points.row(i), control);
}

const auto& R = process_noise;

// unscented transform
VectorXt mean_pred(mean.size());
MatrixXt cov_pred(cov.rows(), cov.cols());

mean_pred.setZero();
cov_pred.setZero();
for (int i = 0; i < S; i++) {
  mean_pred += weights[i] * sigma_points.row(i);
}
for (int i = 0; i < S; i++) {
  VectorXt diff = sigma_points.row(i).transpose() - mean;  
  cov_pred += weights[i] * diff * diff.transpose();
}
cov_pred += R;

mean = mean_pred;
cov = cov_pred;

}

I do not know why not using mean_pred to calculate the cov_pred.

[koide3]

Hi @Freeskylover , Thank you for reporting the issue. You are right, it should be mean_pred. I'll fix it. Did you see any performance issues caused by this mistake?

[Freeskylover]

Hi @Freeskylover , Thank you for reporting the issue. You are right, it should be mean_pred. I'll fix it. Did you see any performance issues caused by this mistake?

Thank for your reply, I have not run this coding, because the bag file is too big so that the download is not finished, i am just reading this code recently.

[koide3]

OK, maybe the bug was affecting the stability of UKF. I committed the fixed code. Thanks again for reporting it!