Live kf whitespace cleanup, fix covariance phone accel

selfdrive/locationd/kalman/models/live_kf.py

@@ -8,7 +8,7 @@ from selfdrive.locationd.kalman.helpers.sympy_helpers import (euler_rotate,
                                                               quat_matrix_r,
                                                               quat_rotate)
 from selfdrive.swaglog import cloudlog
-#from laika.constants import EARTH_GM
+
 EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
 
 
@@ -44,7 +44,6 @@ class LiveKalman():
                         0, 0, 0,
                         0, 0, 0])
 
-
   # state covariance
   initial_P_diag = np.array([10000**2, 10000**2, 10000**2,
                              10**2, 10**2, 10**2,
@@ -155,14 +154,9 @@ class LiveKalman():
     eskf_params = [[err_function_sym, nom_x, delta_x],
                    [inv_err_function_sym, nom_x, true_x],
                    H_mod_sym, f_err_sym, state_err_sym]
-
-
-
     #
     # Observation functions
     #
-
-
     imu_rot = euler_rotate(*imu_angles)
     h_gyro_sym = imu_rot * sp.Matrix([vroll + roll_bias,
                                       vpitch + pitch_bias,
@@ -171,18 +165,16 @@ class LiveKalman():
     pos = sp.Matrix([x, y, z])
     gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
     h_acc_sym = imu_rot * (gravity + acceleration)
-    h_phone_rot_sym = sp.Matrix([vroll,
-                                vpitch,
-                                vyaw])
-    speed = vx**2 + vy**2 + vz**2
-    h_speed_sym = sp.Matrix([sp.sqrt(speed)*odo_scale])
+    h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
+
+    speed = sp.sqrt(vx**2 + vy**2 + vz**2)
+    h_speed_sym = sp.Matrix([speed * odo_scale])
 
     h_pos_sym = sp.Matrix([x, y, z])
     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],
                [h_phone_rot_sym, ObservationKind.NO_ROT, None],
@@ -200,7 +192,7 @@ class LiveKalman():
 
     self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
                       ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
-                      ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5*2]),
+                      ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]),
                       ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
                       ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
                       ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),