From dad6ef4b951fe44936d59001fcbcb8ab86820a1b Mon Sep 17 00:00:00 2001 From: Yuesong Xie Date: Sun, 23 Dec 2018 11:22:21 -0500 Subject: [PATCH] Perception: fixed a bug in information matrix update step (#6190) * fixed a bug in information matrix update step * update to comply with lint requirement --- .../perception/fusion/common/information_filter.cc | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/modules/perception/fusion/common/information_filter.cc b/modules/perception/fusion/common/information_filter.cc index 860780eaba..f55f607606 100644 --- a/modules/perception/fusion/common/information_filter.cc +++ b/modules/perception/fusion/common/information_filter.cc @@ -139,23 +139,27 @@ bool InformationFilter::Correct( } cur_observation_ = cur_observation; cur_observation_uncertainty_ = cur_observation_uncertainty; + // global_uncertainty now stores information matrix global_uncertainty_ = global_uncertainty_.inverse(); + // tmp_states_ is information vector tmp_states_ = global_uncertainty_ * global_states_; + // cur_observation_uncertainty_ is now the inverse of covariance matrix cur_observation_uncertainty_ = cur_observation_uncertainty_.inverse(); if (last_observation_init_) { + // propate to current time last_observation_ = last_to_cur_transform_matrix_ * last_observation_; last_observation_uncertainty_ = last_to_cur_transform_matrix_ * last_observation_uncertainty_ * - last_to_cur_transform_matrix_.transpose() + + last_to_cur_transform_matrix_.transpose() + last_to_cur_env_uncertainty_; - last_observation_uncertainty_ = + last_observation_uncertainty_ = // transform to measurement space c_matrix_ * last_observation_uncertainty_ * c_matrix_.transpose(); - global_uncertainty_ = - c_matrix_.transpose() * global_uncertainty_ + + global_uncertainty_ = // update information matrix + global_uncertainty_ + (c_matrix_.transpose() * cur_observation_uncertainty_ * c_matrix_ - c_matrix_.transpose() * last_observation_uncertainty_.inverse() * c_matrix_); - tmp_states_ += + tmp_states_ += // update information vector (c_matrix_.transpose() * cur_observation_uncertainty_ * cur_observation_ - c_matrix_.transpose() * last_observation_uncertainty_.inverse() * -- GitLab