parent
a4ffd8c226
commit
273e81715a
|
@ -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
|
||||
|
|
|
@ -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',
|
||||
|
|
|
@ -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)
|
||||
|
|
Loading…
Reference in New Issue