mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-10-01 03:43:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

Support quectel gps (#25299)

Browse Source 
* qcom clock gets corrected randomly it seems

* Use quectel gps

* fix small laikad bugs

* Support both

* Support ublox and qcom clock model

* fix laikad test

* fix typo

* Back to original value

* More typos

Co-authored-by: Comma Device <device@comma.ai>
This commit is contained in:
HaraldSchafer
2022-07-28 19:56:59 +02:00
committed by GitHub
parent b2c1098cd8
commit 11da2804f6
5 changed files with 98 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/models/gnss_kf.py
+8 -7
View File
@@ -39,12 +39,6 @@ class GNSSKalman():
1e14, (100)**2, (0.2)**2,
(10)**2, (1)**2])
# process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
3**2, 3**2, 3**2,
(.1)**2, (0)**2, (0.005)**2,
.1**2, (.01)**2])
maha_test_kinds: List[int] = [] # ObservationKind.PSEUDORANGE_RATE, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_GLONASS]
@staticmethod
@@ -120,7 +114,14 @@ class GNSSKalman():
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds)
def __init__(self, generated_dir, cython=False):
def __init__(self, generated_dir, cython=False, erratic_clock=False):
# process noise
clock_error_drift = 100.0 if erratic_clock else 0.1
self.Q = np.diag([0.03**2, 0.03**2, 0.03**2,
3**2, 3**2, 3**2,
(clock_error_drift)**2, (0)**2, (0.005)**2,
.1**2, (.01)**2])
self.dim_state = self.x_initial.shape[0]
# init filter
selfdrive/locationd/models/loc_kf.py
+21 -17
View File
@@ -91,22 +91,6 @@ class LocKalman():
0.05**2, 0.05**2, 0.05**2,
0.01**2, 0.01**2, 0.01**2])
# process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
0.0**2, 0.0**2, 0.0**2,
0.0**2, 0.0**2, 0.0**2,
0.1**2, 0.1**2, 0.1**2,
(.1)**2, (0.0)**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.02 / 100)**2,
3**2, 3**2, 3**2,
0.001**2,
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
(.1)**2, (.01)**2,
0.005**2,
(0.02 / 100)**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
# measurements that need to pass mahalanobis distance outlier rejector
maha_test_kinds = [ObservationKind.ORB_FEATURES, ObservationKind.ORB_FEATURES_WIDE] # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
@@ -345,9 +329,29 @@ class LocKalman():
msckf_params = None
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, msckf_params, maha_test_kinds)
def __init__(self, generated_dir, N=4):
def __init__(self, generated_dir, N=4, erratic_clock=False):
name = f"{self.name}_{N}"
# process noise
clock_error_drift = 100.0 if erratic_clock else 0.1
self.Q = np.diag([0.03**2, 0.03**2, 0.03**2,
0.0**2, 0.0**2, 0.0**2,
0.0**2, 0.0**2, 0.0**2,
0.1**2, 0.1**2, 0.1**2,
(clock_error_drift)**2, (0)**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.02 / 100)**2,
3**2, 3**2, 3**2,
0.001**2,
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
(.1)**2, (.01)**2,
0.005**2,
(0.02 / 100)**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
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]),