openpilot v0.8.12 release

selfdrive/locationd/locationd.cc

@@ -18,6 +18,7 @@ const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
 const double VALID_TIME_SINCE_RESET = 1.0; // s
 const double VALID_POS_STD = 50.0; // m
 const double MAX_RESET_TRACKER = 5.0;
+const double SANE_GPS_UNCERTAINTY = 1500.0; // m
 
 static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
   VectorXd res(floatlist.size());
@@ -130,16 +131,16 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   Vector3d nans = Vector3d(NAN, NAN, NAN);
 
   // write measurements to msg
-  init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->last_gps_fix > 0);
-  init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->last_gps_fix > 0);
-  init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->last_gps_fix > 0);
-  init_measurement(fix.initVelocityNED(), ned_vel, nans, this->last_gps_fix > 0);
+  init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
+  init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
+  init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
+  init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
   init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
   init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
-  init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->last_gps_fix > 0);
-  init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->last_gps_fix > 0);
-  init_measurement(fix.initOrientationNED(), orientation_ned, nans, this->last_gps_fix > 0);
-  init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->last_gps_fix > 0);
+  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.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);
   init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
@@ -243,27 +244,39 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
   }
 }
 
+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_LEN>(STATE_ECEF_POS_START);
+  VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
+  MatrixXdr ecef_pos_R = this->kf->get_fake_gps_pos_cov();
+  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) {
   // ignore the message if the fix is invalid
-  if (log.getFlags() % 2 == 0) {
-    return;
-  }
 
-  // Sanity checks
-  if ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)) {
-    return;
-  }
+  bool gps_invalid_flag = (log.getFlags() % 2 == 0);
+  bool gps_unreasonable = (Vector2d(log.getAccuracy(), 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 = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
 
-  if ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)) {
-    return;
-  }
-
-  if (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK) {
+  if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane){
+    this->determine_gps_mode(current_time);
     return;
   }
 
   // Process message
   this->last_gps_fix = current_time;
+  this->gps_mode = true;
   Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
   this->converter = std::make_unique<LocalCoord>(geodetic);
 
@@ -273,7 +286,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
   MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy() * 10.0, 2)).asDiagonal();
   
   this->unix_timestamp_millis = log.getTimestamp();
-  double gps_est_error = (this->kf->get_x().head(3) - ecef_pos).norm();
+  double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
 
   VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
   VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
@@ -290,11 +303,11 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
 
   if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
     LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
-    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos);
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
     this->kf->predict_and_observe(current_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);
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
   }
 
   this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
@@ -358,7 +371,8 @@ void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibra
 
 void Localizer::reset_kalman(double current_time) {
   VectorXd init_x = this->kf->get_initial_x();
-  this->reset_kalman(current_time, init_x.segment<4>(3), init_x.head(3));
+  MatrixXdr init_P = this->kf->get_initial_P();
+  this->reset_kalman(current_time, init_x, init_P);
 }
 
 void Localizer::finite_check(double current_time) {
@@ -390,13 +404,27 @@ void Localizer::update_reset_tracker() {
   }
 }
 
-void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos) {
+void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos, VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R) {
   // too nonlinear to init on completely wrong
-  VectorXd init_x = this->kf->get_initial_x();
+  VectorXd current_x = this->kf->get_x();
+  MatrixXdr current_P = this->kf->get_P();
   MatrixXdr init_P = this->kf->get_initial_P();
-  init_x.segment<4>(3) = init_orient;
-  init_x.head(3) = init_pos;
+  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_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
+  current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel;
+  current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) = init_pos;
+
+  init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
+  init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
+  init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(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, VectorXd init_x, MatrixXdr init_P) {
   this->kf->init_state(init_x, init_P, current_time);
   this->last_reset_time = current_time;
   this->reset_tracker += 1.0;
@@ -447,14 +475,28 @@ bool Localizer::isGpsOK() {
   return this->kf->get_filter_time() - this->last_gps_fix < 1.0;
 }
 
+void Localizer::determine_gps_mode(double current_time) {
+  // 1. If the pos_std is greater than what's not acceptible and localizer is in gps-mode, reset to no-gps-mode
+  // 2. If the pos_std is greater than what's not acceptible and localizer is in no-gps-mode, fake obs
+  // 3. If the pos_std is smaller than what's not acceptible, let gps-mode be whatever it is
+  VectorXd current_pos_std = this->kf->get_P().block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(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);
+    }
+  }
+}
+
 int Localizer::locationd_thread() {
   const std::initializer_list<const char *> service_list =
       { "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" };
   PubMaster pm({ "liveLocationKalman" });
   SubMaster sm(service_list, nullptr, { "gpsLocationExternal" });
 
-  Params params;
-
   while (!do_exit) {
     sm.update();
     for (const char* service : service_list) {
@@ -479,8 +521,8 @@ int Localizer::locationd_thread() {
         std::string lastGPSPosJSON = util::string_format(
           "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
 
-        std::thread([&params] (const std::string gpsjson) {
-          params.put("LastGPSPosition", gpsjson);
+        std::thread([] (const std::string gpsjson) {
+          Params().put("LastGPSPosition", gpsjson);
         }, lastGPSPosJSON).detach();
       }
     }
@@ -489,7 +531,7 @@ int Localizer::locationd_thread() {
 }
 
 int main() {
-  set_realtime_priority(5);
+  util::set_realtime_priority(5);
 
   Localizer localizer;
   return localizer.locationd_thread();

selfdrive/locationd/locationd.h

@@ -27,11 +27,13 @@ public:
   int locationd_thread();
 
   void reset_kalman(double current_time = NAN);
-  void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos);
+  void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos, Eigen::VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R);
+  void reset_kalman(double current_time, Eigen::VectorXd init_x, MatrixXdr init_P);
   void finite_check(double current_time = NAN);
   void time_check(double current_time = NAN);
   void update_reset_tracker();
   bool isGpsOK();
+  void determine_gps_mode(double current_time);
 
   kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
     bool inputsOK, bool sensorsOK, bool gpsOK);
@@ -49,6 +51,8 @@ public:
   void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
   void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log);
 
+  void input_fake_gps_observations(double current_time);
+
 private:
   std::unique_ptr<LiveKalman> kf;
 
@@ -67,4 +71,5 @@ private:
   double last_gps_fix = 0;
   double reset_tracker = 0.0;
   bool device_fell = false;
+  bool gps_mode = false;
 };

selfdrive/locationd/models/live_kf.cc

@@ -28,11 +28,14 @@ std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(std::vector<MatrixXdr>& mat_ve
 }
 
 LiveKalman::LiveKalman() {
-  this->dim_state = 26;
-  this->dim_state_err = 25;
+  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();
@@ -87,6 +90,10 @@ std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, std:
   return r;
 }
 
+void LiveKalman::predict(double t) {
+  this->filter->predict(t);
+}
+
 Eigen::VectorXd LiveKalman::get_initial_x() {
   return this->initial_x;
 }
@@ -95,6 +102,18 @@ MatrixXdr LiveKalman::get_initial_P() {
   return this->initial_P;
 }
 
+MatrixXdr LiveKalman::get_fake_gps_pos_cov() {
+  return this->fake_gps_pos_cov;
+}
+
+MatrixXdr LiveKalman::get_fake_gps_vel_cov() {
+  return this->fake_gps_vel_cov;
+}
+
+MatrixXdr LiveKalman::get_reset_orientation_P() {
+  return this->reset_orientation_P;
+}
+
 MatrixXdr LiveKalman::H(VectorXd in) {
   assert(in.size() == 6);
   Matrix<double, 3, 6, Eigen::RowMajor> res;

selfdrive/locationd/models/live_kf.h

@@ -36,9 +36,13 @@ public:
   std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
   std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
   std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
+  void predict(double t);
 
   Eigen::VectorXd get_initial_x();
   MatrixXdr get_initial_P();
+  MatrixXdr get_fake_gps_pos_cov();
+  MatrixXdr get_fake_gps_vel_cov();
+  MatrixXdr get_reset_orientation_P();
 
   MatrixXdr H(Eigen::VectorXd in);
 
@@ -52,6 +56,9 @@ private:
 
   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<int, MatrixXdr> obs_noise;
 };

selfdrive/locationd/models/live_kf.py

@@ -26,10 +26,8 @@ class States():
   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
-  ODO_SCALE = slice(16, 17)  # odometer scale
-  ACCELERATION = slice(17, 20)  # Acceleration in device frame in m/s**2
-  IMU_OFFSET = slice(20, 23)  # imu offset angles in radians
-  ACC_BIAS = slice(23, 26)
+  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)
@@ -37,10 +35,8 @@ class States():
   ECEF_VELOCITY_ERR = slice(6, 9)
   ANGULAR_VELOCITY_ERR = slice(9, 12)
   GYRO_BIAS_ERR = slice(12, 15)
-  ODO_SCALE_ERR = slice(15, 16)
-  ACCELERATION_ERR = slice(16, 19)
-  IMU_OFFSET_ERR = slice(19, 22)
-  ACC_BIAS_ERR = slice(22, 25)
+  ACCELERATION_ERR = slice(15, 18)
+  ACC_BIAS_ERR = slice(18, 21)
 
 
 class LiveKalman():
@@ -51,38 +47,37 @@ class LiveKalman():
                         0, 0, 0,
                         0, 0, 0,
                         0, 0, 0,
-                        1,
-                        0, 0, 0,
                         0, 0, 0,
                         0, 0, 0])
 
   # state covariance
-  initial_P_diag = np.array([1e3**2, 1e3**2, 1e3**2,
-                             0.5**2, 0.5**2, 0.5**2,
+  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,
-                             0.02**2,
                              100**2, 100**2, 100**2,
-                             0.01**2, 0.01**2, 0.01**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,
-                     (0.02 / 100)**2,
                      3**2, 3**2, 3**2,
-                     (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
                      0.005**2, 0.005**2, 0.005**2])
 
-  obs_noise_diag = {ObservationKind.ODOMETRIC_SPEED: np.array([0.2**2]),
-                    ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
+  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.IMU_FRAME: np.array([0.05**2, 0.05**2, 0.05**2]),
                     ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
                     ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
                     ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
@@ -105,7 +100,6 @@ class LiveKalman():
     vroll, vpitch, vyaw = omega
     roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
     acceleration = state[States.ACCELERATION, :]
-    imu_angles = state[States.IMU_OFFSET, :]
     acc_bias = state[States.ACC_BIAS, :]
 
     dt = sp.Symbol('dt')
@@ -140,7 +134,6 @@ class LiveKalman():
     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)
@@ -183,7 +176,6 @@ class LiveKalman():
     #
     # Observation functions
     #
-    # imu_rot = euler_rotate(*imu_angles)
     h_gyro_sym = sp.Matrix([
       vroll + roll_bias,
       vpitch + pitch_bias,
@@ -194,19 +186,12 @@ class LiveKalman():
     h_acc_sym = (gravity + acceleration + acc_bias)
     h_acc_stationary_sym = acceleration
     h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
-
-    speed = sp.sqrt(vx**2 + vy**2 + vz**2 + 1e-6)
-    h_speed_sym = sp.Matrix([speed])
-
     h_pos_sym = sp.Matrix([x, y, z])
     h_vel_sym = sp.Matrix([vx, vy, vz])
     h_orientation_sym = q
-    h_imu_frame_sym = sp.Matrix(imu_angles)
-
     h_relative_motion = sp.Matrix(quat_rot.T * v)
 
-    obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
-               [h_gyro_sym, ObservationKind.PHONE_GYRO, None],
+    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],
@@ -214,12 +199,11 @@ class LiveKalman():
                [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_imu_frame_sym, ObservationKind.IMU_FRAME, 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]]))
+    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)
@@ -241,6 +225,9 @@ class LiveKalman():
 
     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<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n"
     for kind, noise in LiveKalman.obs_noise_diag.items():

selfdrive/locationd/ublox_msg.cc

@@ -37,11 +37,11 @@ inline bool UbloxMsgParser::valid_cheksum() {
     ck_b = (ck_b + ck_a) & 0xFF;
   }
   if(ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) {
-    LOGD("Checksum a mismtach: %02X, %02X", ck_a, msg_parse_buf[6]);
+    LOGD("Checksum a mismatch: %02X, %02X", ck_a, msg_parse_buf[6]);
     return false;
   }
   if(ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) {
-    LOGD("Checksum b mismtach: %02X, %02X", ck_b, msg_parse_buf[7]);
+    LOGD("Checksum b mismatch: %02X, %02X", ck_b, msg_parse_buf[7]);
     return false;
   }
   return true;

selfdrive/locationd/ubloxd.cc

@@ -17,14 +17,14 @@ int main() {
 
   PubMaster pm({"ubloxGnss", "gpsLocationExternal"});
 
-  Context * context = Context::create();
-  SubSocket * subscriber = SubSocket::create(context, "ubloxRaw");
+  std::unique_ptr<Context> context(Context::create());
+  std::unique_ptr<SubSocket> subscriber(SubSocket::create(context.get(), "ubloxRaw"));
   assert(subscriber != NULL);
   subscriber->setTimeout(100);
 
 
   while (!do_exit) {
-    Message * msg = subscriber->receive();
+    std::unique_ptr<Message> msg(subscriber->receive());
     if (!msg) {
       if (errno == EINTR) {
         do_exit = true;
@@ -32,7 +32,7 @@ int main() {
       continue;
     }
 
-    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg));
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
     cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
     auto ubloxRaw = event.getUbloxRaw();
 
@@ -58,11 +58,7 @@ int main() {
       }
       bytes_consumed += bytes_consumed_this_time;
     }
-    delete msg;
   }
 
-  delete subscriber;
-  delete context;
-
   return 0;
 }