From e9db5723ef348954118643501a92cf0715402fea Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 6 May 2021 11:01:58 +0200 Subject: [PATCH] Locationd 100 Hz (#20816) * fix std transform * 100Hz * new ref * no more decimation * clean up confusing maths * static typing * Revert "static typing" This reverts commit 23d87337de648e629fbd35dd8c04a740bbefca47. * 100Hz costs more * move normalization into core * add quat idxs * add big eps * this is not safe in the filter * more sensible * updates to rednose * not tested * normalize in python too * update rednose * nan check * check for infs too * all should be finite * update ref * rednose pr now in master Co-authored-by: Harald Schafer --- rednose_repo | 2 +- selfdrive/locationd/locationd.cc | 87 ++++++++++++---------- selfdrive/locationd/locationd.h | 9 +-- selfdrive/locationd/models/live_kf.cc | 6 +- selfdrive/locationd/models/live_kf.py | 4 +- selfdrive/locationd/models/loc_kf.py | 9 +-- selfdrive/locationd/test/test_locationd.py | 4 +- selfdrive/test/process_replay/ref_commit | 2 +- selfdrive/test/test_onroad.py | 2 +- 9 files changed, 61 insertions(+), 64 deletions(-) diff --git a/rednose_repo b/rednose_repo index f069de89..7fff192e 160000 --- a/rednose_repo +++ b/rednose_repo @@ -1 +1 @@ -Subproject commit f069de89f6ccd277a557fb14590f8cb0fb069a8a +Subproject commit 7fff192e234cd91f50deed12adb3616d96f12d3e diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 08f105dd..1f9b7dee 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -30,6 +30,17 @@ static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder me meas.setValid(valid); } + +static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) { + // To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix + return ((rot_matrix * cov_in) * rot_matrix.transpose()); +} + +static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) { + // Stds cannot be rotated like values, only covariances can be rotated + return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt(); +} + Localizer::Localizer() { this->kf = std::make_unique(); this->reset_kalman(); @@ -64,11 +75,12 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { VectorXd calibrated_orientation_ecef = rot2euler(this->calib_from_device * device_from_ecef); VectorXd acc_calib = this->calib_from_device * predicted_state.segment(STATE_ACCELERATION_START); - VectorXd acc_calib_std = ((this->calib_from_device * predicted_cov.block(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START)) * this->calib_from_device.transpose()).diagonal().array().sqrt(); + MatrixXdr acc_calib_cov = predicted_cov.block(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START); + VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt(); VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment(STATE_ANGULAR_VELOCITY_START); - MatrixXdr vel_angular_err = predicted_cov.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START); - VectorXd ang_vel_calib_std = ((this->calib_from_device * vel_angular_err) * this->calib_from_device.transpose()).diagonal().array().sqrt(); + MatrixXdr vel_angular_cov = predicted_cov.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START); + VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt(); VectorXd vel_device = device_from_ecef * vel_ecef; VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))).transpose(); @@ -88,7 +100,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt(); VectorXd vel_calib = this->calib_from_device * vel_device; - VectorXd vel_calib_std = ((this->calib_from_device * vel_device_cov) * this->calib_from_device.transpose()).diagonal().array().sqrt(); + VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt(); VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef); //orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned @@ -170,11 +182,8 @@ void Localizer::handle_sensors(double current_time, const capnp::Listgyro_counter++; - if (this->gyro_counter % SENSOR_DECIMATION == 0) { - auto v = sensor_reading.getGyroUncalibrated().getV(); - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) }); - } + auto v = sensor_reading.getGyroUncalibrated().getV(); + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) }); } // Accelerometer @@ -185,10 +194,7 @@ void Localizer::handle_sensors(double current_time, const capnp::Listdevice_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0; - this->acc_counter++; - if (this->acc_counter % SENSOR_DECIMATION == 0) { - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { Vector3d(-v[2], -v[1], -v[0]) }); - } + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { Vector3d(-v[2], -v[1], -v[0]) }); } } } @@ -239,36 +245,33 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R } void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) { - this->speed_counter++; - - if (this->speed_counter % SENSOR_DECIMATION == 0) { - this->kf->predict_and_observe(current_time, OBSERVATION_ODOMETRIC_SPEED, { (VectorXd(1) << log.getVEgoRaw()).finished() }); - this->car_speed = std::abs(log.getVEgo()); - if (this->car_speed < 1e-3) { - this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) }); - } + //this->kf->predict_and_observe(current_time, OBSERVATION_ODOMETRIC_SPEED, { (VectorXd(1) << log.getVEgoRaw()).finished() }); + this->car_speed = std::abs(log.getVEgo()); + if (this->car_speed < 1e-3) { + this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) }); } } void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { - this->cam_counter++; + VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); + VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); - if (this->cam_counter % VISION_DECIMATION == 0) { - VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); - VectorXd rot_device_std = (this->device_from_calib * floatlist2vector(log.getRotStd())) * 10.0; - 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() }); + VectorXd rot_calib_std = floatlist2vector(log.getRotStd()); + VectorXd trans_calib_std = floatlist2vector(log.getTransStd()); - VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); - VectorXd trans_device_std = this->device_from_calib * floatlist2vector(log.getTransStd()); + this->posenet_stds.pop_front(); + this->posenet_stds.push_back(trans_calib_std[0]); - this->posenet_stds.pop_front(); - this->posenet_stds.push_back(trans_device_std[0]); + // 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); - trans_device_std *= 10.0; - 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() }); - } + 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() }); + 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() }); } void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { @@ -285,6 +288,14 @@ void Localizer::reset_kalman(double current_time) { this->reset_kalman(current_time, init_x.segment<4>(3), init_x.head(3)); } +void Localizer::finite_check(double current_time) { + bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all(); + if (!all_finite){ + LOGE("Non-finite values detected, kalman reset"); + this->reset_kalman(current_time); + } +} + void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos) { // too nonlinear to init on completely wrong VectorXd init_x = this->kf->get_initial_x(); @@ -293,11 +304,6 @@ void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_x.head(3) = init_pos; this->kf->init_state(init_x, init_P, current_time); - - this->gyro_counter = 0; - this->acc_counter = 0; - this->speed_counter = 0; - this->cam_counter = 0; } void Localizer::handle_msg_bytes(const char *data, const size_t size) { @@ -322,6 +328,7 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) { } else if (log.isLiveCalibration()) { this->handle_live_calib(t, log.getLiveCalibration()); } + this->finite_check(); } kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index ea8a0486..b99993d0 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -17,9 +17,6 @@ #include "models/live_kf.h" -#define VISION_DECIMATION 2 -#define SENSOR_DECIMATION 10 - #define POSENET_STD_HIST_HALF 20 class Localizer { @@ -30,6 +27,7 @@ public: void reset_kalman(double current_time = NAN); void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos); + void finite_check(double current_time = NAN); kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, bool inputsOK, bool sensorsOK, bool gpsOK); @@ -61,9 +59,4 @@ private: int64_t unix_timestamp_millis = 0; double last_gps_fix = 0; bool device_fell = false; - - int gyro_counter = 0; - int acc_counter = 0; - int speed_counter = 0; - int cam_counter = 0; }; diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc index 073a3b89..6661b06f 100755 --- a/selfdrive/locationd/models/live_kf.cc +++ b/selfdrive/locationd/models/live_kf.cc @@ -39,8 +39,9 @@ LiveKalman::LiveKalman() { } // init filter - this->filter = std::make_shared(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x), get_mapmat(initial_P), - this->dim_state, this->dim_state_err, 0, 0, 0, std::vector(), std::vector(), 0.2); + this->filter = std::make_shared(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x), + get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector(), + std::vector{3}, std::vector(), 0.2); } void LiveKalman::init_state(VectorXd& state, VectorXd& covs_diag, double filter_time) { @@ -92,7 +93,6 @@ std::optional LiveKalman::predict_and_observe(double t, int kind, std: r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R)); break; } - this->filter->normalize_state(STATE_ECEF_ORIENTATION_START, STATE_ECEF_ORIENTATION_END); return r; } diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index c0a32181..3df1416a 100755 --- a/selfdrive/locationd/models/live_kf.py +++ b/selfdrive/locationd/models/live_kf.py @@ -55,8 +55,8 @@ class LiveKalman(): # state covariance initial_P_diag = np.array([1e16, 1e16, 1e16, - 1e6, 1e6, 1e6, - 1e4, 1e4, 1e4, + 10**2, 10**2, 10**2, + 10**2, 10**2, 10**2, 1**2, 1**2, 1**2, 0.05**2, 0.05**2, 0.05**2, 0.02**2, diff --git a/selfdrive/locationd/models/loc_kf.py b/selfdrive/locationd/models/loc_kf.py index 08ce672b..27394942 100755 --- a/selfdrive/locationd/models/loc_kf.py +++ b/selfdrive/locationd/models/loc_kf.py @@ -382,9 +382,11 @@ class LocKalman(): self.computer = LstSqComputer(generated_dir, N) self.max_tracks = max_tracks + self.quaternion_idxs = [3,] + [(self.dim_main + i*self.dim_augment + 3)for i in range(self.N)] + # init filter self.filter = EKF_sym(generated_dir, name, Q, x_initial, P_initial, self.dim_main, self.dim_main_err, - N, self.dim_augment, self.dim_augment_err, self.maha_test_kinds) + N, self.dim_augment, self.dim_augment_err, self.maha_test_kinds, self.quaternion_idxs) @property def x(self): @@ -453,11 +455,6 @@ class LocKalman(): # Should not continue if the quats behave this weirdly if not 0.1 < quat_norm < 10: raise RuntimeError("Sir! The filter's gone all wobbly!") - self.filter.normalize_state(3, 7) - for i in range(self.N): - d1 = self.dim_main - d3 = self.dim_augment - self.filter.normalize_state(d1 + d3 * i + 3, d1 + d3 * i + 7) return r def get_R(self, kind, n): diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 37fbe878..ac8aa649 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -12,8 +12,8 @@ from common.params import Params from selfdrive.manager.process_config import managed_processes -SENSOR_DECIMATION = 10 -VISION_DECIMATION = 2 +SENSOR_DECIMATION = 1 +VISION_DECIMATION = 1 LIBLOCATIOND_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '../liblocationd.so')) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 5e83d3bd..e1516107 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f1515b6efca6de7305d411355f286d2c6b9fb19b \ No newline at end of file +fcfadaaad7f452fdbb69c1057058327b0d2004bf \ No newline at end of file diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index ef93d600..078b1cb4 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -20,7 +20,7 @@ from tools.lib.logreader import LogReader PROCS = { "selfdrive.controls.controlsd": 50.0, "./loggerd": 45.0, - "./locationd": 3.5, + "./locationd": 9.1, "selfdrive.controls.plannerd": 20.0, "./_ui": 15.0, "selfdrive.locationd.paramsd": 9.1,