Loc kf whitespace, fix phone accelerometer covariance

old-commit-hash: 965a9ae042

selfdrive/locationd/kalman/models/loc_kf.py

@@ -1,7 +1,5 @@
 #!/usr/bin/env python3
 
-import os
-
 import numpy as np
 import sympy as sp
 
@@ -11,7 +9,6 @@ from selfdrive.locationd.kalman.helpers.lst_sq_computer import LstSqComputer
 from selfdrive.locationd.kalman.helpers.sympy_helpers import (euler_rotate,
                                                               quat_matrix_r,
                                                               quat_rotate)
-#from laika.constants import EARTH_GM
 EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
 
 
@@ -192,7 +189,6 @@ class LocKalman():
     for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
       H_mod_sym[q_idx[0]:q_idx[1], q_err_idx[0]:q_err_idx[1]] = 0.5 * quat_matrix_r(state[q_idx[0]:q_idx[1]])[:, 1:]
 
-
     # these error functions are defined so that say there
     # is a nominal x and true x:
     # true x = err_function(nominal x, delta x)
@@ -219,9 +215,6 @@ class LocKalman():
     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
     #
@@ -238,17 +231,21 @@ class LocKalman():
     los_x, los_y, los_z = sat_los_sym
     orb_x, orb_y, orb_z = orb_epos_sym
 
-    h_pseudorange_sym = sp.Matrix([sp.sqrt(
+    h_pseudorange_sym = sp.Matrix([
+      sp.sqrt(
         (x - sat_x)**2 +
         (y - sat_y)**2 +
-                                    (z - sat_z)**2) +
-                                    cb])
+        (z - sat_z)**2
+      ) + cb
+    ])
 
-    h_pseudorange_glonass_sym = sp.Matrix([sp.sqrt(
+    h_pseudorange_glonass_sym = sp.Matrix([
+      sp.sqrt(
         (x - sat_x)**2 +
         (y - sat_y)**2 +
-                                    (z - sat_z)**2) +
-                                    cb + glonass_bias + glonass_freq_slope*glonass_freq])
+        (z - sat_z)**2
+      ) + cb + glonass_bias + glonass_freq_slope * glonass_freq
+    ])
 
     los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z]))
     los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 + los_vector[2]**2)
@@ -265,11 +262,10 @@ class LocKalman():
     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])
 
     # orb stuff
     orb_pos_sym = sp.Matrix([orb_x - x, orb_y - y, orb_z - z])
@@ -283,7 +279,6 @@ class LocKalman():
 
     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],
@@ -315,7 +310,7 @@ class LocKalman():
 
       for n in range(N):
         idx = dim_main + n * dim_augment
-        err_idx = dim_main_err + n*dim_augment_err
+        err_idx = dim_main_err + n * dim_augment_err  # FIXME: Why is this not used?
         x, y, z = state[idx:idx + 3]
         q = state[idx + 3:idx + 7]
         quat_rot = quat_rotate(*q)
@@ -324,6 +319,7 @@ class LocKalman():
         h_track_sym[n * 2:n * 2 + 2, :] = sp.Matrix([focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]),
                                                      focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])])
         h_msckf_test_sym[n * 3:n * 3 + 3, :] = sp.Matrix([track_x - x, track_y - y, track_z - z])
+
       obs_eqs.append([h_msckf_test_sym, ObservationKind.MSCKF_TEST, track_epos_sym])
       obs_eqs.append([h_track_sym, ObservationKind.ORB_FEATURES, track_epos_sym])
       obs_eqs.append([h_track_sym, ObservationKind.FEATURE_TRACK_TEST, track_epos_sym])
@@ -337,7 +333,7 @@ class LocKalman():
 
     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]),
@@ -454,7 +450,6 @@ class LocKalman():
       R[i, :, :] = R_i
     return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_freq)
 
-
   def predict_and_update_pseudorange_rate(self, meas, t, kind):
     R = np.zeros((len(meas), 1, 1))
     z = np.zeros((len(meas), 1))
@@ -507,8 +502,10 @@ class LocKalman():
     if times.any() and np.allclose(times[0, :-1], self.filter.augment_times, rtol=1e-6):
       for i, track in enumerate(tracks):
         img_positions = track.reshape((self.N + 1, 4))[:, 2:]
+
         # TODO not perfect as last pose not used
         # img_positions = unroll_shutter(img_positions, poses, self.filter.state()[7:10], self.filter.state()[10:13], ecef_pos[i])
+
         ecef_pos[i] = self.computer.compute_pos(poses, img_positions[:-1])
         z[i] = img_positions.flatten()
         R[i, :, :] = np.diag([0.005**2] * (k))
@@ -524,8 +521,8 @@ class LocKalman():
     ret = self.filter.predict_and_update_batch(t, kind, z[good_idxs], R[good_idxs], ecef_pos[good_idxs], augment=True)
     if ret is None:
       return
+
     y_full = np.zeros((z.shape[0], z.shape[1] - 3))
-    #print sum(good_idxs), len(tracks)
     if sum(good_idxs) > 0:
         y_full[good_idxs] = np.array(ret[6])
     ret = ret[:6] + (y_full, z, ecef_pos)