pure init (#1569)
parent
c78602e8cc
commit
7c8d6cbdef
|
@ -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):
|
||||
|
|
|
@ -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):
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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,
|
||||
|
|
Loading…
Reference in New Issue