Update rednose: use EKF_sym_pyx (#24978)

* Update rednose

* Update rednose

* cleanup
pull/24983/head
Gijs Koning 3 years ago committed by GitHub
parent d693285b02
commit fdc22554b8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
  1. 2
      rednose_repo
  2. 4
      selfdrive/locationd/models/car_kf.py
  3. 16
      selfdrive/locationd/models/gnss_kf.py

@ -1 +1 @@
Subproject commit 7663289f1e68860f53dc34337ef080dde69a2586
Subproject commit 3a6f937ad1bb234dca7b0b2052faabdd58f3e085

@ -15,7 +15,7 @@ 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 # pylint: disable=no-name-in-module, import-error
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx # pylint: disable=no-name-in-module, import-error
i = 0
@ -171,7 +171,7 @@ class CarKalman(KalmanFilter):
if P_initial is not None:
self.P_initial = P_initial
# init filter
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)
self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)
if __name__ == "__main__":

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

Loading…
Cancel
Save