From 3420707ad58b060333246eb43db09474bb336d39 Mon Sep 17 00:00:00 2001 From: Joost Wooning Date: Tue, 20 Apr 2021 11:56:43 +0200 Subject: [PATCH] convert locationd to c++ (#20622) * live_kf to c++ * first locationd code * Running in process_replay * locationd handle cam_odo and live_calib * log event handlers * working message receiving * compiling message sending * correctly sending some messages * correct receiving and sending * update ref_commit with some all_alive_and_valid being false, minor fixes * fix std abs * linking on device fix * fix cpu usage test * generate kf constants and defines * fix replay test * replay without acks, cleanup * operate on bytearray messages * cleanup * send msg fix * small sleep, less flaky test * remove python locationd * review feedback * bump rednose --- SConstruct | 16 +- common/transformations/coordinates.cc | 2 - common/transformations/coordinates.hpp | 6 + rednose_repo | 2 +- release/files_common | 5 +- selfdrive/locationd/.gitignore | 3 +- selfdrive/locationd/SConscript | 10 +- selfdrive/locationd/locationd.cc | 378 ++++++++++++++++++ selfdrive/locationd/locationd.h | 69 ++++ selfdrive/locationd/locationd.py | 353 ---------------- selfdrive/locationd/models/live_kf.cc | 140 +++++++ selfdrive/locationd/models/live_kf.h | 56 +++ selfdrive/locationd/models/live_kf.py | 155 +++---- selfdrive/manager/process_config.py | 2 +- .../test/process_replay/process_replay.py | 1 + selfdrive/test/profiling/profiler.py | 2 - selfdrive/test/test_onroad.py | 3 +- 17 files changed, 730 insertions(+), 473 deletions(-) create mode 100755 selfdrive/locationd/locationd.cc create mode 100755 selfdrive/locationd/locationd.h delete mode 100755 selfdrive/locationd/locationd.py create mode 100755 selfdrive/locationd/models/live_kf.cc create mode 100755 selfdrive/locationd/models/live_kf.h diff --git a/SConstruct b/SConstruct index 3a5cd5f25..d0ac47c49 100644 --- a/SConstruct +++ b/SConstruct @@ -366,19 +366,19 @@ Export('common', 'gpucommon', 'visionipc') rednose_config = { 'generated_folder': '#selfdrive/locationd/models/generated', 'to_build': { - 'live': ('#selfdrive/locationd/models/live_kf.py', True), - 'car': ('#selfdrive/locationd/models/car_kf.py', True), + 'live': ('#selfdrive/locationd/models/live_kf.py', True, ['live_kf_constants.h']), + 'car': ('#selfdrive/locationd/models/car_kf.py', True, []), }, } if arch != "aarch64": rednose_config['to_build'].update({ - 'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True), - 'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True), - 'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False), - 'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False), - 'feature_handler_5': ('#rednose/helpers/feature_handler.py', False), - 'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True), + 'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []), + 'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, []), + 'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, []), + 'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, []), + 'feature_handler_5': ('#rednose/helpers/feature_handler.py', False, []), + 'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True, []), }) Export('rednose_config') diff --git a/common/transformations/coordinates.cc b/common/transformations/coordinates.cc index 8a1aa0ad7..b729ac3d8 100644 --- a/common/transformations/coordinates.cc +++ b/common/transformations/coordinates.cc @@ -6,8 +6,6 @@ #include "coordinates.hpp" -#define DEG2RAD(x) ((x) * M_PI / 180.0) -#define RAD2DEG(x) ((x) * 180.0 / M_PI) double a = 6378137; // lgtm [cpp/short-global-name] diff --git a/common/transformations/coordinates.hpp b/common/transformations/coordinates.hpp index d8beb59ea..f5ba0d3fe 100644 --- a/common/transformations/coordinates.hpp +++ b/common/transformations/coordinates.hpp @@ -1,5 +1,8 @@ #pragma once +#define DEG2RAD(x) ((x) * M_PI / 180.0) +#define RAD2DEG(x) ((x) * 180.0 / M_PI) + struct ECEF { double x, y, z; Eigen::Vector3d to_vector(){ @@ -9,6 +12,9 @@ struct ECEF { struct NED { double n, e, d; + Eigen::Vector3d to_vector(){ + return Eigen::Vector3d(n, e, d); + } }; struct Geodetic { diff --git a/rednose_repo b/rednose_repo index a0eb4d249..f069de89f 160000 --- a/rednose_repo +++ b/rednose_repo @@ -1 +1 @@ -Subproject commit a0eb4d249e4739cae7e4da4c38ab81c4cafa5df9 +Subproject commit f069de89f6ccd277a557fb14590f8cb0fb069a8a diff --git a/release/files_common b/release/files_common index d5b2be974..30c6cd9cc 100644 --- a/release/files_common +++ b/release/files_common @@ -289,12 +289,15 @@ selfdrive/locationd/generated/ubx.h selfdrive/locationd/generated/gps.cpp selfdrive/locationd/generated/gps.h -selfdrive/locationd/locationd.py +selfdrive/locationd/locationd.h +selfdrive/locationd/locationd.cc selfdrive/locationd/paramsd.py selfdrive/locationd/models/.gitignore selfdrive/locationd/models/live_kf.py selfdrive/locationd/models/car_kf.py selfdrive/locationd/models/constants.py +selfdrive/locationd/models/live_kf.h +selfdrive/locationd/models/live_kf.cc selfdrive/locationd/calibrationd.py diff --git a/selfdrive/locationd/.gitignore b/selfdrive/locationd/.gitignore index 6ea757462..526890278 100644 --- a/selfdrive/locationd/.gitignore +++ b/selfdrive/locationd/.gitignore @@ -1,4 +1,5 @@ ubloxd ubloxd_test params_learner -paramsd \ No newline at end of file +paramsd +locationd diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index d170cd04a..b7d0e128b 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -1,8 +1,7 @@ -Import('env', 'common', 'cereal', 'messaging') +Import('env', 'common', 'cereal', 'messaging', 'libkf', 'transformations') loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'kaitai', 'pthread'] - if GetOption('kaitai'): generated = Dir('generated').srcnode().abspath cmd = f"kaitai-struct-compiler --target cpp_stl --outdir {generated} $SOURCES" @@ -10,3 +9,10 @@ if GetOption('kaitai'): env.Command(['generated/gps.cpp', 'generated/gps.h'], 'gps.ksy', cmd) env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp"], LIBS=loc_libs) + +ekf_sym_cc = env.Object("#rednose/helpers/ekf_sym.cc") +lenv = env.Clone() +lenv["_LIBFLAGS"] += f' {libkf[0].get_labspath()}' +locationd = lenv.Program("locationd", ["locationd.cc", "models/live_kf.cc", ekf_sym_cc], + LIBS=loc_libs + transformations) +lenv.Depends(locationd, libkf) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc new file mode 100755 index 000000000..fa2e85f8e --- /dev/null +++ b/selfdrive/locationd/locationd.cc @@ -0,0 +1,378 @@ +#include "locationd.h" + +using namespace EKFS; +using namespace Eigen; + +ExitHandler do_exit; + +static VectorXd floatlist2vector(const capnp::List::Reader& floatlist) { + VectorXd res(floatlist.size()); + for (int i = 0; i < floatlist.size(); i++) { + res[i] = floatlist[i]; + } + return res; +} + +static Vector4d quat2vector(const Quaterniond& quat) { + return Vector4d(quat.w(), quat.x(), quat.y(), quat.z()); +} + +static Quaterniond vector2quat(const VectorXd& vec) { + return Quaterniond(vec(0), vec(1), vec(2), vec(3)); +} + +static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) { + meas.setValue(kj::arrayPtr(val.data(), val.size())); + meas.setStd(kj::arrayPtr(std.data(), std.size())); + meas.setValid(valid); +} + +Localizer::Localizer() { + this->kf = std::make_unique(); + this->reset_kalman(); + + this->calib = Vector3d(0.0, 0.0, 0.0); + this->device_from_calib = MatrixXdr::Identity(3, 3); + this->calib_from_device = MatrixXdr::Identity(3, 3); + + for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) { + this->posenet_stds.push_back(10.0); + } + + VectorXd ecef_pos = this->kf->get_x().segment(STATE_ECEF_POS_START); + this->converter = std::make_unique((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); +} + +void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { + VectorXd predicted_state = this->kf->get_x(); + MatrixXdr predicted_cov = this->kf->get_P(); + VectorXd predicted_std = predicted_cov.diagonal().array().sqrt(); + + VectorXd fix_ecef = predicted_state.segment(STATE_ECEF_POS_START); + ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) }; + VectorXd fix_ecef_std = predicted_std.segment(STATE_ECEF_POS_ERR_START); + VectorXd vel_ecef = predicted_state.segment(STATE_ECEF_VELOCITY_START); + VectorXd vel_ecef_std = predicted_std.segment(STATE_ECEF_VELOCITY_ERR_START); + VectorXd fix_pos_geo_vec = this->get_position_geodetic(); + //fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo) + VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))); + VectorXd orientation_ecef_std = predicted_std.segment(STATE_ECEF_ORIENTATION_ERR_START); + MatrixXdr device_from_ecef = quat2rot(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))).transpose(); + 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(); + 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(); + + VectorXd vel_device = device_from_ecef * vel_ecef; + VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))).transpose(); + MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN); + condensed_cov.topLeftCorner() = + predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); + condensed_cov.topRightCorner() = + predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START); + condensed_cov.bottomRightCorner() = + predicted_cov.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START); + condensed_cov.bottomLeftCorner() = + predicted_cov.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); + VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size()); + H_input << device_from_ecef_eul, vel_ecef; + MatrixXdr HH = this->kf->H(H_input); + MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose(); + 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 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 + VectorXd nextfix_ecef = fix_ecef + vel_ecef; + VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector(); + //ned_vel_std = self.converter->ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter->ecef2ned(fix_ecef + vel_ecef) + + VectorXd accDevice = predicted_state.segment(STATE_ACCELERATION_START); + VectorXd accDeviceErr = predicted_std.segment(STATE_ACCELERATION_ERR_START); + + VectorXd angVelocityDevice = predicted_state.segment(STATE_ANGULAR_VELOCITY_START); + VectorXd angVelocityDeviceErr = predicted_std.segment(STATE_ANGULAR_VELOCITY_ERR_START); + + Vector3d nans = Vector3d(NAN, NAN, NAN); + + // write measurements to msg + init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, true); + init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, true); + init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, true); + init_measurement(fix.initVelocityNED(), ned_vel, nans, true); + init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true); + init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); + init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, true); + init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated); + init_measurement(fix.initOrientationNED(), orientation_ned, nans, true); + init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); + init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated); + init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated); + init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated); + + double old_mean = 0.0, new_mean = 0.0; + int i = 0; + for (double x : this->posenet_stds) { + if (i < POSENET_STD_HIST_HALF) { + old_mean += x; + } else { + new_mean += x; + } + i++; + } + old_mean /= POSENET_STD_HIST_HALF; + new_mean /= POSENET_STD_HIST_HALF; + // experimentally found these values, no false positives in 20k minutes of driving + bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0); + + fix.setPosenetOK(!(std_spike && this->car_speed > 5.0)); + fix.setDeviceStable(!this->device_fell); + this->device_fell = false; + + //fix.setGpsWeek(this->time.week); + //fix.setGpsTimeOfWeek(this->time.tow); + fix.setUnixTimestampMillis(this->unix_timestamp_millis); + + if (fix_ecef_std.norm() < 50.0 && this->calibrated) { + fix.setStatus(cereal::LiveLocationKalman::Status::VALID); + } else if (fix_ecef_std.norm() < 50.0) { + fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED); + } else { + fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED); + } +} + +VectorXd Localizer::get_position_geodetic() { + VectorXd fix_ecef = this->kf->get_x().segment(STATE_ECEF_POS_START); + ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) }; + Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef); + return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt); +} + +void Localizer::handle_sensors(double current_time, const capnp::List::Reader& log) { + // TODO does not yet account for double sensor readings in the log + for (int i = 0; i < log.size(); i++) { + const cereal::SensorEventData::Reader& sensor_reading = log[i]; + double sensor_time = 1e-9 * sensor_reading.getTimestamp(); + // TODO: handle messages from two IMUs at the same time + if (sensor_reading.getSource() == cereal::SensorEventData::SensorSource::LSM6DS3) { + continue; + } + + // Gyro Uncalibrated + if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { + 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 + if (sensor_reading.getSensor() == SENSOR_ACCELEROMETER && sensor_reading.getType() == SENSOR_TYPE_ACCELEROMETER) { + auto v = sensor_reading.getAcceleration().getV(); + + // check if device fell, estimate 10 for g + // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving + this->device_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]) }); + } + } + } +} + +void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) { + // ignore the message if the fix is invalid + if (log.getFlags() % 2 == 0) { + return; + } + + this->last_gps_fix = current_time; + + Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; + this->converter = std::make_unique(geodetic); + + VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector(); + VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos; + MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(3.0 * log.getVerticalAccuracy(), 2)).asDiagonal(); + MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy(), 2)).asDiagonal(); + + this->unix_timestamp_millis = log.getTimestamp(); + double gps_est_error = (this->kf->get_x().head(3) - ecef_pos).norm(); + + VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START))); + VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef); + VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg())); + VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI; + for (int i = 0; i < orientation_error.size(); i++) { + orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI); + if (orientation_error(i) < 0.0) { + orientation_error(i) += 2.0 * M_PI; + } + orientation_error(i) -= M_PI; + } + VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); + + if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) { + LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos); + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); + } else if (gps_est_error > 50.0) { + LOGE("Locationd vs ubloxLocation position difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos); + } + + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_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.getVEgo()).finished() }); + this->car_speed = std::abs(log.getVEgo()); + if (log.getVEgo() == 0.0) { // TODO probably never really 0.0 + 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++; + + 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 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_device_std[0]); + + 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) { + if (log.getRpyCalib().size() > 0) { + this->calib = floatlist2vector(log.getRpyCalib()); + this->device_from_calib = euler2rot(this->calib); + this->calib_from_device = this->device_from_calib.transpose(); + this->calibrated = log.getCalStatus() == 1; + } +} + +void Localizer::reset_kalman(double current_time) { + VectorXd init_x = this->kf->get_initial_x(); + this->reset_kalman(current_time, init_x.segment<4>(3), init_x.head(3)); +} + +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(); + MatrixXdr init_P = this->kf->get_initial_P(); + init_x.segment<4>(3) = init_orient; + 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) { + AlignedBuffer aligned_buf; + + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size)); + cereal::Event::Reader event = cmsg.getRoot(); + + this->handle_msg(event); +} + +void Localizer::handle_msg(const cereal::Event::Reader& log) { + double t = log.getLogMonoTime() * 1e-9; + if (log.isSensorEvents()) { + this->handle_sensors(t, log.getSensorEvents()); + } else if (log.isGpsLocationExternal()) { + this->handle_gps(t, log.getGpsLocationExternal()); + } else if (log.isCarState()) { + this->handle_car_state(t, log.getCarState()); + } else if (log.isCameraOdometry()) { + this->handle_cam_odo(t, log.getCameraOdometry()); + } else if (log.isLiveCalibration()) { + this->handle_live_calib(t, log.getLiveCalibration()); + } +} + +kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, + bool inputsOK, bool sensorsOK, bool gpsOK) +{ + cereal::Event::Builder evt = msg_builder.initEvent(); + evt.setLogMonoTime(logMonoTime); + cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman(); + this->build_live_location(liveLoc); + liveLoc.setInputsOK(inputsOK); + liveLoc.setSensorsOK(sensorsOK); + liveLoc.setGpsOK(gpsOK); + return msg_builder.toBytes(); +} + +int Localizer::locationd_thread() { + const std::initializer_list service_list = + { "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" }; + SubMaster sm(service_list, nullptr, { "gpsLocationExternal" }); + PubMaster pm({ "liveLocationKalman" }); + + Params params; + + while (!do_exit) { + sm.update(); + for (const char* service : service_list) { + if (sm.updated(service) && sm.valid(service)) { + const cereal::Event::Reader log = sm[service]; + this->handle_msg(log); + } + } + + if (sm.updated("cameraOdometry")) { + uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime(); + bool inputsOK = sm.allAliveAndValid(); + bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents"); + bool gpsOK = (logMonoTime / 1e9) - this->last_gps_fix < 1.0; + + MessageBuilder msg_builder; + kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK); + pm.send("liveLocationKalman", bytes.begin(), bytes.size()); + + if (sm.frame % 1200 == 0 && gpsOK) { // once a minute + VectorXd posGeo = this->get_position_geodetic(); + std::string lastGPSPosJSON = util::string_format( + "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2)); + params.put("LastGPSPosition", lastGPSPosJSON); // TODO write async + } + } + } + return 0; +} + +int main() { + Localizer localizer; + return localizer.locationd_thread(); +} diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h new file mode 100755 index 000000000..a11cdeb60 --- /dev/null +++ b/selfdrive/locationd/locationd.h @@ -0,0 +1,69 @@ +#pragma once + +#include +#include +#include + +#include + +#include "messaging.hpp" +#include "common/params.h" +#include "common/util.h" +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/transformations/coordinates.hpp" +#include "common/transformations/orientation.hpp" +#include "selfdrive/sensord/sensors/constants.hpp" + +#include "models/live_kf.h" + +#define VISION_DECIMATION 2 +#define SENSOR_DECIMATION 10 + +#define POSENET_STD_HIST_HALF 20 + +class Localizer { +public: + Localizer(); + + int locationd_thread(); + + void reset_kalman(double current_time = NAN); + void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos); + + kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, + bool inputsOK, bool sensorsOK, bool gpsOK); + void build_live_location(cereal::LiveLocationKalman::Builder& fix); + + Eigen::VectorXd get_position_geodetic(); + + void handle_msg_bytes(const char *data, const size_t size); + void handle_msg(const cereal::Event::Reader& log); + void handle_sensors(double current_time, const capnp::List::Reader& log); + void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log); + void handle_car_state(double current_time, const cereal::CarState::Reader& log); + void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log); + void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log); + +private: + std::unique_ptr kf; + + Eigen::VectorXd calib; + MatrixXdr device_from_calib; + MatrixXdr calib_from_device; + bool calibrated = false; + + int car_speed = 0; + std::deque posenet_stds; + + std::unique_ptr converter; + + 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/locationd.py b/selfdrive/locationd/locationd.py deleted file mode 100755 index 20ef4e981..000000000 --- a/selfdrive/locationd/locationd.py +++ /dev/null @@ -1,353 +0,0 @@ -#!/usr/bin/env python3 -import json -import gc -import numpy as np -import sympy as sp -import cereal.messaging as messaging -from cereal import log -from common.params import Params -import common.transformations.coordinates as coord -from common.transformations.orientation import ecef_euler_from_ned, \ - euler_from_quat, \ - ned_euler_from_ecef, \ - quat_from_euler, euler_from_rot, \ - rot_from_quat, rot_from_euler -from rednose.helpers import KalmanError -from selfdrive.locationd.models.live_kf import LiveKalman, States, ObservationKind -from selfdrive.locationd.models.constants import GENERATED_DIR -from selfdrive.swaglog import cloudlog - -#from datetime import datetime -#from laika.gps_time import GPSTime - -from sympy.utilities.lambdify import lambdify -from rednose.helpers.sympy_helpers import euler_rotate - -SensorSource = log.SensorEventData.SensorSource - - -VISION_DECIMATION = 2 -SENSOR_DECIMATION = 10 -POSENET_STD_HIST = 40 - - -def to_float(arr): - return [float(arr[0]), float(arr[1]), float(arr[2])] - - -def get_H(): - # this returns a function to eval the jacobian - # of the observation function of the local vel - roll = sp.Symbol('roll') - pitch = sp.Symbol('pitch') - yaw = sp.Symbol('yaw') - vx = sp.Symbol('vx') - vy = sp.Symbol('vy') - vz = sp.Symbol('vz') - - h = euler_rotate(roll, pitch, yaw).T*(sp.Matrix([vx, vy, vz])) - H = h.jacobian(sp.Matrix([roll, pitch, yaw, vx, vy, vz])) - H_f = lambdify([roll, pitch, yaw, vx, vy, vz], H) - return H_f - - -class Localizer(): - def __init__(self, disabled_logs=None, dog=None): - if disabled_logs is None: - disabled_logs = [] - - self.kf = LiveKalman(GENERATED_DIR) - self.reset_kalman() - self.max_age = .1 # seconds - self.disabled_logs = disabled_logs - self.calib = np.zeros(3) - self.device_from_calib = np.eye(3) - self.calib_from_device = np.eye(3) - self.calibrated = False - self.H = get_H() - - self.posenet_invalid_count = 0 - self.posenet_speed = 0 - self.car_speed = 0 - self.posenet_stds = 10*np.ones((POSENET_STD_HIST)) - - self.converter = coord.LocalCoord.from_ecef(self.kf.x[States.ECEF_POS]) - - self.unix_timestamp_millis = 0 - self.last_gps_fix = 0 - self.device_fell = False - - @staticmethod - def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov, calibrated): - predicted_std = np.sqrt(np.diagonal(predicted_cov)) - - fix_ecef = predicted_state[States.ECEF_POS] - fix_ecef_std = predicted_std[States.ECEF_POS_ERR] - vel_ecef = predicted_state[States.ECEF_VELOCITY] - vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR] - fix_pos_geo = coord.ecef2geodetic(fix_ecef) - #fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo) - orientation_ecef = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]) - orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR] - device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T - calibrated_orientation_ecef = euler_from_rot(calib_from_device.dot(device_from_ecef)) - - acc_calib = calib_from_device.dot(predicted_state[States.ACCELERATION]) - acc_calib_std = np.sqrt(np.diagonal(calib_from_device.dot( - predicted_cov[States.ACCELERATION_ERR, States.ACCELERATION_ERR]).dot( - calib_from_device.T))) - ang_vel_calib = calib_from_device.dot(predicted_state[States.ANGULAR_VELOCITY]) - ang_vel_calib_std = np.sqrt(np.diagonal(calib_from_device.dot( - predicted_cov[States.ANGULAR_VELOCITY_ERR, States.ANGULAR_VELOCITY_ERR]).dot( - calib_from_device.T))) - - vel_device = device_from_ecef.dot(vel_ecef) - device_from_ecef_eul = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]).T - idxs = list(range(States.ECEF_ORIENTATION_ERR.start, States.ECEF_ORIENTATION_ERR.stop)) + \ - list(range(States.ECEF_VELOCITY_ERR.start, States.ECEF_VELOCITY_ERR.stop)) - condensed_cov = predicted_cov[idxs][:, idxs] - HH = H(*list(np.concatenate([device_from_ecef_eul, vel_ecef]))) - vel_device_cov = HH.dot(condensed_cov).dot(HH.T) - vel_device_std = np.sqrt(np.diagonal(vel_device_cov)) - - vel_calib = calib_from_device.dot(vel_device) - vel_calib_std = np.sqrt(np.diagonal(calib_from_device.dot( - vel_device_cov).dot(calib_from_device.T))) - - orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef) - #orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned - ned_vel = converter.ecef2ned(fix_ecef + vel_ecef) - converter.ecef2ned(fix_ecef) - #ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef) - - fix = messaging.log.LiveLocationKalman.new_message() - - # write measurements to msg - measurements = [ - # measurement field, value, std, valid - (fix.positionGeodetic, fix_pos_geo, np.nan*np.zeros(3), True), - (fix.positionECEF, fix_ecef, fix_ecef_std, True), - (fix.velocityECEF, vel_ecef, vel_ecef_std, True), - (fix.velocityNED, ned_vel, np.nan*np.zeros(3), True), - (fix.velocityDevice, vel_device, vel_device_std, True), - (fix.accelerationDevice, predicted_state[States.ACCELERATION], predicted_std[States.ACCELERATION_ERR], True), - (fix.orientationECEF, orientation_ecef, orientation_ecef_std, True), - (fix.calibratedOrientationECEF, calibrated_orientation_ecef, np.nan*np.zeros(3), calibrated), - (fix.orientationNED, orientation_ned, np.nan*np.zeros(3), True), - (fix.angularVelocityDevice, predicted_state[States.ANGULAR_VELOCITY], predicted_std[States.ANGULAR_VELOCITY_ERR], True), - (fix.velocityCalibrated, vel_calib, vel_calib_std, calibrated), - (fix.angularVelocityCalibrated, ang_vel_calib, ang_vel_calib_std, calibrated), - (fix.accelerationCalibrated, acc_calib, acc_calib_std, calibrated), - ] - - for field, value, std, valid in measurements: - # TODO: can we write the lists faster? - field.value = to_float(value) - field.std = to_float(std) - field.valid = valid - - return fix - - def liveLocationMsg(self): - fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P, self.calibrated) - # experimentally found these values, no false positives in 20k minutes of driving - old_mean, new_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST//2]), np.mean(self.posenet_stds[POSENET_STD_HIST//2:]) - std_spike = new_mean/old_mean > 4 and new_mean > 7 - - fix.posenetOK = not (std_spike and self.car_speed > 5) - fix.deviceStable = not self.device_fell - self.device_fell = False - - #fix.gpsWeek = self.time.week - #fix.gpsTimeOfWeek = self.time.tow - fix.unixTimestampMillis = self.unix_timestamp_millis - - if np.linalg.norm(fix.positionECEF.std) < 50 and self.calibrated: - fix.status = 'valid' - elif np.linalg.norm(fix.positionECEF.std) < 50: - fix.status = 'uncalibrated' - else: - fix.status = 'uninitialized' - return fix - - def update_kalman(self, time, kind, meas, R=None): - try: - self.kf.predict_and_observe(time, kind, meas, R) - except KalmanError: - cloudlog.error("Error in predict and observe, kalman reset") - self.reset_kalman() - - def handle_gps(self, current_time, log): - # ignore the message if the fix is invalid - if log.flags % 2 == 0: - return - - self.last_gps_fix = current_time - - self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude]) - ecef_pos = self.converter.ned2ecef([0, 0, 0]) - ecef_vel = self.converter.ned2ecef(np.array(log.vNED)) - ecef_pos - ecef_pos_R = np.diag([(3*log.verticalAccuracy)**2]*3) - ecef_vel_R = np.diag([(log.speedAccuracy)**2]*3) - - #self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3)) - self.unix_timestamp_millis = log.timestamp - gps_est_error = np.sqrt((self.kf.x[0] - ecef_pos[0])**2 + - (self.kf.x[1] - ecef_pos[1])**2 + - (self.kf.x[2] - ecef_pos[2])**2) - - orientation_ecef = euler_from_quat(self.kf.x[States.ECEF_ORIENTATION]) - orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef) - orientation_ned_gps = np.array([0, 0, np.radians(log.bearingDeg)]) - orientation_error = np.mod(orientation_ned - orientation_ned_gps - np.pi, 2*np.pi) - np.pi - initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps)) - if np.linalg.norm(ecef_vel) > 5 and np.linalg.norm(orientation_error) > 1: - cloudlog.error("Locationd vs ubloxLocation orientation difference too large, kalman reset") - self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat) - self.update_kalman(current_time, ObservationKind.ECEF_ORIENTATION_FROM_GPS, initial_pose_ecef_quat) - elif gps_est_error > 50: - cloudlog.error("Locationd vs ubloxLocation position difference too large, kalman reset") - self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat) - - self.update_kalman(current_time, ObservationKind.ECEF_POS, ecef_pos, R=ecef_pos_R) - self.update_kalman(current_time, ObservationKind.ECEF_VEL, ecef_vel, R=ecef_vel_R) - - def handle_car_state(self, current_time, log): - self.speed_counter += 1 - - if self.speed_counter % SENSOR_DECIMATION == 0: - self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, [log.vEgo]) - self.car_speed = abs(log.vEgo) - if log.vEgo == 0: - self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0]) - - def handle_cam_odo(self, current_time, log): - self.cam_counter += 1 - - if self.cam_counter % VISION_DECIMATION == 0: - rot_device = self.device_from_calib.dot(log.rot) - rot_device_std = self.device_from_calib.dot(log.rotStd) - self.update_kalman(current_time, - ObservationKind.CAMERA_ODO_ROTATION, - np.concatenate([rot_device, 10*rot_device_std])) - trans_device = self.device_from_calib.dot(log.trans) - trans_device_std = self.device_from_calib.dot(log.transStd) - self.posenet_speed = np.linalg.norm(trans_device) - self.posenet_stds[:-1] = self.posenet_stds[1:] - self.posenet_stds[-1] = trans_device_std[0] - self.update_kalman(current_time, - ObservationKind.CAMERA_ODO_TRANSLATION, - np.concatenate([trans_device, 10*trans_device_std])) - - def handle_sensors(self, current_time, log): - # TODO does not yet account for double sensor readings in the log - for sensor_reading in log: - sensor_time = 1e-9 * sensor_reading.timestamp - # TODO: handle messages from two IMUs at the same time - if sensor_reading.source == SensorSource.lsm6ds3: - continue - - # Gyro Uncalibrated - if sensor_reading.sensor == 5 and sensor_reading.type == 16: - self.gyro_counter += 1 - if self.gyro_counter % SENSOR_DECIMATION == 0: - v = sensor_reading.gyroUncalibrated.v - self.update_kalman(sensor_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]]) - - # Accelerometer - if sensor_reading.sensor == 1 and sensor_reading.type == 1: - # check if device fell, estimate 10 for g - # 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving - self.device_fell = self.device_fell or (np.linalg.norm(np.array(sensor_reading.acceleration.v) - np.array([10, 0, 0])) > 40) - - self.acc_counter += 1 - if self.acc_counter % SENSOR_DECIMATION == 0: - v = sensor_reading.acceleration.v - self.update_kalman(sensor_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]]) - - def handle_live_calib(self, current_time, log): - if len(log.rpyCalib): - self.calib = log.rpyCalib - self.device_from_calib = rot_from_euler(self.calib) - self.calib_from_device = self.device_from_calib.T - self.calibrated = log.calStatus == 1 - - def reset_kalman(self, current_time=None, init_orient=None, init_pos=None): - self.filter_time = current_time - init_x = LiveKalman.initial_x.copy() - # too nonlinear to init on completely wrong - if init_orient is not None: - init_x[3:7] = init_orient - if init_pos is not None: - init_x[:3] = init_pos - self.kf.init_state(init_x, covs=np.diag(LiveKalman.initial_P_diag), filter_time=current_time) - - self.observation_buffer = [] - - self.gyro_counter = 0 - self.acc_counter = 0 - self.speed_counter = 0 - self.cam_counter = 0 - - -def locationd_thread(sm, pm, disabled_logs=None): - gc.disable() - - if disabled_logs is None: - disabled_logs = [] - - if sm is None: - socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration', 'carState'] - sm = messaging.SubMaster(socks, ignore_alive=['gpsLocationExternal'], ignore_avg_freq=['sensorEvents']) - if pm is None: - pm = messaging.PubMaster(['liveLocationKalman']) - - params = Params() - localizer = Localizer(disabled_logs=disabled_logs) - - while True: - sm.update() - - for sock, updated in sm.updated.items(): - if updated and sm.valid[sock]: - t = sm.logMonoTime[sock] * 1e-9 - if sock == "sensorEvents": - localizer.handle_sensors(t, sm[sock]) - elif sock == "gpsLocationExternal": - localizer.handle_gps(t, sm[sock]) - elif sock == "carState": - localizer.handle_car_state(t, sm[sock]) - elif sock == "cameraOdometry": - localizer.handle_cam_odo(t, sm[sock]) - elif sock == "liveCalibration": - localizer.handle_live_calib(t, sm[sock]) - - if sm.updated['cameraOdometry']: - t = sm.logMonoTime['cameraOdometry'] - msg = messaging.new_message('liveLocationKalman') - msg.logMonoTime = t - - msg.liveLocationKalman = localizer.liveLocationMsg() - msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid() - msg.liveLocationKalman.sensorsOK = sm.alive['sensorEvents'] and sm.valid['sensorEvents'] - - gps_age = (t / 1e9) - localizer.last_gps_fix - msg.liveLocationKalman.gpsOK = gps_age < 1.0 - pm.send('liveLocationKalman', msg) - - if sm.frame % 1200 == 0 and msg.liveLocationKalman.gpsOK: # once a minute - location = { - 'latitude': msg.liveLocationKalman.positionGeodetic.value[0], - 'longitude': msg.liveLocationKalman.positionGeodetic.value[1], - 'altitude': msg.liveLocationKalman.positionGeodetic.value[2], - } - params.put("LastGPSPosition", json.dumps(location)) - - -def main(sm=None, pm=None): - locationd_thread(sm, pm) - - -if __name__ == "__main__": - import os - os.environ["OMP_NUM_THREADS"] = "1" - main() diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc new file mode 100755 index 000000000..073a3b89a --- /dev/null +++ b/selfdrive/locationd/models/live_kf.cc @@ -0,0 +1,140 @@ +#include "live_kf.h" + +using namespace EKFS; +using namespace Eigen; + +Eigen::Map get_mapvec(Eigen::VectorXd& vec) { + return Eigen::Map(vec.data(), vec.rows(), vec.cols()); +} + +Eigen::Map get_mapmat(MatrixXdr& mat) { + return Eigen::Map(mat.data(), mat.rows(), mat.cols()); +} + +std::vector> get_vec_mapvec(std::vector& vec_vec) { + std::vector> res; + for (Eigen::VectorXd& vec : vec_vec) { + res.push_back(get_mapvec(vec)); + } + return res; +} + +std::vector> get_vec_mapmat(std::vector& mat_vec) { + std::vector> res; + for (MatrixXdr& mat : mat_vec) { + res.push_back(get_mapmat(mat)); + } + return res; +} + +LiveKalman::LiveKalman() { + this->dim_state = 23; + this->dim_state_err = 22; + + this->initial_x = live_initial_x; + this->initial_P = live_initial_P_diag.asDiagonal(); + this->Q = live_Q_diag.asDiagonal(); + for (auto& pair : live_obs_noise_diag) { + this->obs_noise[pair.first] = pair.second.asDiagonal(); + } + + // 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); +} + +void LiveKalman::init_state(VectorXd& state, VectorXd& covs_diag, double filter_time) { + MatrixXdr covs = covs_diag.asDiagonal(); + this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); +} + +void LiveKalman::init_state(VectorXd& state, MatrixXdr& covs, double filter_time) { + this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); +} + +void LiveKalman::init_state(VectorXd& state, double filter_time) { + MatrixXdr covs = this->filter->covs(); + this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); +} + +VectorXd LiveKalman::get_x() { + return this->filter->state(); +} + +MatrixXdr LiveKalman::get_P() { + return this->filter->covs(); +} + +std::vector LiveKalman::get_R(int kind, int n) { + std::vector R; + for (int i = 0; i < n; i++) { + R.push_back(this->obs_noise[kind]); + } + return R; +} + +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; + } + this->filter->normalize_state(STATE_ECEF_ORIENTATION_START, STATE_ECEF_ORIENTATION_END); + 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; +} + +MatrixXdr LiveKalman::get_initial_P() { + return this->initial_P; +} + +MatrixXdr LiveKalman::H(VectorXd in) { + assert(in.size() == 6); + Matrix res; + this->filter->get_extra_routine("H")(in.data(), res.data()); + return res; +} diff --git a/selfdrive/locationd/models/live_kf.h b/selfdrive/locationd/models/live_kf.h new file mode 100755 index 000000000..c4411bbcb --- /dev/null +++ b/selfdrive/locationd/models/live_kf.h @@ -0,0 +1,56 @@ +#pragma once + +#include +#include +#include + +#include +#include + +#include "generated/live_kf_constants.h" +#include "rednose/helpers/ekf_sym.h" + +#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth) + +using namespace EKFS; + +Eigen::Map get_mapvec(Eigen::VectorXd& vec); +Eigen::Map get_mapmat(MatrixXdr& mat); +std::vector> get_vec_mapvec(std::vector& vec_vec); +std::vector> get_vec_mapmat(std::vector& mat_vec); + +class LiveKalman { +public: + LiveKalman(); + + void init_state(Eigen::VectorXd& state, Eigen::VectorXd& covs_diag, double filter_time); + void init_state(Eigen::VectorXd& state, MatrixXdr& covs, double filter_time); + void init_state(Eigen::VectorXd& state, double filter_time); + + Eigen::VectorXd get_x(); + MatrixXdr get_P(); + std::vector get_R(int kind, int n); + + std::optional predict_and_observe(double t, int kind, std::vector meas, std::vector R = {}); + std::optional predict_and_update_odo_speed(std::vector speed, double t, int kind); + std::optional predict_and_update_odo_trans(std::vector trans, double t, int kind); + std::optional predict_and_update_odo_rot(std::vector rot, double t, int kind); + + Eigen::VectorXd get_initial_x(); + MatrixXdr get_initial_P(); + + MatrixXdr H(Eigen::VectorXd in); + +private: + std::string name = "live"; + + std::shared_ptr filter; + + int dim_state; + int dim_state_err; + + Eigen::VectorXd initial_x; + MatrixXdr initial_P; + MatrixXdr Q; // process noise + std::unordered_map obs_noise; +}; diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index 9e194b6fc..c0a32181e 100755 --- a/selfdrive/locationd/models/live_kf.py +++ b/selfdrive/locationd/models/live_kf.py @@ -1,21 +1,25 @@ #!/usr/bin/env python3 import sys +import os import numpy as np -from selfdrive.swaglog import cloudlog from selfdrive.locationd.models.constants import ObservationKind -if __name__ == '__main__': # Generating sympy - import sympy as sp - from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate - from rednose.helpers.ekf_sym import gen_code -else: - from rednose.helpers.ekf_sym_pyx import EKF_sym # pylint: disable=no-name-in-module, import-error +import sympy as sp +import inspect +from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate +from rednose.helpers.ekf_sym import gen_code EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth) +def numpy2eigenstring(arr): + assert(len(arr.shape) == 1) + arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '') + return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()" + + class States(): ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef @@ -60,14 +64,24 @@ class LiveKalman(): (0.01)**2, (0.01)**2, (0.01)**2]) # process noise - Q = np.diag([0.03**2, 0.03**2, 0.03**2, - 0.001**2, 0.001**2, 0.001**2, - 0.01**2, 0.01**2, 0.01**2, - 0.1**2, 0.1**2, 0.1**2, - (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, - (0.02 / 100)**2, - 3**2, 3**2, 3**2, - (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2]) + Q_diag = np.array([0.03**2, 0.03**2, 0.03**2, + 0.001**2, 0.001**2, 0.001**2, + 0.01**2, 0.01**2, 0.01**2, + 0.1**2, 0.1**2, 0.1**2, + (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, + (0.02 / 100)**2, + 3**2, 3**2, 3**2, + (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2]) + + obs_noise_diag = {ObservationKind.ODOMETRIC_SPEED: np.array([0.2**2]), + ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]), + ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]), + ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.IMU_FRAME: np.array([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]), + ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]), + ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]), + ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])} @staticmethod def generate_code(generated_dir): @@ -193,96 +207,37 @@ class LiveKalman(): [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], [h_imu_frame_sym, ObservationKind.IMU_FRAME, None]] - gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params) + # this returns a sympy routine for the jacobian of the observation function of the local vel + in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz + h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T*(sp.Matrix([in_vec[3], in_vec[4], in_vec[5]])) + extra_routines = [('H', h.jacobian(in_vec), [in_vec])] - def __init__(self, generated_dir): - self.dim_state = self.initial_x.shape[0] - self.dim_state_err = self.initial_P_diag.shape[0] + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines) - self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2), - ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]), - ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]), - ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]), - ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]), - ObservationKind.NO_ROT: np.diag([0.005**2, 0.005**2, 0.005**2]), - ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]), - ObservationKind.ECEF_VEL: np.diag([.5**2, .5**2, .5**2]), - ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.diag([.2**2, .2**2, .2**2, .2**2])} + # write constants to extra header file for use in cpp + live_kf_header = "#pragma once\n\n" + live_kf_header += "#include \n" + live_kf_header += "#include \n\n" + for state, slc in inspect.getmembers(States, lambda x: type(x) == slice): + assert(slc.step is None) # unsupported + live_kf_header += f'#define STATE_{state}_START {slc.start}\n' + live_kf_header += f'#define STATE_{state}_END {slc.stop}\n' + live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n' + live_kf_header += "\n" - # init filter - self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err, max_rewind_age=0.2, logger=cloudlog) + for kind, val in inspect.getmembers(ObservationKind, lambda x: type(x) == int): + live_kf_header += f'#define OBSERVATION_{kind} {val}\n' + live_kf_header += "\n" - @property - def x(self): - return self.filter.state() + live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n" + live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n" + live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n" + live_kf_header += "static const std::unordered_map> live_obs_noise_diag = {\n" + for kind, noise in LiveKalman.obs_noise_diag.items(): + live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n" + live_kf_header += "};\n\n" - @property - def t(self): - return self.filter.get_filter_time() - - @property - def P(self): - return self.filter.covs() - - def rts_smooth(self, estimates): - return self.filter.rts_smooth(estimates, norm_quats=True) - - def init_state(self, state, covs_diag=None, covs=None, filter_time=None): - if covs_diag is not None: - P = np.diag(covs_diag) - elif covs is not None: - P = covs - else: - P = self.filter.covs() - self.filter.init_state(state, P, filter_time) - - def predict_and_observe(self, t, kind, meas, R=None): - if len(meas) > 0: - meas = np.atleast_2d(meas) - if kind == ObservationKind.CAMERA_ODO_TRANSLATION: - r = self.predict_and_update_odo_trans(meas, t, kind) - elif kind == ObservationKind.CAMERA_ODO_ROTATION: - r = self.predict_and_update_odo_rot(meas, t, kind) - elif kind == ObservationKind.ODOMETRIC_SPEED: - r = self.predict_and_update_odo_speed(meas, t, kind) - else: - if R is None: - R = self.get_R(kind, len(meas)) - elif len(R.shape) == 2: - R = R[None] - r = self.filter.predict_and_update_batch(t, kind, meas, R) - - self.filter.normalize_state(States.ECEF_ORIENTATION.start, States.ECEF_ORIENTATION.stop) - return r - - def get_R(self, kind, n): - obs_noise = self.obs_noise[kind] - dim = obs_noise.shape[0] - R = np.zeros((n, dim, dim)) - for i in range(n): - R[i, :, :] = obs_noise - return R - - def predict_and_update_odo_speed(self, speed, t, kind): - z = np.array(speed) - R = np.zeros((len(speed), 1, 1)) - for i, _ in enumerate(z): - R[i, :, :] = np.diag([0.2**2]) - return self.filter.predict_and_update_batch(t, kind, z, R) - - def predict_and_update_odo_trans(self, trans, t, kind): - z = trans[:, :3] - R = np.zeros((len(trans), 3, 3)) - for i, _ in enumerate(z): - R[i, :, :] = np.diag(trans[i, 3:]**2) - return self.filter.predict_and_update_batch(t, kind, z, R) - - def predict_and_update_odo_rot(self, rot, t, kind): - z = rot[:, :3] - R = np.zeros((len(rot), 3, 3)) - for i, _ in enumerate(z): - R[i, :, :] = np.diag(rot[i, 3:]**2) - return self.filter.predict_and_update_batch(t, kind, z, R) + open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header) if __name__ == "__main__": diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index f0ae7da2b..b58f4dc2c 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -18,11 +18,11 @@ procs = [ NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC, persistent=EON, sigkill=EON), NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)), NativeProcess("ui", "selfdrive/ui", ["./ui"], persistent=True, watchdog_max_dt=(10 if TICI else None)), + NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]), PythonProcess("calibrationd", "selfdrive.locationd.calibrationd"), PythonProcess("controlsd", "selfdrive.controls.controlsd"), PythonProcess("deleter", "selfdrive.loggerd.deleter", persistent=True), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), driverview=True), - PythonProcess("locationd", "selfdrive.locationd.locationd"), PythonProcess("logmessaged", "selfdrive.logmessaged", persistent=True), PythonProcess("pandad", "selfdrive.pandad", persistent=True), PythonProcess("paramsd", "selfdrive.locationd.paramsd"), diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 1ff3ba332..ff06d66cd 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -432,6 +432,7 @@ def cpp_replay_process(cfg, lr): if not len(resp_sockets): while not pm.all_readers_updated(msg.which()): time.sleep(0) + time.sleep(0.0001) managed_processes[cfg.proc_name].stop() return log_msgs diff --git a/selfdrive/test/profiling/profiler.py b/selfdrive/test/profiling/profiler.py index f48eab0da..0e47f1839 100755 --- a/selfdrive/test/profiling/profiler.py +++ b/selfdrive/test/profiling/profiler.py @@ -74,13 +74,11 @@ def profile(proc, func, car='vw'): if __name__ == '__main__': from selfdrive.controls.controlsd import main as controlsd_thread from selfdrive.controls.radard import radard_thread - from selfdrive.locationd.locationd import locationd_thread from selfdrive.locationd.paramsd import main as paramsd_thread procs = { 'radard': radard_thread, 'controlsd': controlsd_thread, - 'locationd': locationd_thread, 'paramsd': paramsd_thread, } diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 6fc442966..fbff0e228 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -18,7 +18,7 @@ from tools.lib.logreader import LogReader PROCS = { "selfdrive.controls.controlsd": 50.0, "./loggerd": 45.0, - "selfdrive.locationd.locationd": 32.8, + "./locationd": 3.5, "selfdrive.controls.plannerd": 20.0, "./_ui": 15.0, "selfdrive.locationd.paramsd": 9.1, @@ -44,7 +44,6 @@ if TICI: "./loggerd": 60.0, "selfdrive.controls.controlsd": 26.0, "./camerad": 25.0, - "selfdrive.locationd.locationd": 21.0, "selfdrive.controls.plannerd": 12.0, "selfdrive.locationd.paramsd": 5.0, "./_dmonitoringmodeld": 10.0,