diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 3f86ecf333..e49718af7a 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -82,8 +82,9 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { //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); + 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); MatrixXdr acc_calib_cov = predicted_cov.block(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START); @@ -115,6 +116,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { 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 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) @@ -137,6 +139,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { 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.initCalibratedOrientationNED(), calibrated_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); diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 530f243f3b..99919c6254 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -1d005ab2b4f6747ec0cae5d1a5e251131df453e8 \ No newline at end of file +8a2cac15ef8decb0881eb7e67e47de8a81e592aa \ No newline at end of file diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 4db9ce5e73..41777ec1b5 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -129,7 +129,7 @@ void MapWindow::timerUpdate() { if (localizer_valid) { auto pos = location.getPositionGeodetic(); - auto orientation = location.getOrientationNED(); + auto orientation = location.getCalibratedOrientationNED(); float velocity = location.getVelocityCalibrated().getValue()[0]; float bearing = RAD2DEG(orientation.getValue()[2]); diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index 16730e6c5c..cdfa4b3cea 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -17,7 +17,7 @@ QMapbox::CoordinatesCollections model_to_collection( Eigen::Vector3d ecef(positionECEF.getValue()[0], positionECEF.getValue()[1], positionECEF.getValue()[2]); Eigen::Vector3d orient(calibratedOrientationECEF.getValue()[0], calibratedOrientationECEF.getValue()[1], calibratedOrientationECEF.getValue()[2]); - Eigen::Matrix3d ecef_from_local = euler2rot(orient).transpose(); + Eigen::Matrix3d ecef_from_local = euler2rot(orient); QMapbox::Coordinates coordinates; auto x = line.getX();