retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

xRevert "way cleaner" 

This reverts commit a4ffd8c226.
albatross
Harald Schafer 2020-05-15 14:29:34 -07:00
parent a4ffd8c226
commit 273e81715a
3 changed files with 43 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
selfdrive/locationd/locationd.py
View File

@ -147,20 +147,21 @@ class Localizer():
#fix.gpsTimeOfWeek = self.time.tow
fix.unixTimestampMillis = self.unix_timestamp_millis
if np.limalg.norm(fix.positionECEF.std) < 50 and self.calibrated:
if self.filter_ready and self.calibrated:
fix.status = 'valid'
elif np.limalg.norm(fix.positionECEF.std) < 50:
elif self.filter_ready:
fix.status = 'uncalibrated'
else:
fix.status = 'uninitialized'
return fix
def update_kalman(self, time, kind, meas):
try:
self.kf.predict_and_observe(time, kind, meas)
except KalmanError:
cloudlog.error("Error in predict and observe, kalman reset")
self.reset_kalman()
if self.filter_ready:
try:
self.kf.predict_and_observe(time, kind, meas)
except KalmanError:
cloudlog.error("Error in predict and observe, kalman reset")
self.reset_kalman()
#idx = bisect_right([x[0] for x in self.observation_buffer], time)
#self.observation_buffer.insert(idx, (time, kind, meas))
#while len(self.observation_buffer) > 0 and self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
@ -170,19 +171,40 @@ class Localizer():
def handle_gps(self, current_time, log):
self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
fix_ecef = self.converter.ned2ecef([0, 0, 0])
vel_ecef = self.converter.ned2ecef_matrix.dot(np.array(log.vNED))
#self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3))
self.unix_timestamp_millis = log.timestamp
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)
if gps_est_error > 50:
cloudlog.error("Locationd vs ubloxLocation difference too large, kalman reset")
self.reset_kalman(current_time)
self.update_kalman(current_time, ObservationKind.ECEF_POS, fix_ecef)
self.update_kalman(current_time, ObservationKind.ECEF_VEL, vel_ecef)
# TODO initing with bad bearing not allowed, maybe not bad?
if not self.filter_ready and log.speed > 5:
self.filter_ready = True
initial_ecef = fix_ecef
gps_bearing = math.radians(log.bearing)
initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing])
initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef)
gps_speed = log.speed
quat_uncertainty = 0.2**2
initial_state = LiveKalman.initial_x
initial_covs_diag = LiveKalman.initial_P_diag
initial_state[States.ECEF_POS] = initial_ecef
initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat
initial_state[States.ECEF_VELOCITY] = rot_from_quat(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)
cloudlog.info("Filter initialized")
elif self.filter_ready:
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)
if gps_est_error > 50:
cloudlog.error("Locationd vs ubloxLocation difference too large, kalman reset")
self.reset_kalman()
def handle_car_state(self, current_time, log):
self.speed_counter += 1
@ -234,9 +256,9 @@ class Localizer():
self.calib_from_device = self.device_from_calib.T
self.calibrated = log.calStatus == 1
def reset_kalman(self, current_time=None):
self.filter_time = current_time
self.kf.init_state(self.kf.x, covs=np.diag(LiveKalman.initial_P_diag), filter_time=current_time)
def reset_kalman(self):
self.filter_time = None
self.filter_ready = False
self.observation_buffer = []
self.gyro_counter = 0
@ -270,7 +292,7 @@ def locationd_thread(sm, pm, disabled_logs=[]):
elif sock == "liveCalibration":
localizer.handle_live_calib(t, sm[sock])
if sm.updated['gpsLocationExternal']:
if localizer.filter_ready and sm.updated['gpsLocationExternal']:
t = sm.logMonoTime['gpsLocationExternal']
msg = messaging.new_message('liveLocationKalman')
msg.logMonoTime = t

2
selfdrive/locationd/models/constants.py
View File

@ -27,7 +27,6 @@ class ObservationKind:
PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE = 22
PSEUDORANGE_RATE = 23
ECEF_VEL = 31
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
@ -37,7 +36,6 @@ class ObservationKind:
STEER_RATIO = 29 # [-]
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
names = [
'Unknown',
'No observation',

5
selfdrive/locationd/models/live_kf.py
View File

@ -172,7 +172,6 @@ class LiveKalman():
h_speed_sym = sp.Matrix([speed * odo_scale])
h_pos_sym = sp.Matrix([x, y, z])
h_vel_sym = sp.Matrix([vx, vy, vz])
h_imu_frame_sym = sp.Matrix(imu_angles)
h_relative_motion = sp.Matrix(quat_rot.T * v)
@ -182,7 +181,6 @@ class LiveKalman():
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, None],
[h_vel_sym, ObservationKind.ECEF_VEL, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_imu_frame_sym, ObservationKind.IMU_FRAME, None]]
@ -199,8 +197,7 @@ class LiveKalman():
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]),
ObservationKind.ECEF_VEL: np.diag([1**2, 1**2, 1**2])}
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
# init filter
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)