parent
a4ffd8c226
commit
273e81715a
|
@ -147,20 +147,21 @@ class Localizer():
|
||||||
#fix.gpsTimeOfWeek = self.time.tow
|
#fix.gpsTimeOfWeek = self.time.tow
|
||||||
fix.unixTimestampMillis = self.unix_timestamp_millis
|
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'
|
fix.status = 'valid'
|
||||||
elif np.limalg.norm(fix.positionECEF.std) < 50:
|
elif self.filter_ready:
|
||||||
fix.status = 'uncalibrated'
|
fix.status = 'uncalibrated'
|
||||||
else:
|
else:
|
||||||
fix.status = 'uninitialized'
|
fix.status = 'uninitialized'
|
||||||
return fix
|
return fix
|
||||||
|
|
||||||
def update_kalman(self, time, kind, meas):
|
def update_kalman(self, time, kind, meas):
|
||||||
try:
|
if self.filter_ready:
|
||||||
self.kf.predict_and_observe(time, kind, meas)
|
try:
|
||||||
except KalmanError:
|
self.kf.predict_and_observe(time, kind, meas)
|
||||||
cloudlog.error("Error in predict and observe, kalman reset")
|
except KalmanError:
|
||||||
self.reset_kalman()
|
cloudlog.error("Error in predict and observe, kalman reset")
|
||||||
|
self.reset_kalman()
|
||||||
#idx = bisect_right([x[0] for x in self.observation_buffer], time)
|
#idx = bisect_right([x[0] for x in self.observation_buffer], time)
|
||||||
#self.observation_buffer.insert(idx, (time, kind, meas))
|
#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:
|
#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):
|
def handle_gps(self, current_time, log):
|
||||||
self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
|
self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
|
||||||
fix_ecef = self.converter.ned2ecef([0, 0, 0])
|
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.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3))
|
||||||
self.unix_timestamp_millis = log.timestamp
|
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)
|
# TODO initing with bad bearing not allowed, maybe not bad?
|
||||||
self.update_kalman(current_time, ObservationKind.ECEF_VEL, vel_ecef)
|
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):
|
def handle_car_state(self, current_time, log):
|
||||||
self.speed_counter += 1
|
self.speed_counter += 1
|
||||||
|
@ -234,9 +256,9 @@ class Localizer():
|
||||||
self.calib_from_device = self.device_from_calib.T
|
self.calib_from_device = self.device_from_calib.T
|
||||||
self.calibrated = log.calStatus == 1
|
self.calibrated = log.calStatus == 1
|
||||||
|
|
||||||
def reset_kalman(self, current_time=None):
|
def reset_kalman(self):
|
||||||
self.filter_time = current_time
|
self.filter_time = None
|
||||||
self.kf.init_state(self.kf.x, covs=np.diag(LiveKalman.initial_P_diag), filter_time=current_time)
|
self.filter_ready = False
|
||||||
self.observation_buffer = []
|
self.observation_buffer = []
|
||||||
|
|
||||||
self.gyro_counter = 0
|
self.gyro_counter = 0
|
||||||
|
@ -270,7 +292,7 @@ def locationd_thread(sm, pm, disabled_logs=[]):
|
||||||
elif sock == "liveCalibration":
|
elif sock == "liveCalibration":
|
||||||
localizer.handle_live_calib(t, sm[sock])
|
localizer.handle_live_calib(t, sm[sock])
|
||||||
|
|
||||||
if sm.updated['gpsLocationExternal']:
|
if localizer.filter_ready and sm.updated['gpsLocationExternal']:
|
||||||
t = sm.logMonoTime['gpsLocationExternal']
|
t = sm.logMonoTime['gpsLocationExternal']
|
||||||
msg = messaging.new_message('liveLocationKalman')
|
msg = messaging.new_message('liveLocationKalman')
|
||||||
msg.logMonoTime = t
|
msg.logMonoTime = t
|
||||||
|
|
|
@ -27,7 +27,6 @@ class ObservationKind:
|
||||||
PSEUDORANGE_RATE_GLONASS = 21
|
PSEUDORANGE_RATE_GLONASS = 21
|
||||||
PSEUDORANGE = 22
|
PSEUDORANGE = 22
|
||||||
PSEUDORANGE_RATE = 23
|
PSEUDORANGE_RATE = 23
|
||||||
ECEF_VEL = 31
|
|
||||||
|
|
||||||
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
|
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
|
||||||
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
|
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
|
||||||
|
@ -37,7 +36,6 @@ class ObservationKind:
|
||||||
STEER_RATIO = 29 # [-]
|
STEER_RATIO = 29 # [-]
|
||||||
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
|
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
|
||||||
|
|
||||||
|
|
||||||
names = [
|
names = [
|
||||||
'Unknown',
|
'Unknown',
|
||||||
'No observation',
|
'No observation',
|
||||||
|
|
|
@ -172,7 +172,6 @@ class LiveKalman():
|
||||||
h_speed_sym = sp.Matrix([speed * odo_scale])
|
h_speed_sym = sp.Matrix([speed * odo_scale])
|
||||||
|
|
||||||
h_pos_sym = sp.Matrix([x, y, z])
|
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_imu_frame_sym = sp.Matrix(imu_angles)
|
||||||
|
|
||||||
h_relative_motion = sp.Matrix(quat_rot.T * v)
|
h_relative_motion = sp.Matrix(quat_rot.T * v)
|
||||||
|
@ -182,7 +181,6 @@ class LiveKalman():
|
||||||
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
|
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
|
||||||
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
|
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
|
||||||
[h_pos_sym, ObservationKind.ECEF_POS, None],
|
[h_pos_sym, ObservationKind.ECEF_POS, None],
|
||||||
[h_vel_sym, ObservationKind.ECEF_VEL, None],
|
|
||||||
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
|
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
|
||||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
|
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
|
||||||
[h_imu_frame_sym, ObservationKind.IMU_FRAME, 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.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.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.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_POS: np.diag([5**2, 5**2, 5**2])}
|
||||||
ObservationKind.ECEF_VEL: np.diag([1**2, 1**2, 1**2])}
|
|
||||||
|
|
||||||
# init filter
|
# 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)
|
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)
|
||||||
|
|
Loading…
Reference in a new issue