diff --git a/cereal/log.capnp b/cereal/log.capnp index a82122e1a5..079b62e209 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1281,7 +1281,7 @@ struct LivePose { posenetOK @5 :Bool = false; sensorsOK @6 :Bool = false; - filterState @7 :FilterState; + debugFilterState @7 :FilterState; struct XYZMeasurement { x @0 :Float32; @@ -1297,6 +1297,14 @@ struct LivePose { value @0 : List(Float64); std @1 : List(Float64); valid @2 : Bool; + + observations @3 :List(Observation); + + struct Observation { + kind @0 :Int32; + value @1 :List(Float32); + error @2 :List(Float32); + } } } diff --git a/common/params.cc b/common/params.cc index 098e1de58e..0932acd54d 100644 --- a/common/params.cc +++ b/common/params.cc @@ -157,6 +157,7 @@ std::unordered_map keys = { {"LastUpdateTime", PERSISTENT}, {"LiveParameters", PERSISTENT}, {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, + {"LocationFilterInitialState", PERSISTENT}, {"LongitudinalPersonality", PERSISTENT}, {"NetworkMetered", PERSISTENT}, {"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index 27cd4d5b40..e8eeff7e0c 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -1,17 +1,15 @@ -Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations') - -loc_libs = [messaging, common, 'pthread', 'dl'] +Import('env', 'rednose') # build ekf models rednose_gen_dir = 'models/generated' rednose_gen_deps = [ "models/constants.py", ] -live_ekf = env.RednoseCompileFilter( - target='live', - filter_gen_script='models/live_kf.py', +pose_ekf = env.RednoseCompileFilter( + target='pose', + filter_gen_script='models/pose_kf.py', output_dir=rednose_gen_dir, - extra_gen_artifacts=['live_kf_constants.h'], + extra_gen_artifacts=[], gen_script_deps=rednose_gen_deps, ) car_ekf = env.RednoseCompileFilter( @@ -21,17 +19,3 @@ car_ekf = env.RednoseCompileFilter( extra_gen_artifacts=[], gen_script_deps=rednose_gen_deps, ) - -# locationd build -locationd_sources = ["locationd.cc", "models/live_kf.cc"] - -lenv = env.Clone() -# ekf filter libraries need to be linked, even if no symbols are used -if arch != "Darwin": - lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"] - -lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath) -lenv["RPATH"].append(Dir(rednose_gen_dir).abspath) -locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations) -lenv.Depends(locationd, rednose) -lenv.Depends(locationd, live_ekf) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc deleted file mode 100644 index e1e5cd1bb2..0000000000 --- a/selfdrive/locationd/locationd.cc +++ /dev/null @@ -1,716 +0,0 @@ -#include "selfdrive/locationd/locationd.h" - -#include -#include - -#include -#include -#include - -using namespace EKFS; -using namespace Eigen; - -ExitHandler do_exit; -const double ACCEL_SANITY_CHECK = 100.0; // m/s^2 -const double ROTATION_SANITY_CHECK = 10.0; // rad/s -const double TRANS_SANITY_CHECK = 200.0; // m/s -const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg) -const double ALTITUDE_SANITY_CHECK = 10000; // m -const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad -const double SANE_GPS_UNCERTAINTY = 1500.0; // m -const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker -const double RESET_TRACKER_DECAY = 0.99995; -const double DECAY = 0.9993; // ~10 secs to resume after a bad input -const double MAX_FILTER_REWIND_TIME = 0.8; // s -const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30; - -// TODO: GPS sensor time offsets are empirically calculated -// They should be replaced with synced time from a real clock -const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s -const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s -const float GPS_POS_STD_THRESHOLD = 50.0; -const float GPS_VEL_STD_THRESHOLD = 5.0; -const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0; -const float GPS_POS_STD_RESET_THRESHOLD = 2.0; -const float GPS_VEL_STD_RESET_THRESHOLD = 0.5; -const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0; -const int GPS_ORIENTATION_ERROR_RESET_CNT = 3; - -const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0"; - -template -static Vector floatlist2vector(const ListType& floatlist) { - Vector res(floatlist.size()); - for (int i = 0; i < floatlist.size(); i++) { - res[i] = floatlist[i]; - } - return res; -} - -template -static VectorXd float64list2vector(const ListType& floatlist) { - return floatlist2vector(floatlist); -} - -template -static VectorXf float32list2vector(const ListType& floatlist) { - return floatlist2vector(floatlist); -} - -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_xyz_measurement(cereal::LivePose::XYZMeasurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) { - meas.setX(val[0]); meas.setY(val[1]); meas.setZ(val[2]); - meas.setXStd(std[0]); meas.setYStd(std[1]); meas.setZStd(std[2]); - 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(LocalizerGnssSource gnss_source) { - 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] }); - this->configure_gnss_source(gnss_source); -} - -void Localizer::build_live_pose(cereal::LivePose::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 orientation_ecef = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))); - VectorXd orientation_ecef_std = predicted_std.segment(STATE_ECEF_ORIENTATION_ERR_START); - MatrixXdr orientation_ecef_cov = predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); - MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose(); - - MatrixXdr vel_angular_cov = predicted_cov.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START); - - 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 orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef); - VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt(); - 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(); - - 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); - - // write measurements to msg - init_xyz_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true); - init_xyz_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); - init_xyz_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode); - init_xyz_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); - if (DEBUG) { - cereal::LivePose::FilterState::Builder filter_state_builder = fix.initFilterState(); - filter_state_builder.setValue(kj::arrayPtr(predicted_state.data(), predicted_state.size())); - filter_state_builder.setStd(kj::arrayPtr(predicted_std.data(), predicted_std.size())); - filter_state_builder.setValid(true); - } - - 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)); -} - -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); -} - -VectorXd Localizer::get_state() { - return this->kf->get_x(); -} - -VectorXd Localizer::get_stdev() { - return this->kf->get_P().diagonal().array().sqrt(); -} - -bool Localizer::are_inputs_ok() { - return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid; -} - -void Localizer::observation_timings_invalid_reset(){ - this->observation_timings_invalid = false; -} - -void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) { - // TODO does not yet account for double sensor readings in the log - - // Ignore empty readings (e.g. in case the magnetometer had no data ready) - if (log.getTimestamp() == 0) { - return; - } - - double sensor_time = 1e-9 * log.getTimestamp(); - - // sensor time and log time should be close - if (std::abs(current_time - sensor_time) > 0.1) { - LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); - this->observation_timings_invalid = true; - return; - } else if (!this->is_timestamp_valid(sensor_time)) { - this->observation_timings_invalid = true; - return; - } - - // TODO: handle messages from two IMUs at the same time - if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) { - return; - } - - // Gyro Uncalibrated - if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { - auto v = log.getGyroUncalibrated().getV(); - auto meas = Vector3d(-v[2], -v[1], -v[0]); - - VectorXd gyro_bias = this->kf->get_x().segment(STATE_GYRO_BIAS_START); - float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]); - float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1]; - bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold; - - if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) { - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); - this->observation_values_invalid["gyroscope"] *= DECAY; - } else { - this->observation_values_invalid["gyroscope"] += 1.0; - } - } - - // Accelerometer - if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) { - auto v = log.getAcceleration().getV(); - - // TODO: reduce false positives and re-enable this check - // 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 |= (float64list2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0; - - auto meas = Vector3d(-v[2], -v[1], -v[0]); - if (meas.norm() < ACCEL_SANITY_CHECK) { - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas }); - this->observation_values_invalid["accelerometer"] *= DECAY; - } else { - this->observation_values_invalid["accelerometer"] += 1.0; - } - } -} - -void Localizer::input_fake_gps_observations(double current_time) { - // This is done to make sure that the error estimate of the position does not blow up - // when the filter is in no-gps mode - // Steps : first predict -> observe current obs with reasonable STD - this->kf->predict(current_time); - - VectorXd current_x = this->kf->get_x(); - VectorXd ecef_pos = current_x.segment(STATE_ECEF_POS_START); - VectorXd ecef_vel = current_x.segment(STATE_ECEF_VELOCITY_START); - const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov(); - const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov(); - - 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_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) { - bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY); - bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)); - bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)); - bool gps_vel_insane = (float64list2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK); - - if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { - //this->gps_valid = false; - this->determine_gps_mode(current_time); - return; - } - - double sensor_time = current_time - sensor_time_offset; - - // Process message - //this->gps_valid = true; - this->gps_mode = true; - 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; - float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2)); - MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal(); - MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal(); - - this->unix_timestamp_millis = log.getUnixTimestampMillis(); - double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - 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, ecef_vel, ecef_pos_R, ecef_vel_R); - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); - } else if (gps_est_error > 100.0) { - LOGE("Locationd vs ubloxLocation position difference too large, kalman reset"); - this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); - } - - this->last_gps_msg = sensor_time; - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); -} - -void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) { - - if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) { - this->determine_gps_mode(current_time); - return; - } - - double sensor_time = log.getMeasTime() * 1e-9; - sensor_time -= this->gps_time_offset; - - auto ecef_pos_v = log.getPositionECEF().getValue(); - VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]); - - // indexed at 0 cause all std values are the same MAE - auto ecef_pos_std = log.getPositionECEF().getStd()[0]; - MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal(); - - auto ecef_vel_v = log.getVelocityECEF().getValue(); - VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]); - - // indexed at 0 cause all std values are the same MAE - auto ecef_vel_std = log.getVelocityECEF().getStd()[0]; - MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal(); - - double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - 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); - - LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); - ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]}; - VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector(); - double bearing_rad = atan2(ned_vel[1], ned_vel[0]); - - VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad); - 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_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) { - this->determine_gps_mode(current_time); - return; - } - - // prevent jumping gnss measurements (covered lots, standstill...) - bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD; - orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD; - orientation_reset &= !this->standstill; - if (orientation_reset) { - this->orientation_reset_count++; - } else { - this->orientation_reset_count = 0; - } - - if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) { - // always reset on first gps message and if the location is off but the accuracy is high - LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset"); - this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); - } else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) { - LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset"); - this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); - this->orientation_reset_count = 0; - } - - this->gps_mode = true; - this->last_gps_msg = sensor_time; - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); -} - -void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) { - this->car_speed = std::abs(log.getVEgo()); - this->standstill = log.getStandstill(); - if (this->standstill) { - 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_NO_ACCEL, { 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 * float64list2vector(log.getRot()); - VectorXd trans_device = this->device_from_calib * float64list2vector(log.getTrans()); - - if (!this->is_timestamp_valid(current_time)) { - this->observation_timings_invalid = true; - return; - } - - if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) { - this->observation_values_invalid["cameraOdometry"] += 1.0; - return; - } - - VectorXd rot_calib_std = float64list2vector(log.getRotStd()); - VectorXd trans_calib_std = float64list2vector(log.getTransStd()); - - if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) { - this->observation_values_invalid["cameraOdometry"] += 1.0; - return; - } - - if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) { - this->observation_values_invalid["cameraOdometry"] += 1.0; - return; - } - - this->posenet_stds.pop_front(); - this->posenet_stds.push_back(trans_calib_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; - 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, - { rot_device }, { rot_device_cov }); - this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION, - { trans_device }, { trans_device_cov }); - this->observation_values_invalid["cameraOdometry"] *= DECAY; - this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]); -} - -void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { - if (!this->is_timestamp_valid(current_time)) { - this->observation_timings_invalid = true; - return; - } - - if (log.getRpyCalib().size() > 0) { - auto live_calib = float64list2vector(log.getRpyCalib()); - if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) { - this->observation_values_invalid["liveCalibration"] += 1.0; - return; - } - - this->calib = live_calib; - this->device_from_calib = euler2rot(this->calib); - this->calib_from_device = this->device_from_calib.transpose(); - this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED; - this->observation_values_invalid["liveCalibration"] *= DECAY; - } -} - -void Localizer::reset_kalman(double current_time) { - const VectorXd &init_x = this->kf->get_initial_x(); - const MatrixXdr &init_P = this->kf->get_initial_P(); - this->reset_kalman(current_time, init_x, init_P); -} - -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::time_check(double current_time) { - if (std::isnan(this->last_reset_time)) { - this->last_reset_time = current_time; - } - if (std::isnan(this->first_valid_log_time)) { - this->first_valid_log_time = current_time; - } - double filter_time = this->kf->get_filter_time(); - bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10); - if (big_time_gap) { - LOGE("Time gap of over 10s detected, kalman reset"); - this->reset_kalman(current_time); - } -} - -void Localizer::update_reset_tracker() { - // reset tracker is tuned to trigger when over 1reset/10s over 2min period - if (this->is_gps_ok()) { - this->reset_tracker *= RESET_TRACKER_DECAY; - } else { - this->reset_tracker = 0.0; - } -} - -void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) { - // too nonlinear to init on completely wrong - VectorXd current_x = this->kf->get_x(); - MatrixXdr current_P = this->kf->get_P(); - MatrixXdr init_P = this->kf->get_initial_P(); - const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P(); - int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN); - - current_x.segment(STATE_ECEF_ORIENTATION_START) = init_orient; - current_x.segment(STATE_ECEF_VELOCITY_START) = init_vel; - current_x.segment(STATE_ECEF_POS_START) = init_pos; - - init_P.block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal(); - init_P.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal(); - init_P.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal(); - init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START, - STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal(); - - this->reset_kalman(current_time, current_x, init_P); -} - -void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) { - this->kf->init_state(init_x, init_P, current_time); - this->last_reset_time = current_time; - this->reset_tracker += 1.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; - this->time_check(t); - if (log.isAccelerometer()) { - this->handle_sensor(t, log.getAccelerometer()); - } else if (log.isGyroscope()) { - this->handle_sensor(t, log.getGyroscope()); - } else if (log.isGpsLocation()) { - this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); - } else if (log.isGpsLocationExternal()) { - this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); - //} else if (log.isGnssMeasurements()) { - // this->handle_gnss(t, log.getGnssMeasurements()); - } 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()); - } - this->finite_check(); - this->update_reset_tracker(); -} - -void Localizer::build_pose_message( - MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool msgValid) { - cereal::Event::Builder evt = msg_builder.initEvent(); - evt.setValid(msgValid); - cereal::LivePose::Builder livePose = evt.initLivePose(); - this->build_live_pose(livePose); - livePose.setSensorsOK(sensorsOK); - livePose.setInputsOK(inputsOK); -} - -bool Localizer::is_gps_ok() { - return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0; -} - -bool Localizer::critical_services_valid(const std::map &critical_services) { - for (auto &kv : critical_services){ - if (kv.second >= INPUT_INVALID_THRESHOLD){ - return false; - } - } - return true; -} - -bool Localizer::is_timestamp_valid(double current_time) { - double filter_time = this->kf->get_filter_time(); - if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) { - LOGE("Observation timestamp is older than the max rewind threshold of the filter"); - return false; - } - return true; -} - -void Localizer::determine_gps_mode(double current_time) { - // 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode - // 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs - // 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is - VectorXd current_pos_std = this->kf->get_P().block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt(); - if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){ - if (this->gps_mode){ - this->gps_mode = false; - this->reset_kalman(current_time); - } else { - this->input_fake_gps_observations(current_time); - } - } -} - -void Localizer::configure_gnss_source(const LocalizerGnssSource &source) { - this->gnss_source = source; - if (source == LocalizerGnssSource::UBLOX) { - this->gps_std_factor = 10.0; - this->gps_variance_factor = 1.0; - this->gps_vertical_variance_factor = 1.0; - this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET; - } else { - this->gps_std_factor = 2.0; - this->gps_variance_factor = 0.0; - this->gps_vertical_variance_factor = 3.0; - this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET; - } -} - -int Localizer::locationd_thread() { - Params params; - LocalizerGnssSource source; - const char* gps_location_socket; - if (params.getBool("UbloxAvailable")) { - source = LocalizerGnssSource::UBLOX; - gps_location_socket = "gpsLocationExternal"; - } else { - source = LocalizerGnssSource::QCOM; - gps_location_socket = "gpsLocation"; - } - - this->configure_gnss_source(source); - const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", - "carState", "accelerometer", "gyroscope"}; - - SubMaster sm(service_list, {}, nullptr, {gps_location_socket}); - PubMaster pm({"livePose"}); - - uint64_t cnt = 0; - bool filterInitialized = false; - const std::vector critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"}; - for (std::string service : critical_input_services) { - this->observation_values_invalid.insert({service, 0.0}); - } - - while (!do_exit) { - sm.update(); - if (filterInitialized){ - this->observation_timings_invalid_reset(); - for (const char* service : service_list) { - if (sm.updated(service) && sm.valid(service)){ - const cereal::Event::Reader log = sm[service]; - this->handle_msg(log); - } - } - } else { - filterInitialized = sm.allAliveAndValid(); - } - - const char* trigger_msg = "cameraOdometry"; - if (sm.updated(trigger_msg)) { - bool inputsOK = sm.allValid() && this->are_inputs_ok(); - bool gpsOK = this->is_gps_ok(); - bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"}); - - // Log time to first fix - if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) { - this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time); - } - - MessageBuilder pose_msg_builder; - this->build_pose_message(pose_msg_builder, inputsOK, sensorsOK, filterInitialized); - - kj::ArrayPtr pose_bytes = pose_msg_builder.toBytes(); - pm.send("livePose", pose_bytes.begin(), pose_bytes.size()); - - if (cnt % 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.putNonBlocking("LastGPSPosition", lastGPSPosJSON); - } - cnt++; - } - } - return 0; -} - -int main() { - util::set_realtime_priority(5); - - Localizer localizer; - return localizer.locationd_thread(); -} diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py new file mode 100755 index 0000000000..14a315da85 --- /dev/null +++ b/selfdrive/locationd/locationd.py @@ -0,0 +1,321 @@ +#!/usr/bin/env python3 +import os +import json +import time +import capnp +import numpy as np +from enum import Enum +from collections import defaultdict + +from cereal import log, messaging +from openpilot.common.transformations.orientation import rot_from_euler +from openpilot.common.realtime import config_realtime_process +from openpilot.common.params import Params +from openpilot.selfdrive.locationd.helpers import rotate_std +from openpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States +from openpilot.selfdrive.locationd.models.constants import ObservationKind, GENERATED_DIR + +ACCEL_SANITY_CHECK = 100.0 # m/s^2 +ROTATION_SANITY_CHECK = 10.0 # rad/s +TRANS_SANITY_CHECK = 200.0 # m/s +CALIB_RPY_SANITY_CHECK = 0.5 # rad (+- 30 deg) +MIN_STD_SANITY_CHECK = 1e-5 # m or rad +MAX_FILTER_REWIND_TIME = 0.8 # s +MAX_SENSOR_TIME_DIFF = 0.1 # s +YAWRATE_CROSS_ERR_CHECK_FACTOR = 30 +INPUT_INVALID_THRESHOLD = 0.5 +INPUT_INVALID_DECAY = 0.9993 # ~10 secs to resume after a bad input +POSENET_STD_INITIAL_VALUE = 10.0 +POSENET_STD_HIST_HALF = 20 + + +def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool): + assert len(values) == len(stds) == 3 + for i, d in enumerate(("x", "y", "z")): + setattr(measurement, d, float(values[i])) + setattr(measurement, f"{d}Std", float(stds[i])) + measurement.valid = valid + + +class HandleLogResult(Enum): + SUCCESS = 0 + TIMING_INVALID = 1 + INPUT_INVALID = 2 + SENSOR_SOURCE_INVALID = 3 + + +class LocationEstimator: + def __init__(self, debug: bool): + self.kf = PoseKalman(GENERATED_DIR, MAX_FILTER_REWIND_TIME) + + self.debug = debug + + self.posenet_stds = [POSENET_STD_INITIAL_VALUE] * (POSENET_STD_HIST_HALF * 2) + self.car_speed = 0.0 + self.camodo_yawrate_distribution = np.array([0.0, 10.0]) # mean, std + self.device_from_calib = np.eye(3) + + obs_kinds = [ObservationKind.PHONE_ACCEL, ObservationKind.PHONE_GYRO, ObservationKind.CAMERA_ODO_ROTATION, ObservationKind.CAMERA_ODO_TRANSLATION] + self.observations = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds} + self.observation_errors = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds} + + def reset(self, t: float, x_initial: np.ndarray = PoseKalman.initial_x, P_initial: np.ndarray = PoseKalman.initial_P): + self.kf.reset(t, x_initial, P_initial) + + def _validate_sensor_source(self, source: log.SensorEventData.SensorSource): + # some segments have two IMUs, ignore the second one + return source != log.SensorEventData.SensorSource.bmx055 + + def _validate_sensor_time(self, sensor_time: float, t: float): + # ignore empty readings + if sensor_time == 0: + return False + + # sensor time and log time should be close + sensor_time_invalid = abs(sensor_time - t) > MAX_SENSOR_TIME_DIFF + if sensor_time_invalid: + print("Sensor reading ignored, sensor timestamp more than 100ms off from log time") + return not sensor_time_invalid + + def _validate_timestamp(self, t: float): + kf_t = self.kf.t + invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME + if invalid: + print("Observation timestamp is older than the max rewind threshold of the filter") + return not invalid + + def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray): + all_finite = np.isfinite(new_x).all() and np.isfinite(new_P).all() + if not all_finite: + print("Non-finite values detected, kalman reset") + self.reset(t) + + def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult: + new_x, new_P = None, None + if which == "accelerometer" and msg.which() == "acceleration": + sensor_time = msg.timestamp * 1e-9 + + if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time): + return HandleLogResult.TIMING_INVALID + + if not self._validate_sensor_source(msg.source): + return HandleLogResult.SENSOR_SOURCE_INVALID + + v = msg.acceleration.v + meas = np.array([-v[2], -v[1], -v[0]]) + if np.linalg.norm(meas) >= ACCEL_SANITY_CHECK: + return HandleLogResult.INPUT_INVALID + + acc_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_ACCEL, meas) + if acc_res is not None: + _, new_x, _, new_P, _, _, (acc_err,), _, _ = acc_res + self.observation_errors[ObservationKind.PHONE_ACCEL] = np.array(acc_err) + self.observations[ObservationKind.PHONE_ACCEL] = meas + + elif which == "gyroscope" and msg.which() == "gyroUncalibrated": + sensor_time = msg.timestamp * 1e-9 + + if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time): + return HandleLogResult.TIMING_INVALID + + if not self._validate_sensor_source(msg.source): + return HandleLogResult.SENSOR_SOURCE_INVALID + + v = msg.gyroUncalibrated.v + meas = np.array([-v[2], -v[1], -v[0]]) + + gyro_bias = self.kf.x[States.GYRO_BIAS] + gyro_camodo_yawrate_err = np.abs((meas[2] - gyro_bias[2]) - self.camodo_yawrate_distribution[0]) + gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * self.camodo_yawrate_distribution[1] + gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold + + if np.linalg.norm(meas) >= ROTATION_SANITY_CHECK or not gyro_valid: + return HandleLogResult.INPUT_INVALID + + gyro_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_GYRO, meas) + if gyro_res is not None: + _, new_x, _, new_P, _, _, (gyro_err,), _, _ = gyro_res + self.observation_errors[ObservationKind.PHONE_GYRO] = np.array(gyro_err) + self.observations[ObservationKind.PHONE_GYRO] = meas + + elif which == "carState": + self.car_speed = abs(msg.vEgo) + + elif which == "liveCalibration": + if len(msg.rpyCalib) > 0: + calib = np.array(msg.rpyCalib) + if calib.min() < -CALIB_RPY_SANITY_CHECK or calib.max() > CALIB_RPY_SANITY_CHECK: + return HandleLogResult.INPUT_INVALID + + self.device_from_calib = rot_from_euler(calib) + self.calibrated = msg.calStatus == log.LiveCalibrationData.Status.calibrated + + elif which == "cameraOdometry": + if not self._validate_timestamp(t): + return HandleLogResult.TIMING_INVALID + + rot_device = np.matmul(self.device_from_calib, np.array(msg.rot)) + trans_device = np.matmul(self.device_from_calib, np.array(msg.trans)) + + if np.linalg.norm(rot_device) > ROTATION_SANITY_CHECK or np.linalg.norm(trans_device) > TRANS_SANITY_CHECK: + return HandleLogResult.INPUT_INVALID + + rot_calib_std = np.array(msg.rotStd) + trans_calib_std = np.array(msg.transStd) + + if rot_calib_std.min() <= MIN_STD_SANITY_CHECK or trans_calib_std.min() <= MIN_STD_SANITY_CHECK: + return HandleLogResult.INPUT_INVALID + + if np.linalg.norm(rot_calib_std) > 10 * ROTATION_SANITY_CHECK or np.linalg.norm(trans_calib_std) > 10 * TRANS_SANITY_CHECK: + return HandleLogResult.INPUT_INVALID + + self.posenet_stds.pop(0) + self.posenet_stds.append(trans_calib_std[0]) + + # Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise + rot_calib_std *= 10 + trans_calib_std *= 2 + + rot_device_std = rotate_std(self.device_from_calib, rot_calib_std) + trans_device_std = rotate_std(self.device_from_calib, trans_calib_std) + rot_device_noise = rot_device_std ** 2 + trans_device_noise = trans_device_std ** 2 + + cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, rot_device_noise) + cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, trans_device_noise) + self.camodo_yawrate_distribution = np.array([rot_device[2], rot_device_std[2]]) + if cam_odo_rot_res is not None: + _, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res + self.observation_errors[ObservationKind.CAMERA_ODO_ROTATION] = np.array(cam_odo_rot_err) + self.observations[ObservationKind.CAMERA_ODO_ROTATION] = rot_device + if cam_odo_trans_res is not None: + _, new_x, _, new_P, _, _, (cam_odo_trans_err,), _, _ = cam_odo_trans_res + self.observation_errors[ObservationKind.CAMERA_ODO_TRANSLATION] = np.array(cam_odo_trans_err) + self.observations[ObservationKind.CAMERA_ODO_TRANSLATION] = trans_device + + if new_x is not None and new_P is not None: + self._finite_check(t, new_x, new_P) + return HandleLogResult.SUCCESS + + def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_valid: bool): + state, cov = self.kf.x, self.kf.P + std = np.sqrt(np.diag(cov)) + + orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION] + velocity_device, velocity_device_std = state[States.DEVICE_VELOCITY], std[States.DEVICE_VELOCITY] + angular_velocity_device, angular_velocity_device_std = state[States.ANGULAR_VELOCITY], std[States.ANGULAR_VELOCITY] + acceleration_device, acceleration_device_std = state[States.ACCELERATION], std[States.ACCELERATION] + + msg = messaging.new_message("livePose") + msg.valid = filter_valid + + livePose = msg.livePose + init_xyz_measurement(livePose.orientationNED, orientation_ned, orientation_ned_std, filter_valid) + init_xyz_measurement(livePose.velocityDevice, velocity_device, velocity_device_std, filter_valid) + init_xyz_measurement(livePose.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid) + init_xyz_measurement(livePose.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid) + if self.debug: + livePose.debugFilterState.value = state.tolist() + livePose.debugFilterState.std = std.tolist() + livePose.debugFilterState.valid = filter_valid + livePose.debugFilterState.observations = [ + {'kind': k, 'value': self.observations[k].tolist(), 'error': self.observation_errors[k].tolist()} + for k in self.observations.keys() + ] + + old_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST_HALF]) + new_mean = np.mean(self.posenet_stds[POSENET_STD_HIST_HALF:]) + std_spike = (new_mean / old_mean) > 4.0 and new_mean > 7.0 + + livePose.inputsOK = inputs_valid + livePose.posenetOK = not std_spike or self.car_speed <= 5.0 + livePose.sensorsOK = sensors_valid + + return msg + + +def sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, simulation): + cur_time = time.monotonic() + for which, msgs in [("accelerometer", acc_msgs), ("gyroscope", gyro_msgs)]: + if len(msgs) > 0: + sensor_valid[which] = msgs[-1].valid + sensor_recv_time[which] = cur_time + + if not simulation: + sensor_alive[which] = (cur_time - sensor_recv_time[which]) < 0.1 + else: + sensor_alive[which] = len(msgs) > 0 + + return all(sensor_alive.values()) and all(sensor_valid.values()) + + +def main(): + config_realtime_process([0, 1, 2, 3], 5) + + DEBUG = bool(int(os.getenv("DEBUG", "0"))) + SIMULATION = bool(int(os.getenv("SIMULATION", "0"))) + + pm = messaging.PubMaster(['livePose']) + sm = messaging.SubMaster(['carState', 'liveCalibration', 'cameraOdometry'], poll='cameraOdometry') + # separate sensor sockets for efficiency + sensor_sockets = [messaging.sub_sock(which, timeout=20) for which in ['accelerometer', 'gyroscope']] + sensor_alive, sensor_valid, sensor_recv_time = defaultdict(bool), defaultdict(bool), defaultdict(float) + + params = Params() + + estimator = LocationEstimator(DEBUG) + + filter_initialized = False + critcal_services = ["accelerometer", "gyroscope", "liveCalibration", "cameraOdometry"] + observation_timing_invalid = False + observation_input_invalid = defaultdict(int) + + initial_pose = params.get("LocationFilterInitialState") + if initial_pose is not None: + initial_pose = json.loads(initial_pose) + x_initial = np.array(initial_pose["x"], dtype=np.float64) + P_initial = np.diag(np.array(initial_pose["P"], dtype=np.float64)) + estimator.reset(None, x_initial, P_initial) + + while True: + sm.update() + + acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets) + + if filter_initialized: + observation_timing_invalid = False + + msgs = [] + for msg in acc_msgs + gyro_msgs: + t, valid, which, data = msg.logMonoTime, msg.valid, msg.which(), getattr(msg, msg.which()) + msgs.append((t, valid, which, data)) + for which, updated in sm.updated.items(): + if not updated: + continue + t, valid, data = sm.logMonoTime[which], sm.valid[which], sm[which] + msgs.append((t, valid, which, data)) + + for log_mono_time, valid, which, msg in sorted(msgs, key=lambda x: x[0]): + if valid: + t = log_mono_time * 1e-9 + res = estimator.handle_log(t, which, msg) + if res == HandleLogResult.TIMING_INVALID: + observation_timing_invalid = True + elif res == HandleLogResult.INPUT_INVALID: + observation_input_invalid[which] += 1 + else: + observation_input_invalid[which] *= INPUT_INVALID_DECAY + else: + filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) + + if sm.updated["cameraOdometry"]: + critical_service_inputs_valid = all(observation_input_invalid[s] < INPUT_INVALID_THRESHOLD for s in critcal_services) + inputs_valid = sm.all_valid() and critical_service_inputs_valid and not observation_timing_invalid + sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) + + msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized) + pm.send("livePose", msg) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc deleted file mode 100644 index fc3bfb7246..0000000000 --- a/selfdrive/locationd/models/live_kf.cc +++ /dev/null @@ -1,122 +0,0 @@ -#include "selfdrive/locationd/models/live_kf.h" - -using namespace EKFS; -using namespace Eigen; - -Eigen::Map get_mapvec(const Eigen::VectorXd &vec) { - return Eigen::Map((double*)vec.data(), vec.rows(), vec.cols()); -} - -Eigen::Map get_mapmat(const MatrixXdr &mat) { - return Eigen::Map((double*)mat.data(), mat.rows(), mat.cols()); -} - -std::vector> get_vec_mapvec(const std::vector &vec_vec) { - std::vector> res; - for (const Eigen::VectorXd &vec : vec_vec) { - res.push_back(get_mapvec(vec)); - } - return res; -} - -std::vector> get_vec_mapmat(const std::vector &mat_vec) { - std::vector> res; - for (const MatrixXdr &mat : mat_vec) { - res.push_back(get_mapmat(mat)); - } - return res; -} - -LiveKalman::LiveKalman() { - this->dim_state = live_initial_x.rows(); - this->dim_state_err = live_initial_P_diag.rows(); - - this->initial_x = live_initial_x; - this->initial_P = live_initial_P_diag.asDiagonal(); - this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal(); - this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal(); - this->reset_orientation_P = live_reset_orientation_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{3}, std::vector(), 0.8); -} - -void LiveKalman::init_state(const VectorXd &state, const 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(const VectorXd &state, const MatrixXdr &covs, double filter_time) { - this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); -} - -void LiveKalman::init_state(const 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(); -} - -double LiveKalman::get_filter_time() { - return this->filter->get_filter_time(); -} - -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, const std::vector &meas, std::vector R) { - std::optional r; - 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; -} - -void LiveKalman::predict(double t) { - this->filter->predict(t); -} - -const Eigen::VectorXd &LiveKalman::get_initial_x() { - return this->initial_x; -} - -const MatrixXdr &LiveKalman::get_initial_P() { - return this->initial_P; -} - -const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() { - return this->fake_gps_pos_cov; -} - -const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() { - return this->fake_gps_vel_cov; -} - -const MatrixXdr &LiveKalman::get_reset_orientation_P() { - return this->reset_orientation_P; -} - -MatrixXdr LiveKalman::H(const VectorXd &in) { - assert(in.size() == 6); - Matrix res; - this->filter->get_extra_routine("H")((double*)in.data(), res.data()); - return res; -} diff --git a/selfdrive/locationd/models/live_kf.h b/selfdrive/locationd/models/live_kf.h deleted file mode 100644 index e4b3e326b3..0000000000 --- a/selfdrive/locationd/models/live_kf.h +++ /dev/null @@ -1,66 +0,0 @@ -#pragma once - -#include -#include -#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(const Eigen::VectorXd &vec); -Eigen::Map get_mapmat(const MatrixXdr &mat); -std::vector> get_vec_mapvec(const std::vector &vec_vec); -std::vector> get_vec_mapmat(const std::vector &mat_vec); - -class LiveKalman { -public: - LiveKalman(); - - void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time); - void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time); - void init_state(const Eigen::VectorXd &state, double filter_time); - - Eigen::VectorXd get_x(); - MatrixXdr get_P(); - double get_filter_time(); - std::vector get_R(int kind, int n); - - std::optional predict_and_observe(double t, int kind, const 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); - void predict(double t); - - const Eigen::VectorXd &get_initial_x(); - const MatrixXdr &get_initial_P(); - const MatrixXdr &get_fake_gps_pos_cov(); - const MatrixXdr &get_fake_gps_vel_cov(); - const MatrixXdr &get_reset_orientation_P(); - - MatrixXdr H(const 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 fake_gps_pos_cov; - MatrixXdr fake_gps_vel_cov; - MatrixXdr reset_orientation_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 deleted file mode 100755 index 0cc3eca61d..0000000000 --- a/selfdrive/locationd/models/live_kf.py +++ /dev/null @@ -1,242 +0,0 @@ -#!/usr/bin/env python3 - -import sys -import os -import numpy as np - -from openpilot.selfdrive.locationd.models.constants import ObservationKind - -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 - ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s - ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s - GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases - ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2 - ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2 - - # Error-state has different slices because it is an ESKF - ECEF_POS_ERR = slice(0, 3) - ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error - ECEF_VELOCITY_ERR = slice(6, 9) - ANGULAR_VELOCITY_ERR = slice(9, 12) - GYRO_BIAS_ERR = slice(12, 15) - ACCELERATION_ERR = slice(15, 18) - ACC_BIAS_ERR = slice(18, 21) - - -class LiveKalman: - name = 'live' - - initial_x = np.array([3.88e6, -3.37e6, 3.76e6, - 0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat - 0, 0, 0, - 0, 0, 0, - 0, 0, 0, - 0, 0, 0, - 0, 0, 0]) - - # state covariance - initial_P_diag = np.array([10**2, 10**2, 10**2, - 0.01**2, 0.01**2, 0.01**2, - 10**2, 10**2, 10**2, - 1**2, 1**2, 1**2, - 1**2, 1**2, 1**2, - 100**2, 100**2, 100**2, - 0.01**2, 0.01**2, 0.01**2]) - - # state covariance when resetting midway in a segment - reset_orientation_diag = np.array([1**2, 1**2, 1**2]) - - # fake observation covariance, to ensure the uncertainty estimate of the filter is under control - fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2]) - fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2]) - - # process noise - 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, - 3**2, 3**2, 3**2, - 0.005**2, 0.005**2, 0.005**2]) - - obs_noise_diag = {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.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]), - ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**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): - name = LiveKalman.name - dim_state = LiveKalman.initial_x.shape[0] - dim_state_err = LiveKalman.initial_P_diag.shape[0] - - state_sym = sp.MatrixSymbol('state', dim_state, 1) - state = sp.Matrix(state_sym) - x, y, z = state[States.ECEF_POS, :] - q = state[States.ECEF_ORIENTATION, :] - v = state[States.ECEF_VELOCITY, :] - vx, vy, vz = v - omega = state[States.ANGULAR_VELOCITY, :] - vroll, vpitch, vyaw = omega - roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] - acceleration = state[States.ACCELERATION, :] - acc_bias = state[States.ACC_BIAS, :] - - dt = sp.Symbol('dt') - - # calibration and attitude rotation matrices - quat_rot = quat_rotate(*q) - - # Got the quat predict equations from here - # A New Quaternion-Based Kalman Filter for - # Real-Time Attitude Estimation Using the Two-Step - # Geometrically-Intuitive Correction Algorithm - A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw], - [vroll, 0, vyaw, -vpitch], - [vpitch, -vyaw, 0, vroll], - [vyaw, vpitch, -vroll, 0]]) - q_dot = A * q - - # Time derivative of the state as a function of state - state_dot = sp.Matrix(np.zeros((dim_state, 1))) - state_dot[States.ECEF_POS, :] = v - state_dot[States.ECEF_ORIENTATION, :] = q_dot - state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration - - # Basic descretization, 1st order intergrator - # Can be pretty bad if dt is big - f_sym = state + dt * state_dot - - state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1) - state_err = sp.Matrix(state_err_sym) - quat_err = state_err[States.ECEF_ORIENTATION_ERR, :] - v_err = state_err[States.ECEF_VELOCITY_ERR, :] - omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :] - acceleration_err = state_err[States.ACCELERATION_ERR, :] - - # Time derivative of the state error as a function of state error and state - quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2]) - q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err) - state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1))) - state_err_dot[States.ECEF_POS_ERR, :] = v_err - state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot - state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err) - f_err_sym = state_err + dt * state_err_dot - - # Observation matrix modifier - H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) - H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start) - H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:] - H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop) - - # these error functions are defined so that say there - # is a nominal x and true x: - # true x = err_function(nominal x, delta x) - # delta x = inv_err_function(nominal x, true x) - nom_x = sp.MatrixSymbol('nom_x', dim_state, 1) - true_x = sp.MatrixSymbol('true_x', dim_state, 1) - delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1) - - err_function_sym = sp.Matrix(np.zeros((dim_state, 1))) - delta_quat = sp.Matrix(np.ones(4)) - delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :]) - err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :]) - err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat - err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :]) - - inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1))) - inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0]) - delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0] - inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:]) - inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0]) - - eskf_params = [[err_function_sym, nom_x, delta_x], - [inv_err_function_sym, nom_x, true_x], - H_mod_sym, f_err_sym, state_err_sym] - # - # Observation functions - # - h_gyro_sym = sp.Matrix([ - vroll + roll_bias, - vpitch + pitch_bias, - vyaw + yaw_bias]) - - pos = sp.Matrix([x, y, z]) - gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos) - h_acc_sym = (gravity + acceleration + acc_bias) - h_acc_stationary_sym = acceleration - h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) - h_pos_sym = sp.Matrix([x, y, z]) - h_vel_sym = sp.Matrix([vx, vy, vz]) - h_orientation_sym = q - h_relative_motion = sp.Matrix(quat_rot.T * v) - - obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None], - [h_phone_rot_sym, ObservationKind.NO_ROT, None], - [h_acc_sym, ObservationKind.PHONE_ACCEL, None], - [h_pos_sym, ObservationKind.ECEF_POS, None], - [h_vel_sym, ObservationKind.ECEF_VEL, None], - [h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None], - [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], - [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], - [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]] - - # 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])] - - gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines) - - # 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: isinstance(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" - - for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)): - live_kf_header += f'#define OBSERVATION_{kind} {val}\n' - live_kf_header += "\n" - - 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_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n" - live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n" - live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_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" - - open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header) - - -if __name__ == "__main__": - generated_dir = sys.argv[2] - LiveKalman.generate_code(generated_dir) diff --git a/selfdrive/locationd/models/pose_kf.py b/selfdrive/locationd/models/pose_kf.py new file mode 100755 index 0000000000..df63518441 --- /dev/null +++ b/selfdrive/locationd/models/pose_kf.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python3 + +import sys +import numpy as np + +from openpilot.selfdrive.locationd.models.constants import ObservationKind + +if __name__=="__main__": + import sympy as sp + from rednose.helpers.ekf_sym import gen_code + from rednose.helpers.sympy_helpers import euler_rotate, rot_to_euler +else: + from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx + +EARTH_G = 9.81 + + +class States: + NED_ORIENTATION = slice(0, 3) # roll, pitch, yaw in rad + DEVICE_VELOCITY = slice(3, 6) # ned velocity in m/s + ANGULAR_VELOCITY = slice(6, 9) # roll, pitch and yaw rates in rad/s + GYRO_BIAS = slice(9, 12) # roll, pitch and yaw gyroscope biases in rad/s + ACCELERATION = slice(12, 15) # acceleration in device frame in m/s**2 + ACCEL_BIAS = slice(15, 18) # Acceletometer bias in m/s**2 + + +class PoseKalman: + name = "pose" + + # state + initial_x = np.array([0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0]) + # state covariance + initial_P = np.diag([0.01**2, 0.01**2, 0.01**2, + 10**2, 10**2, 10**2, + 1**2, 1**2, 1**2, + 1**2, 1**2, 1**2, + 100**2, 100**2, 100**2, + 0.01**2, 0.01**2, 0.01**2]) + + # process noise + Q = np.diag([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, + 3**2, 3**2, 3**2, + 0.005**2, 0.005**2, 0.005**2]) + + obs_noise = {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_TRANSLATION: np.array([0.5**2, 0.5**2, 0.5**2]), + ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2])} + + @staticmethod + def generate_code(generated_dir): + name = PoseKalman.name + dim_state = PoseKalman.initial_x.shape[0] + dim_state_err = PoseKalman.initial_P.shape[0] + + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + roll, pitch, yaw = state[States.NED_ORIENTATION, :] + velocity = state[States.DEVICE_VELOCITY, :] + angular_velocity = state[States.ANGULAR_VELOCITY, :] + vroll, vpitch, vyaw = angular_velocity + gyro_bias = state[States.GYRO_BIAS, :] + acceleration = state[States.ACCELERATION, :] + acc_bias = state[States.ACCEL_BIAS, :] + + dt = sp.Symbol('dt') + + ned_from_device = euler_rotate(roll, pitch, yaw) + device_from_ned = ned_from_device.T + + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[States.DEVICE_VELOCITY, :] = acceleration + + f_sym = state + dt * state_dot + device_from_device_t1 = euler_rotate(dt*vroll, dt*vpitch, dt*vyaw) + ned_from_device_t1 = ned_from_device * device_from_device_t1 + f_sym[States.NED_ORIENTATION, :] = rot_to_euler(ned_from_device_t1) + + centripetal_acceleration = angular_velocity.cross(velocity) + gravity = sp.Matrix([0, 0, -EARTH_G]) + h_gyro_sym = angular_velocity + gyro_bias + h_acc_sym = device_from_ned * gravity + acceleration + centripetal_acceleration + acc_bias + h_phone_rot_sym = angular_velocity + h_relative_motion_sym = velocity + obs_eqs = [ + [h_gyro_sym, ObservationKind.PHONE_GYRO, None], + [h_acc_sym, ObservationKind.PHONE_ACCEL, None], + [h_relative_motion_sym, ObservationKind.CAMERA_ODO_TRANSLATION, None], + [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], + ] + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err) + + def __init__(self, generated_dir, max_rewind_age): + dim_state, dim_state_err = PoseKalman.initial_x.shape[0], PoseKalman.initial_P.shape[0] + self.filter = EKF_sym_pyx(generated_dir, self.name, PoseKalman.Q, PoseKalman.initial_x, PoseKalman.initial_P, + dim_state, dim_state_err, max_rewind_age=max_rewind_age) + + @property + def x(self): + return self.filter.state() + + @property + def P(self): + return self.filter.covs() + + @property + def t(self): + return self.filter.get_filter_time() + + def predict_and_observe(self, t, kind, data, obs_noise=None): + data = np.atleast_2d(data) + if obs_noise is None: + obs_noise = self.obs_noise[kind] + R = self._get_R(len(data), obs_noise) + return self.filter.predict_and_update_batch(t, kind, data, R) + + def reset(self, t, x_init, P_init): + self.filter.init_state(x_init, P_init, t) + + def _get_R(self, n, obs_noise): + dim = obs_noise.shape[0] + R = np.zeros((n, dim, dim)) + for i in range(n): + R[i, :, :] = np.diag(obs_noise) + return R + + +if __name__ == "__main__": + generated_dir = sys.argv[2] + PoseKalman.generate_code(generated_dir) diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 74ac7d2962..d0b1a82988 100644 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -1,13 +1,7 @@ -import pytest -import json -import random -import time import capnp import cereal.messaging as messaging -from cereal.services import SERVICE_LIST from openpilot.common.params import Params -from openpilot.common.transformations.coordinates import ecef2geodetic from openpilot.system.manager.process_config import managed_processes @@ -60,30 +54,3 @@ class TestLocationdProc: msg.logMonoTime = t msg.valid = True return msg - - def test_params_gps(self): - random.seed(123489234) - self.params.remove('LastGPSPosition') - - self.x = -2710700 + (random.random() * 1e5) - self.y = -4280600 + (random.random() * 1e5) - self.z = 3850300 + (random.random() * 1e5) - self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z]) - - # get fake messages at the correct frequency, listed in services.py - msgs = [] - for sec in range(65): - for name in self.LLD_MSGS: - for j in range(int(SERVICE_LIST[name].frequency)): - msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9))) - - for msg in sorted(msgs, key=lambda x: x.logMonoTime): - self.pm.send(msg.which(), msg) - if msg.which() == "cameraOdometry": - self.pm.wait_for_readers_to_update(msg.which(), 0.1, dt=0.005) - time.sleep(1) # wait for async params write - - lastGPS = json.loads(self.params.get('LastGPSPosition')) - assert lastGPS['latitude'] == pytest.approx(self.lat, abs=0.001) - assert lastGPS['longitude'] == pytest.approx(self.lon, abs=0.001) - assert lastGPS['altitude'] == pytest.approx(self.alt, abs=0.001) diff --git a/selfdrive/locationd/test/test_locationd_scenarios.py b/selfdrive/locationd/test/test_locationd_scenarios.py index bacb2a10a7..166d715c34 100644 --- a/selfdrive/locationd/test/test_locationd_scenarios.py +++ b/selfdrive/locationd/test/test_locationd_scenarios.py @@ -21,10 +21,6 @@ JUNK_IDX = 100 class Scenario(Enum): BASE = 'base' - GPS_OFF = 'gps_off' - GPS_OFF_MIDWAY = 'gps_off_midway' - GPS_ON_MIDWAY = 'gps_on_midway' - GPS_TUNNEL = 'gps_tunnel' GYRO_OFF = 'gyro_off' GYRO_SPIKE_MIDWAY = 'gyro_spike_midway' ACCEL_OFF = 'accel_off' @@ -51,24 +47,6 @@ def run_scenarios(scenario, logs): if scenario == Scenario.BASE: pass - elif scenario == Scenario.GPS_OFF: - logs = sorted([x for x in logs if x.which() not in GPS_MESSAGES], key=lambda x: x.logMonoTime) - - elif scenario == Scenario.GPS_OFF_MIDWAY: - non_gps = [x for x in logs if x.which() not in GPS_MESSAGES] - gps = [x for x in logs if x.which() in GPS_MESSAGES] - logs = sorted(non_gps + gps[: len(gps) // 2], key=lambda x: x.logMonoTime) - - elif scenario == Scenario.GPS_ON_MIDWAY: - non_gps = [x for x in logs if x.which() not in GPS_MESSAGES] - gps = [x for x in logs if x.which() in GPS_MESSAGES] - logs = sorted(non_gps + gps[len(gps) // 2:], key=lambda x: x.logMonoTime) - - elif scenario == Scenario.GPS_TUNNEL: - non_gps = [x for x in logs if x.which() not in GPS_MESSAGES] - gps = [x for x in logs if x.which() in GPS_MESSAGES] - logs = sorted(non_gps + gps[:len(gps) // 4] + gps[-len(gps) // 4:], key=lambda x: x.logMonoTime) - elif scenario == Scenario.GYRO_OFF: logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime) @@ -116,52 +94,7 @@ class TestLocationdScenarios: - roll: unchanged """ orig_data, replayed_data = run_scenarios(Scenario.BASE, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) - assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) - - def test_gps_off(self): - """ - Test: no GPS message for the entire segment - Expected Result: - - yaw_rate: unchanged - - roll: - - gpsOK: False - """ - orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) - assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) - - def test_gps_off_midway(self): - """ - Test: no GPS message for the second half of the segment - Expected Result: - - yaw_rate: unchanged - - roll: - """ - orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF_MIDWAY, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) - assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) - - def test_gps_on_midway(self): - """ - Test: no GPS message for the first half of the segment - Expected Result: - - yaw_rate: unchanged - - roll: - """ - orig_data, replayed_data = run_scenarios(Scenario.GPS_ON_MIDWAY, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) - assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(1.5)) - - def test_gps_tunnel(self): - """ - Test: no GPS message for the middle section of the segment - Expected Result: - - yaw_rate: unchanged - - roll: - """ - orig_data, replayed_data = run_scenarios(Scenario.GPS_TUNNEL, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35)) assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) def test_gyro_off(self): @@ -186,7 +119,7 @@ class TestLocationdScenarios: - inputsOK: False for some time after the spike, True for the rest """ orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35)) assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) assert np.diff(replayed_data['inputs_flag'])[499] == -1.0 assert np.diff(replayed_data['inputs_flag'])[696] == 1.0 @@ -211,5 +144,5 @@ class TestLocationdScenarios: Expected Result: Right now, the kalman filter is not robust to small spikes like it is to gyroscope spikes. """ orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35)) assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 5e4e1d3ffb..ff04274f90 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -459,13 +459,6 @@ def controlsd_config_callback(params, cfg, lr): cfg.pubs = set(cfg.pubs) - sub_keys -def locationd_config_pubsub_callback(params, cfg, lr): - ublox = params.get_bool("UbloxAvailable") - sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", }) - - cfg.pubs = set(cfg.pubs) - sub_keys - - CONFIGS = [ ProcessConfig( proc_name="controlsd", @@ -531,13 +524,13 @@ CONFIGS = [ ProcessConfig( proc_name="locationd", pubs=[ - "cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal", - "liveCalibration", "carState", "gpsLocation" + "cameraOdometry", "accelerometer", "gyroscope", "liveCalibration", "carState" ], subs=["livePose"], ignore=["logMonoTime"], - config_callback=locationd_config_pubsub_callback, + should_recv_callback=MessageBasedRcvCallback("cameraOdometry"), tolerance=NUMPY_TOLERANCE, + unlocked_pubs=["accelerometer", "gyroscope"], ), ProcessConfig( proc_name="paramsd", diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index a110082a23..4942a6920a 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -bf2bc833e238ec36c0519162edcaad743fad6dc5 \ No newline at end of file +c2022c388da6eb2e26bb23ad6009be9d5314c0be diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index b9b2a8043b..9dd781e89d 100644 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -32,7 +32,7 @@ CPU usage budget * total CPU usage of openpilot (sum(PROCS.values()) should not exceed MAX_TOTAL_CPU """ -MAX_TOTAL_CPU = 250. # total for all 8 cores +MAX_TOTAL_CPU = 260. # total for all 8 cores PROCS = { # Baseline CPU usage by process "selfdrive.controls.controlsd": 32.0, @@ -40,7 +40,6 @@ PROCS = { "./loggerd": 14.0, "./encoderd": 17.0, "./camerad": 14.5, - "./locationd": 11.0, "selfdrive.controls.plannerd": 11.0, "./ui": 18.0, "selfdrive.locationd.paramsd": 9.0, @@ -51,6 +50,7 @@ PROCS = { "system.hardware.hardwared": 3.87, "selfdrive.locationd.calibrationd": 2.0, "selfdrive.locationd.torqued": 5.0, + "selfdrive.locationd.locationd": 25.0, "selfdrive.ui.soundd": 3.5, "selfdrive.monitoring.dmonitoringd": 4.0, "./proclogd": 1.54, diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 791d26a8f4..4688951620 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -59,7 +59,7 @@ procs = [ NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC), NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)), PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad), - NativeProcess("locationd", "selfdrive/locationd", ["./locationd"], only_onroad), + PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad), NativeProcess("pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False), PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad),