locationd: Make live_kf robust to no GPS (#22774)

* add no gps observations

* use correct ecef_location, orientation vals and stds for nogps states

* remove earth radius obs

* move initial loc to the ocean

* remove unnecessary changes

* update refs

selfdrive/locationd/locationd.cc

@@ -335,7 +335,6 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry
   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,

selfdrive/locationd/models/live_kf.py

@@ -44,8 +44,8 @@ class States():
 class LiveKalman():
   name = 'live'
 
-  initial_x = np.array([-2.7e6, 4.2e6, 3.8e6,
-                        1, 0, 0, 0,
+  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,
@@ -54,8 +54,8 @@ class LiveKalman():
                         0, 0, 0])
 
   # state covariance
-  initial_P_diag = np.array([1e16, 1e16, 1e16,
-                             10**2, 10**2, 10**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,
@@ -98,7 +98,6 @@ class LiveKalman():
     omega = state[States.ANGULAR_VELOCITY, :]
     vroll, vpitch, vyaw = omega
     roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
-    odo_scale = state[States.ODO_SCALE, :][0, :]
     acceleration = state[States.ACCELERATION, :]
     imu_angles = state[States.IMU_OFFSET, :]
 
@@ -176,10 +175,11 @@ class LiveKalman():
     #
     # Observation functions
     #
-    #imu_rot = euler_rotate(*imu_angles)
-    h_gyro_sym = sp.Matrix([vroll + roll_bias,
-                                      vpitch + pitch_bias,
-                                      vyaw + yaw_bias])
+    # imu_rot = euler_rotate(*imu_angles)
+    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)
@@ -187,7 +187,7 @@ class LiveKalman():
     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 * odo_scale])
+    h_speed_sym = sp.Matrix([speed])
 
     h_pos_sym = sp.Matrix([x, y, z])
     h_vel_sym = sp.Matrix([vx, vy, vz])

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-086f33a769cfba4bf1e1bcc80fd14455a2b2da46
+0caad1211b58557625231e54a1329be3e8da668f