diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 8423339b59..00996b3198 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -27,17 +27,6 @@ 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(); @@ -72,12 +61,11 @@ 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); - 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 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(); VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment(STATE_ANGULAR_VELOCITY_START); - 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(); + 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(); VectorXd vel_device = device_from_ecef * vel_ecef; VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))).transpose(); @@ -97,7 +85,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 = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt(); + VectorXd vel_calib_std = ((this->calib_from_device * vel_device_cov) * this->calib_from_device.transpose()).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 @@ -179,8 +167,11 @@ void Localizer::handle_sensors(double current_time, const capnp::Listkf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) }); + this->gyro_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]) }); + } } // Accelerometer @@ -191,7 +182,10 @@ void Localizer::handle_sensors(double current_time, const capnp::Listdevice_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0; - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { Vector3d(-v[2], -v[1], -v[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]) }); + } } } } @@ -242,33 +236,36 @@ 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->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->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) }); + } } } void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { - VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); - VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); + this->cam_counter++; - VectorXd rot_calib_std = floatlist2vector(log.getRotStd()); - VectorXd trans_calib_std = floatlist2vector(log.getTransStd()); + 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() }); - this->posenet_stds.pop_front(); - this->posenet_stds.push_back(trans_calib_std[0]); + VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); + VectorXd trans_device_std = this->device_from_calib * floatlist2vector(log.getTransStd()); - // 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); + this->posenet_stds.pop_front(); + this->posenet_stds.push_back(trans_device_std[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() }); - 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_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() }); + } } void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { @@ -293,6 +290,11 @@ 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) { diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 551957737b..6c00a43a10 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -17,6 +17,9 @@ #include "models/live_kf.h" +#define VISION_DECIMATION 2 +#define SENSOR_DECIMATION 10 + #define POSENET_STD_HIST_HALF 20 class Localizer { @@ -58,4 +61,9 @@ 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/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index ac8aa6496a..37fbe87812 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 = 1 -VISION_DECIMATION = 1 +SENSOR_DECIMATION = 10 +VISION_DECIMATION = 2 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 2ab9488bc8..1963b05dd0 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -8bc705247a6f316da0a74d28d393101e8dcfcf57 \ No newline at end of file +e53498342bd6ba53e59976d284fc635368370e3d \ No newline at end of file diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 078b1cb4b0..ef93d6003e 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": 9.1, + "./locationd": 3.5, "selfdrive.controls.plannerd": 20.0, "./_ui": 15.0, "selfdrive.locationd.paramsd": 9.1,