From d22f57e36db00f1e6873e3a50f22b8ab2fa94232 Mon Sep 17 00:00:00 2001 From: Vivek Aithal Date: Thu, 14 Oct 2021 11:57:50 -0700 Subject: [PATCH] remove confusing logic in live_kf, move to locationd (#22558) --- selfdrive/locationd/locationd.cc | 10 +++--- selfdrive/locationd/models/live_kf.cc | 47 ++------------------------- 2 files changed, 8 insertions(+), 49 deletions(-) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 2e58f1fb7f..87657853b3 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -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) { diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc index 00080ab839..4bc655821f 100755 --- a/selfdrive/locationd/models/live_kf.cc +++ b/selfdrive/locationd/models/live_kf.cc @@ -80,54 +80,13 @@ std::vector LiveKalman::get_R(int kind, int n) { std::optional LiveKalman::predict_and_observe(double t, int kind, std::vector meas, std::vector R) { std::optional r; - switch (kind) { - case OBSERVATION_CAMERA_ODO_TRANSLATION: - r = this->predict_and_update_odo_trans(meas, t, kind); - break; - case OBSERVATION_CAMERA_ODO_ROTATION: - r = this->predict_and_update_odo_rot(meas, t, kind); - break; - case OBSERVATION_ODOMETRIC_SPEED: - r = this->predict_and_update_odo_speed(meas, t, kind); - break; - default: - if (R.size() == 0) { - R = this->get_R(kind, meas.size()); - } - r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R)); - break; + if (R.size() == 0) { + R = this->get_R(kind, meas.size()); } + r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R)); return r; } -std::optional LiveKalman::predict_and_update_odo_speed(std::vector speed, double t, int kind) { - std::vector R; - R.assign(speed.size(), (MatrixXdr(1, 1) << std::pow(0.2, 2)).finished().asDiagonal()); - return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(speed), get_vec_mapmat(R)); -} - -std::optional LiveKalman::predict_and_update_odo_trans(std::vector trans, double t, int kind) { - std::vector z; - std::vector R; - for (VectorXd& trns : trans) { - assert(trns.size() == 6); // TODO remove - z.push_back(trns.head(3)); - R.push_back(trns.segment<3>(3).array().square().matrix().asDiagonal()); - } - return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(z), get_vec_mapmat(R)); -} - -std::optional LiveKalman::predict_and_update_odo_rot(std::vector rot, double t, int kind) { - std::vector z; - std::vector R; - for (VectorXd& rt : rot) { - assert(rt.size() == 6); // TODO remove - z.push_back(rt.head(3)); - R.push_back(rt.segment<3>(3).array().square().matrix().asDiagonal()); - } - return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(z), get_vec_mapmat(R)); -} - Eigen::VectorXd LiveKalman::get_initial_x() { return this->initial_x; }