mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-23 17:23:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.5.13 release

Browse Source 
old-commit-hash: dd34ccfe28
This commit is contained in:
Vehicle Researcher
2019-06-06 04:38:45 +00:00
parent 86fb001d62
commit e47a2e6e30
106 changed files with 3754 additions and 1927 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/kalman/chi2_lookup.py
+21
View File
@@ -0,0 +1,21 @@
import numpy as np
import os
def gen_chi2_ppf_lookup(max_dim=200):
from scipy.stats import chi2
table = np.zeros((max_dim, 98))
for dim in range(1,max_dim):
table[dim] = chi2.ppf(np.arange(.01, .99, .01), dim)
#outfile = open('chi2_lookup_table', 'w')
np.save('chi2_lookup_table', table)
def chi2_ppf(p, dim):
table = np.load(os.path.dirname(os.path.realpath(__file__)) + '/chi2_lookup_table.npy')
result = np.interp(p, np.arange(.01, .99, .01), table[dim])
return result
if __name__== "__main__":
gen_chi2_ppf_lookup()
selfdrive/locationd/kalman/chi2_lookup_table.npy
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:0cc301b3a918cce76bd119a219e31722c6a03fa837927eb10fd84e877966cc3e
size 156928
selfdrive/locationd/kalman/ekf_sym.py
+7 -9
View File
@@ -5,8 +5,7 @@ import numpy as np
from numpy import dot
from common.ffi_wrapper import compile_code, wrap_compiled
from common.sympy_helpers import sympy_into_c
import scipy
from scipy.stats import chi2
from chi2_lookup import chi2_ppf
EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__))
@@ -19,12 +18,11 @@ def solve(a, b):
return np.linalg.solve(a, b)
def null(H, eps=1e-12):
from scipy import linalg
u, s, vh = linalg.svd(H)
u, s, vh = np.linalg.svd(H)
padding = max(0,np.shape(H)[1]-np.shape(s)[0])
null_mask = np.concatenate(((s <= eps), np.ones((padding,),dtype=bool)),axis=0)
null_space = scipy.compress(null_mask, vh, axis=0)
return scipy.transpose(null_space)
null_space = np.compress(null_mask, vh, axis=0)
return np.transpose(null_space)
def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[]):
# optional state transition matrix, H modifier
@@ -121,7 +119,7 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No
else:
He_str = 'NULL'
# ea_dim = 1 # not really dim of ea but makes c function work
maha_thresh = chi2.ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection
maha_thresh = chi2_ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection
maha_test = kind in maha_test_kinds
extra_post += """
void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
@@ -471,7 +469,7 @@ class EKF_sym(object):
if self.msckf and kind in self.maha_test_kinds:
a = np.linalg.inv(H.dot(P).dot(H.T) + R)
maha_dist = y.T.dot(a.dot(y))
if maha_dist > chi2.ppf(0.95, y.shape[0]):
if maha_dist > chi2_ppf(0.95, y.shape[0]):
R = 10e16*R
# *** same below this line ***
@@ -513,7 +511,7 @@ class EKF_sym(object):
a = np.linalg.inv(H.dot(P).dot(H.T) + R)
maha_dist = y.T.dot(a.dot(y))
if maha_dist > chi2.ppf(maha_thresh, y.shape[0]):
if maha_dist > chi2_ppf(maha_thresh, y.shape[0]):
return False
else:
return True
selfdrive/locationd/locationd_local.py
+23 -13
View File
@@ -32,11 +32,13 @@ MAX_SR_TH = 1.9
LEARNING_RATE = 3
class Localizer(object):
def __init__(self, disabled_logs=None, dog=None):
self.kf = LocLocalKalman()
self.reset_kalman()
self.sensor_data_t = 0.0
self.max_age = .2 # seconds
self.calibration_valid = False
@@ -73,10 +75,10 @@ class Localizer(object):
self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans,
log.cameraOdometry.transStd]))
def handle_live100(self, log, current_time):
def handle_controls_state(self, log, current_time):
self.speed_counter += 1
if self.speed_counter % 5 == 0:
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.live100.vEgo]))
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.controlsState.vEgo]))
def handle_sensors(self, log, current_time):
for sensor_reading in log.sensorEvents:
@@ -92,9 +94,10 @@ class Localizer(object):
if typ in self.disabled_logs:
return
if typ == "sensorEvents":
self.sensor_data_t = current_time
self.handle_sensors(log, current_time)
elif typ == "live100":
self.handle_live100(log, current_time)
elif typ == "controlsState":
self.handle_controls_state(log, current_time)
elif typ == "cameraOdometry":
self.handle_cam_odo(log, current_time)
@@ -173,7 +176,7 @@ def locationd_thread(gctx, addr, disabled_logs):
ctx = zmq.Context()
poller = zmq.Poller()
live100_socket = messaging.sub_sock(ctx, service_list['live100'].port, poller, addr=addr, conflate=True)
controls_state_socket = messaging.sub_sock(ctx, service_list['controlsState'].port, poller, addr=addr, conflate=True)
sensor_events_socket = messaging.sub_sock(ctx, service_list['sensorEvents'].port, poller, addr=addr, conflate=True)
camera_odometry_socket = messaging.sub_sock(ctx, service_list['cameraOdometry'].port, poller, addr=addr, conflate=True)
@@ -191,17 +194,19 @@ def locationd_thread(gctx, addr, disabled_logs):
# Check if car model matches
if params is not None:
params = json.loads(params)
if params.get('carFingerprint', None) != CP.carFingerprint:
if (params.get('carFingerprint', None) != CP.carFingerprint) or (params.get('carVin', CP.carVin) != CP.carVin):
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'carVin': CP.carVin,
'angleOffsetAverage': 0.0,
'stiffnessFactor': 1.0,
'steerRatio': VM.sR,
}
params_reader.put("LiveParameters", json.dumps(params))
cloudlog.info("Parameter learner resetting to default values")
cloudlog.info("Parameter starting with: %s" % str(params))
@@ -213,29 +218,33 @@ def locationd_thread(gctx, addr, disabled_logs):
steer_ratio=params['steerRatio'],
learning_rate=LEARNING_RATE)
i = 0
i = 1
while True:
for socket, event in poller.poll(timeout=1000):
log = messaging.recv_one(socket)
localizer.handle_log(log)
if socket is live100_socket:
if socket is controls_state_socket:
if not localizer.kf.t:
continue
if i % LEARNING_RATE == 0:
# live100 is not updating the Kalman Filter, so update KF manually
# controlsState is not updating the Kalman Filter, so update KF manually
localizer.kf.predict(1e-9 * log.logMonoTime)
predicted_state = localizer.kf.x
yaw_rate = -float(predicted_state[5])
steering_angle = math.radians(log.live100.angleSteers)
params_valid = learner.update(yaw_rate, log.live100.vEgo, steering_angle)
steering_angle = math.radians(log.controlsState.angleSteers)
params_valid = learner.update(yaw_rate, log.controlsState.vEgo, steering_angle)
log_t = 1e-9 * log.logMonoTime
sensor_data_age = log_t - localizer.sensor_data_t
params = messaging.new_message()
params.init('liveParameters')
params.liveParameters.valid = bool(params_valid)
params.liveParameters.sensorValid = bool(sensor_data_age < 5.0)
params.liveParameters.angleOffset = float(math.degrees(learner.ao))
params.liveParameters.angleOffsetAverage = float(math.degrees(learner.slow_ao))
params.liveParameters.stiffnessFactor = float(learner.x)
@@ -245,8 +254,9 @@ def locationd_thread(gctx, addr, disabled_logs):
if i % 6000 == 0: # once a minute
params = learner.get_values()
params['carFingerprint'] = CP.carFingerprint
params['carVin'] = CP.carVin
params_reader.put("LiveParameters", json.dumps(params))
params_reader.put("ControlsParams", json.dumps({'angle_model_bias': log.live100.angleModelBias}))
params_reader.put("ControlsParams", json.dumps({'angle_model_bias': log.controlsState.angleModelBias}))
i += 1
elif socket is camera_odometry_socket:
@@ -264,7 +274,7 @@ def main(gctx=None, addr="127.0.0.1"):
disabled_logs = os.getenv("DISABLED_LOGS", "").split(",")
# No speed for now
disabled_logs.append('live100')
disabled_logs.append('controlsState')
if IN_CAR:
addr = "192.168.5.11"
selfdrive/locationd/ubloxd_main.cc
+3 -3
View File
@@ -72,7 +72,7 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
// New message available
if(parser.msg_class() == CLASS_NAV) {
if(parser.msg_id() == MSG_NAV_PVT) {
LOGD("MSG_NAV_PVT");
//LOGD("MSG_NAV_PVT");
auto words = parser.gen_solution();
if(words.size() > 0) {
auto bytes = words.asBytes();
@@ -82,14 +82,14 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
LOGW("Unknown nav msg id: 0x%02X", parser.msg_id());
} else if(parser.msg_class() == CLASS_RXM) {
if(parser.msg_id() == MSG_RXM_RAW) {
LOGD("MSG_RXM_RAW");
//LOGD("MSG_RXM_RAW");
auto words = parser.gen_raw();
if(words.size() > 0) {
auto bytes = words.asBytes();
send_func(parser.msg_class(), parser.msg_id(), ubloxGnss, bytes.begin(), bytes.size(), 0);
}
} else if(parser.msg_id() == MSG_RXM_SFRBX) {
LOGD("MSG_RXM_SFRBX");
//LOGD("MSG_RXM_SFRBX");
auto words = parser.gen_nav_data();
if(words.size() > 0) {
auto bytes = words.asBytes();