spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

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

pull/22560/head
Vivek Aithal 2021-10-14 11:57:50 -07:00 committed by GitHub
parent 7386d4f6c3
commit d22f57e36d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 8 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
selfdrive/locationd/locationd.cc
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) {

47
selfdrive/locationd/models/live_kf.cc
View File

@ -80,54 +80,13 @@ std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, std::vector<VectorXd> meas, std::vector<MatrixXdr> R) {
std::optional<Estimate> 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<Estimate> LiveKalman::predict_and_update_odo_speed(std::vector<VectorXd> speed, double t, int kind) {
std::vector<MatrixXdr> 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<Estimate> LiveKalman::predict_and_update_odo_trans(std::vector<VectorXd> trans, double t, int kind) {
std::vector<VectorXd> z;
std::vector<MatrixXdr> 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<Estimate> LiveKalman::predict_and_update_odo_rot(std::vector<VectorXd> rot, double t, int kind) {
std::vector<VectorXd> z;
std::vector<MatrixXdr> 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;
}