diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 35fbd90e5..b723878e9 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -80,11 +80,10 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { 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 orientation_ecef_cov = predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose(); - //VectorXd calibrated_orientation_ecef = rot2euler(device_from_ecef); VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose()); VectorXd acc_calib = this->calib_from_device * predicted_state.segment(STATE_ACCELERATION_START); @@ -116,11 +115,10 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt(); VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef); - //orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned + VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt(); VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef); 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); @@ -130,6 +128,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { Vector3d nans = Vector3d(NAN, NAN, NAN); + // TODO fill in NED and Calibrated stds // write measurements to msg init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode); init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode); @@ -139,7 +138,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode); init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode); - init_measurement(fix.initOrientationNED(), orientation_ned, nans, this->gps_mode); + init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode); init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode); init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated); diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 03de2d646..9557568ba 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -45,10 +45,12 @@ class ParamsLearner: yaw_rate_std = msg.angularVelocityCalibrated.std[2] localizer_roll = msg.orientationNED.value[0] + localizer_roll_std = msg.orientationNED.std[0] roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX if roll_valid: roll = localizer_roll - roll_std = np.radians(1.0) + # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar? + roll_std = 2 * localizer_roll_std else: # This is done to bound the road roll estimate when localizer values are invalid roll = 0.0 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 6ebdb327b..2d3c904da 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -05ebb83207d2c949ee70702e4ec4568f872c6054 \ No newline at end of file +280a712ece99c231ea036c3b66d6aafa55548211 \ No newline at end of file