use roll std from locationd (commaai#23449)

* use roll std from locationd * cleaner * add todo * new ref Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com>
twilsonco · Feb 16, 2022 · bee2b92 · bee2b92
1 parent e85931b
commit bee2b92
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 10 deletions.
diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc
@@ -79,11 +79,10 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
   VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(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_LEN>(STATE_ECEF_ORIENTATION_START)));
   VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
+  MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(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_LEN>(STATE_ACCELERATION_START);
@@ -115,11 +114,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_LEN>(STATE_ACCELERATION_START);
   VectorXd accDeviceErr = predicted_std.segment<STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START);
@@ -129,6 +127,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, true);
   init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, true);
@@ -138,7 +137,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, true);
   init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated);
-  init_measurement(fix.initOrientationNED(), orientation_ned, nans, true);
+  init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, 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);

diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
@@ -1,5 +1 @@
-<<<<<<< HEAD
-f283a33700e03e358b84db0c8fbf3b4d6740ee64
-=======
-80430e515ea7ca44f2c73f047692d357856e74c1
->>>>>>> cf466222f (Road Roll Compensation Rebased (#23251))
+280a712ece99c231ea036c3b66d6aafa55548211