mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 16:23:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

remove confusing logic in live_kf, move to locationd (#22558)

Browse Source 
old-commit-hash: d22f57e36db00f1e6873e3a50f22b8ab2fa94232
This commit is contained in:
Vivek Aithal
2021-10-14 11:57:50 -07:00
committed by GitHub
parent b8215f213c
commit 0bc7ccfbaa
2 changed files with 8 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.cc
+5 -5
View File
@@ -325,13 +325,13 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry
// Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
trans_calib_std *= 10.0;
rot_calib_std *= 10.0;
VectorXd rot_device_std = rotate_std(this->device_from_calib, rot_calib_std);
VectorXd trans_device_std = rotate_std(this->device_from_calib, trans_calib_std);
MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal();
MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal();
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
{ (VectorXd(rot_device.rows() + rot_device_std.rows()) << rot_device, rot_device_std).finished() });
{ rot_device }, { rot_device_cov });
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
{ (VectorXd(trans_device.rows() + trans_device_std.rows()) << trans_device, trans_device_std).finished() });
{ trans_device }, { trans_device_cov });
}
void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {