|
|
|
@ -3,12 +3,17 @@ import sys |
|
|
|
|
from typing import List |
|
|
|
|
|
|
|
|
|
import numpy as np |
|
|
|
|
import sympy as sp |
|
|
|
|
|
|
|
|
|
from rednose.helpers.ekf_sym import EKF_sym, gen_code |
|
|
|
|
from selfdrive.locationd.models.constants import ObservationKind |
|
|
|
|
from selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr |
|
|
|
|
|
|
|
|
|
if __name__ == '__main__': # Generating sympy |
|
|
|
|
import sympy as sp |
|
|
|
|
from rednose.helpers.ekf_sym import gen_code |
|
|
|
|
else: |
|
|
|
|
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx # pylint: disable=no-name-in-module,import-error |
|
|
|
|
from rednose.helpers.ekf_sym import EKF_sym # pylint: disable=no-name-in-module,import-error |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class States(): |
|
|
|
|
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters |
|
|
|
@ -115,12 +120,13 @@ 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): |
|
|
|
|
def __init__(self, generated_dir, cython=False): |
|
|
|
|
self.dim_state = self.x_initial.shape[0] |
|
|
|
|
|
|
|
|
|
# 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) |
|
|
|
|
filter_cls = EKF_sym_pyx if cython else EKF_sym |
|
|
|
|
self.filter = filter_cls(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 |
|
|
|
|