sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

get ready for live localizer

Browse Source
pull/1074/head
Harald Schafer 5 years ago
parent 4f05efb6bc
commit dcb14dd830
2 changed files with 44 additions and 92 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 17
      selfdrive/locationd/kalman/live_kf.py
  2. 103
      selfdrive/locationd/locationd.py

17
selfdrive/locationd/kalman/live_kf.py
Unescape View File

@ -6,10 +6,7 @@ from .kalman_helpers import ObservationKind
from .ekf_sym import EKF_sym
class LiveKalman():
def __init__(self, N=0, max_tracks=3000):
x_initial = np.array([-2.7e6, 4.2e6, 3.8e6,
initial_x = np.array([-2.7e6, 4.2e6, 3.8e6,
1, 0, 0, 0,
0, 0, 0,
0, 0, 0,
@ -18,8 +15,9 @@ class LiveKalman():
0, 0, 0,
0, 0, 0])
# state covariance
P_initial = np.diag([10000**2, 10000**2, 10000**2,
# state covariance
initial_P_diag = np.array([10000**2, 10000**2, 10000**2,
10**2, 10**2, 10**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
@ -28,6 +26,11 @@ class LiveKalman():
1**2, 1**2, 1**2,
(0.01)**2, (0.01)**2, (0.01)**2])
class LiveKalman():
def __init__(self, N=0, max_tracks=3000):
# process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
0.0**2, 0.0**2, 0.0**2,
@ -52,7 +55,7 @@ class LiveKalman():
gen_model(name, self.dim_state, self.dim_state_err)
# init filter
self.filter = EKF_sym(name, Q, x_initial, P_initial, self.dim_state, self.dim_state_err)
self.filter = EKF_sym(name, Q, initial_x, np.diag(initial_P_diag), self.dim_state, self.dim_state_err)
@property
def x(self):

103
selfdrive/locationd/locationd.py
Unescape View File

@ -5,26 +5,18 @@ import numpy as np
from bisect import bisect_right
import cereal.messaging as messaging
from selfdrive.swaglog import cloudlog
from cereal.services import service_list
from common.transformations.orientation import rotations_from_quats, ecef_euler_from_ned, euler2quat, ned_euler_from_ecef, quat2euler
import common.transformations.coordinates as coord
import laika.raw_gnss as gnss
from laika.astro_dog import AstroDog
from selfdrive.locationd.kalman.loc_kf import LocKalman
from selfdrive.locationd.kalman.live_kf import LiveKalman, States, inital_x, initial_P_diag
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind
os.environ["OMP_NUM_THREADS"] = "1"
class Localizer():
def __init__(self, disabled_logs=[], dog=None):
self.kf = LocKalman(0)
self.kf = LiveKalman()
self.reset_kalman()
if dog:
self.dog = dog
else:
self.dog = AstroDog(auto_update=True)
self.max_age = .2 # seconds
self.disabled_logs = disabled_logs
self.week = None
@ -32,32 +24,31 @@ class Localizer():
def liveLocationMsg(self, time):
fix = messaging.log.LiveLocationData.new_message()
predicted_state = self.kf.x
fix_ecef = predicted_state[0:3]
fix_ecef = predicted_state[States.ECEF_POS]
fix_pos_geo = coord.ecef2geodetic(fix_ecef)
fix.lat = float(fix_pos_geo[0])
fix.lon = float(fix_pos_geo[1])
fix.alt = float(fix_pos_geo[2])
fix.speed = float(np.linalg.norm(predicted_state[7:10]))
fix.speed = float(np.linalg.norm(predicted_state[States.ECEF_VELOCITY]))
orientation_ned_euler = ned_euler_from_ecef(fix_ecef, quat2euler(predicted_state[3:7]))
orientation_ned_euler = ned_euler_from_ecef(fix_ecef, quat2euler(predicted_state[States.ECEF_ORIENTATION]))
fix.roll = float(orientation_ned_euler[0]*180/np.pi)
fix.pitch = float(orientation_ned_euler[1]*180/np.pi)
fix.heading = float(orientation_ned_euler[2]*180/np.pi)
fix.gyro = [float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12])]
fix.accel = [float(predicted_state[19]), float(predicted_state[20]), float(predicted_state[21])]
local_vel = rotations_from_quats(predicted_state[3:7]).T.dot(predicted_state[7:10])
local_vel = rotations_from_quats(predicted_state[States.ECEF_ORIENTATION]).T.dot(predicted_state[States.ECEF_VELOCITY])
fix.pitchCalibration = float((np.arctan2(local_vel[2], local_vel[0]))*180/np.pi)
fix.yawCalibration = float((np.arctan2(local_vel[1], local_vel[0]))*180/np.pi)
fix.imuFrame = [(180/np.pi)*float(predicted_state[23]), (180/np.pi)*float(predicted_state[24]), (180/np.pi)*float(predicted_state[25])]
#fix.imuFrame = [(180/np.pi)*float(predicted_state[23]), (180/np.pi)*float(predicted_state[24]), (180/np.pi)*float(predicted_state[25])]
return fix
def update_kalman(self, time, kind, meas):
idx = bisect_right([x[0] for x in self.observation_buffer], time)
self.observation_buffer.insert(idx, (time, kind, meas))
#print len(self.observation_buffer), idx, self.kf.filter.filter_time, time
while self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
if self.filter_ready:
self.kf.predict_and_observe(*self.observation_buffer.pop(0))
@ -67,38 +58,31 @@ class Localizer():
def handle_gps(self, log, current_time):
self.converter = coord.LocalCoord.from_geodetic([log.gpsLocationExternal.latitude, log.gpsLocationExternal.longitude, log.gpsLocationExternal.altitude])
fix_ecef = self.converter.ned2ecef([0,0,0])
# initing with bad bearing allowed, maybe bad?
if not self.filter_ready and len(list(self.dog.orbits.keys())) >6: # and log.gpsLocationExternal.speed > 5:
# TODO initing with bad bearing not allowed, maybe not bad?
if not self.filter_ready and log.gpsLocationExternal.speed > 5:
self.filter_ready = True
initial_ecef = fix_ecef
initial_state = np.zeros(29)
gps_bearing = log.gpsLocationExternal.bearing*(np.pi/180)
initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing])
initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
gps_speed = log.gpsLocationExternal.speed
quat_uncertainty = 0.2**2
initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
initial_state[:3] = initial_ecef
initial_state[3:7] = initial_pose_ecef_quat
initial_state[7:10] = rotations_from_quats(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0]))
initial_state[18] = 1
initial_state[22] = 1
covs_diag = np.array([10**2,10**2,10**2,
quat_uncertainty, quat_uncertainty, quat_uncertainty,
2**2, 2**2, 2**2,
1, 1, 1,
20000000**2, 100**2,
0.01**2, 0.01**2, 0.01**2,
0.02**2,
2**2, 2**2, 2**2,
.01**2,
0.01**2, 0.01**2, 0.01**2,
10**2, 1**2,
0.2**2])
self.kf.init_state(initial_state, covs=np.diag(covs_diag), filter_time=current_time)
initial_state = inital_x
initial_covs_diag = initial_P_diag
initial_state[States.ECEF_POS] = initial_ecef
initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat
initial_state[States.ECEF_VELOCITY] = rotations_from_quats(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0]))
initial_covs_diag[States.ECEF_POS_ERR] = 10**2
initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty
initial_covs_diag[States.ECEF_VELOCITY_ERR] = 1**2
self.kf.init_state(initial_state, covs=np.diag(initial_covs_diag), filter_time=current_time)
print("Filter initialized")
elif self.filter_ready:
#self.update_kalman(current_time, ObservationKind.ECEF_POS, fix_ecef)
self.update_kalman(current_time, ObservationKind.ECEF_POS, fix_ecef)
gps_est_error = np.sqrt((self.kf.x[0] - fix_ecef[0])**2 +
(self.kf.x[1] - fix_ecef[1])**2 +
(self.kf.x[2] - fix_ecef[2])**2)
@ -113,34 +97,11 @@ class Localizer():
if log.carState.vEgo == 0:
self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0])
def handle_ublox_gnss(self, log, current_time):
if hasattr(log.ubloxGnss, 'measurementReport'):
self.raw_gnss_counter += 1
if True or self.raw_gnss_counter % 3==0:
processed_raw = gnss.process_measurements(gnss.read_raw_ublox(log.ubloxGnss.measurementReport), dog=self.dog)
corrected_raw = gnss.correct_measurements(processed_raw, self.kf.x[:3], dog=self.dog)
corrected_raw = np.array([c.as_array() for c in corrected_raw]).reshape((-1,14))
self.update_kalman(current_time, ObservationKind.PSEUDORANGE_GPS, corrected_raw)
self.update_kalman(current_time, ObservationKind.PSEUDORANGE_RATE_GPS, corrected_raw)
#elif hasattr(log.ubloxGnss, 'ephemeris'):
# self.dog.add_ublox_ephems([log])
# if len(self.dog.orbits.keys()) < 6:
# print 'Added ublox ephem now has ', len(self.dog.orbits.keys())
def handle_qcom_gnss(self, log, current_time):
if hasattr(log.qcomGnss, 'drSvPoly') and self.week is not None:
self.dog.add_qcom_ephems([log], self.week)
if len(list(self.dog.orbits.keys())) < 6:
print('Added qcom ephem now has ', len(list(self.dog.orbits.keys())))
if hasattr(log.qcomGnss, 'drMeasurementReport') and log.qcomGnss.drMeasurementReport.source == "gps":
self.week = log.qcomGnss.drMeasurementReport.gpsWeek
def handle_cam_odo(self, log, current_time):
self.update_kalman(current_time, ObservationKind.CAMERA_ODO_ROTATION, np.concatenate([log.cameraOdometry.rot,
log.cameraOdometry.rotStd]))
self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans,
log.cameraOdometry.transStd]))
pass
def handle_sensors(self, log, current_time):
for sensor_reading in log.sensorEvents:
@ -148,7 +109,7 @@ class Localizer():
if sensor_reading.type == 4:
self.gyro_counter += 1
if True or self.gyro_counter % 5==0:
if max(abs(self.kf.x[23:26])) > 0.07:
if max(abs(self.kf.x[States.IMU_OFFSET])) > 0.07:
print('imu frame angles exceeded, correcting')
self.update_kalman(current_time, ObservationKind.IMU_FRAME, [0, 0, 0])
self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-sensor_reading.gyro.v[2], -sensor_reading.gyro.v[1], -sensor_reading.gyro.v[0]])
@ -168,10 +129,6 @@ class Localizer():
self.handle_gps(log, current_time)
elif typ == "carState":
self.handle_car_state(log, current_time)
elif typ == "ubloxGnss":
self.handle_ublox_gnss(log, current_time)
elif typ == "qcomGnss":
self.handle_qcom_gnss(log, current_time)
elif typ == "cameraOdometry":
self.handle_cam_odo(log, current_time)
@ -182,18 +139,16 @@ class Localizer():
self.converter = None
self.gyro_counter = 0
self.acc_counter = 0
self.raw_gnss_counter = 0
self.speed_counter = 0
def locationd_thread(gctx, addr, disabled_logs):
def locationd_thread(gctx, addr, disabled_logs=[]):
poller = zmq.Poller()
#carstate = messaging.sub_sock('carState', poller, addr=addr, conflate=True)
gpsLocationExternal = messaging.sub_sock('gpsLocationExternal', poller, addr=addr, conflate=True)
ubloxGnss = messaging.sub_sock('ubloxGnss', poller, addr=addr, conflate=True)
qcomGnss = messaging.sub_sock('qcomGnss', poller, addr=addr, conflate=True)
sensorEvents = messaging.sub_sock('sensorEvents', poller, addr=addr, conflate=True)
cameraOdometry = messaging.sub_sock('cameraOdometry', poller, addr=addr, conflate=True)
liveLocation = messaging.pub_sock('liveLocation')
@ -219,13 +174,7 @@ def locationd_thread(gctx, addr, disabled_logs):
def main(gctx=None, addr="127.0.0.1"):
IN_CAR = os.getenv("IN_CAR", False)
disabled_logs = os.getenv("DISABLED_LOGS", "").split(",")
# No speed for now
disabled_logs.append('carState')
if IN_CAR:
addr = "192.168.5.11"
locationd_thread(gctx, addr, disabled_logs)
locationd_thread(gctx, addr)
if __name__ == "__main__":

Write Preview
Loading…
Cancel
Save