retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

pure init (#1569)

albatross
HaraldSchafer 2020-05-26 20:49:14 -07:00 committed by GitHub
parent c78602e8cc
commit 7c8d6cbdef
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 12 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
common/transformations/coordinates.py
View File

@ -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):

9
selfdrive/locationd/models/gnss_kf.py
View File

@ -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):

2
selfdrive/locationd/models/live_kf.py
View File

@ -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,

8
selfdrive/locationd/models/loc_kf.py
View File

@ -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,