mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-02-22 09:33:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

bury scipy for now

Browse Source
This commit is contained in:
Harald Schafer
2020-02-12 13:13:17 -08:00
parent 9e6e1cba56
commit a8261c8017
2 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
selfdrive/locationd/kalman/helpers/lst_sq_computer.py
View File

@@ -3,8 +3,8 @@
import os
import numpy as np
import scipy.optimize as opt
import sympy as sp
#import scipy.optimize as opt
import common.transformations.orientation as orient
from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code,

6
selfdrive/locationd/kalman/models/loc_kf.py
View File

@@ -5,17 +5,18 @@ import os
import numpy as np
import sympy as sp
from laika.constants import EARTH_GM
from laika.raw_gnss import GNSSMeasurement
from selfdrive.locationd.kalman.helpers import ObservationKind
from selfdrive.locationd.kalman.helpers.ekf_sym import EKF_sym, gen_code
from selfdrive.locationd.kalman.helpers.lst_sq_computer import LstSqComputer
from selfdrive.locationd.kalman.helpers.sympy_helpers import (euler_rotate,
quat_matrix_r,
quat_rotate)
#from laika.constants import EARTH_GM
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
def parse_prr(m):
from laika.raw_gnss import GNSSMeasurement
sat_pos_vel_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
m[GNSSMeasurement.SAT_VEL]))
R_i = np.atleast_2d(m[GNSSMeasurement.PRR_STD]**2)
@@ -24,6 +25,7 @@ def parse_prr(m):
def parse_pr(m):
from laika.raw_gnss import GNSSMeasurement
pseudorange = m[GNSSMeasurement.PR]
pseudorange_stdev = m[GNSSMeasurement.PR_STD]
sat_pos_freq_i = np.concatenate((m[GNSSMeasurement.SAT_POS],