Open LeisureLei opened 3 weeks ago
I don't understand how lidar noise works in func update_iterated_dyn_share_modified, like below: cov Ptemp = (P / R).inverse(); // Eigen::Matrix<scalar_type, 12, Eigen::Dynamic> h_T = hx.transpose(); Eigen::Matrix<scalar_type, 12, 12> HTH = hx.transpose() hx; P_temp.template block<12, 12>(0, 0) += HTH; cov P_inv = P_temp.inverse(); K_h = P_inv.template block<n, 12>(0, 0) hx.transpose() dyn_share.h; Kx.setZero(); // = cov::Zero(); K_x.template block<n, 12>(0, 0) = P_inv.template block<n, 12>(0, 0) HTH; Matrix<scalartype, n, 1> dx = K_h + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new;
This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.
I don't understand how lidar noise works in func update_iterated_dyn_share_modified, like below: cov Ptemp = (P / R).inverse(); // Eigen::Matrix<scalar_type, 12, Eigen::Dynamic> h_T = hx.transpose(); Eigen::Matrix<scalar_type, 12, 12> HTH = hx.transpose() hx; P_temp.template block<12, 12>(0, 0) += HTH; cov P_inv = P_temp.inverse(); K_h = P_inv.template block<n, 12>(0, 0) hx.transpose() dyn_share.h; Kx.setZero(); // = cov::Zero(); K_x.template block<n, 12>(0, 0) = P_inv.template block<n, 12>(0, 0) HTH; Matrix<scalartype, n, 1> dx = K_h + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new;