pure init (#1569)

common/transformations/coordinates.py

@@ -78,6 +78,8 @@ class LocalCoord():
                                      [-np.sin(lat)*np.sin(lon), np.cos(lon), -np.cos(lat)*np.sin(lon)],
                                      [np.cos(lat), 0, -np.sin(lat)]])
     self.ecef2ned_matrix = self.ned2ecef_matrix.T
+    self.ecef_from_ned_matrix = self.ned2ecef_matrix
+    self.ned_from_ecef_matrix = self.ecef2ned_matrix
 
   @classmethod
   def from_geodetic(cls, init_geodetic):

selfdrive/locationd/models/gnss_kf.py

@@ -28,15 +28,15 @@ class GNSSKalman():
                         0, 0])
 
   # state covariance
-  P_initial = np.diag([10000**2, 10000**2, 10000**2,
+  P_initial = np.diag([1e16, 1e16, 1e16,
                        10**2, 10**2, 10**2,
-                       (2000000)**2, (100)**2, (0.5)**2,
+                       1e14, (100)**2, (0.2)**2,
                        (10)**2, (1)**2])
 
   # process noise
-  Q = np.diag([0.3**2, 0.3**2, 0.3**2,
+  Q = np.diag([0.03**2, 0.03**2, 0.03**2,
                3**2, 3**2, 3**2,
-               (.1)**2, (0)**2, (0.01)**2,
+               (.1)**2, (0)**2, (0.005)**2,
                .1**2, (.01)**2])
 
   maha_test_kinds = []  # ObservationKind.PSEUDORANGE_RATE, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_GLONASS]
@@ -119,6 +119,7 @@ class GNSSKalman():
 
     # init filter
     self.filter = EKF_sym(generated_dir, self.name, self.Q, self.x_initial, self.P_initial, self.dim_state, self.dim_state, maha_test_kinds=self.maha_test_kinds)
+    self.init_state(GNSSKalman.x_initial, covs=GNSSKalman.P_initial)
 
   @property
   def x(self):

selfdrive/locationd/models/live_kf.py

@@ -46,7 +46,7 @@ class LiveKalman():
                         0, 0, 0])
 
   # state covariance
-  initial_P_diag = np.array([1e14, 1e14, 1e14,
+  initial_P_diag = np.array([1e16, 1e16, 1e16,
                              1e6, 1e6, 1e6,
                              1e4, 1e4, 1e4,
                              1**2, 1**2, 1**2,

selfdrive/locationd/models/loc_kf.py

@@ -82,18 +82,18 @@ class LocKalman():
                         0])
 
   # state covariance
-  P_initial = np.diag([10000**2, 10000**2, 10000**2,
+  P_initial = np.diag([1e16, 1e16, 1e16,
                        10**2, 10**2, 10**2,
                        10**2, 10**2, 10**2,
                        1**2, 1**2, 1**2,
-                       (200000)**2, (100)**2,
+                       1e14, (100)**2,
                        0.05**2, 0.05**2, 0.05**2,
                        0.02**2,
-                       1**2, 1**2, 1**2,
+                       2**2, 2**2, 2**2,
                        0.01**2,
                        (0.01)**2, (0.01)**2, (0.01)**2,
                        10**2, 1**2,
-                       0.05**2])
+                       0.2**2])
 
   # process noise
   Q = np.diag([0.03**2, 0.03**2, 0.03**2,