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

Update rednose: use EKF_sym_pyx (#24978)

Browse Source 
* Update rednose

* Update rednose

* cleanup
This commit is contained in:
Gijs Koning
2022-06-28 11:32:51 +02:00
committed by GitHub
parent d693285b02
commit fdc22554b8
3 changed files with 14 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rednose_repo
+1 -1
Submodule rednose_repo updated: 7663289f1e...3a6f937ad1
selfdrive/locationd/models/car_kf.py
+2 -2
View File
@@ -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__":
selfdrive/locationd/models/gnss_kf.py
+11 -5
View File
@@ -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