class Kalman_Localizer(Kalman):
def __init__(self, mapping=False, name=None):
super().__init__(mapping, name)
self.mapdata = Map(name)
def set_initial_position(self, gps_pos, attitude):
""" Initialize the position of the car as a first guess """
self.position, self.attitude = projection(self.mapdata.gps_pos,
self.mapdata.attitude,
gps_pos, attitude)
def localize(self, new_data, gps_guess=False):
""" Find the position of a image thanks to the map """
if gps_guess:
mapping_img, _ = self.mapdata.extract_from_map(
new_data.gps_pos, new_data.attitude, np.shape(new_data.img))
gps_pos, attitude = projection(self.mapdata.gps_pos,
self.mapdata.attitude,
new_data.gps_pos, new_data.attitude)
mapping_data = RadarData(None, mapping_img, gps_pos, attitude)
else:
mapping_img, _ = self.mapdata.extract_from_map(
self.position, self.attitude, np.shape(new_data.img))
mapping_data = RadarData(None, mapping_img, self.position,
self.attitude)
self.position, self.attitude = new_data.image_position_from(
mapping_data)
self.last_data = RadarData(new_data.id, new_data.img, self.position,
self.attitude)
if self.mapping:
self.mapdata.add_data(self.last_data)
return deepcopy(self.position), deepcopy(self.attitude)
class Kalman_Localizer(Kalman):
def __init__(self, mapping=False, name=None):
super().__init__(mapping, name)
self.mapdata = Map(name)
def set_covariances(self, gps_pos_std, gps_att_std, cv2_trans_std,
cv2_rot_std):
""" Set covariances Q and R of the Kalman Filter """
self.Q = np.block(
[[np.zeros((3, 3)), np.zeros((3, 3))],
[
np.zeros((3, 3)),
np.diag([gps_pos_std**2, gps_pos_std**2, gps_att_std**2])
]])
self.R = np.diag([cv2_trans_std**2, cv2_trans_std**2, cv2_rot_std**2])
def set_initial_position(self, gps_pos, attitude):
""" Initialize the position of the car as a first guess """
self.position, self.attitude = projection(self.mapdata.gps_pos,
self.mapdata.attitude,
gps_pos, attitude)
def localize(self, new_data, gps_guess=False):
""" Find the position of a image thanks to the map """
if gps_guess:
mapping_img, _ = self.mapdata.extract_from_map(
new_data.gps_pos, new_data.attitude, np.shape(new_data.img))
gps_pos, attitude = projection(self.mapdata.gps_pos,
self.mapdata.attitude,
new_data.gps_pos, new_data.attitude)
mapping_data = RadarData(None, mapping_img, gps_pos, attitude)
else:
mapping_img, _ = self.mapdata.extract_from_map(
self.position, self.attitude, np.shape(new_data.img))
mapping_data = RadarData(None, mapping_img, self.position,
self.attitude)
self.position, self.attitude = new_data.image_position_from(
mapping_data)
self.last_data = RadarData(new_data.id, new_data.img, self.position,
self.attitude)
if self.mapping:
self.mapdata.add_data(self.last_data)
return deepcopy(self.position), deepcopy(self.attitude)
class Kalman:
"""
Class used as a basis for all Kalman filter classes
"""
def __init__(self, mapping=True, name=None, bias_estimation=False):
self.mapping = mapping
if mapping:
self.mapdata = Map(name)
else:
self.mapdata = None
self.last_data = None
self.position = None # position in ECEF
self.attitude = None # from ECEF to rbd
self.pos2D = np.zeros(2) # position in 2D map
self.att2D = 0 # attitude in 2D map
self.innovation = None
self.bias_estimation = bias_estimation
self.bias = np.zeros(3)
self.init_default_covariance()
def init_default_covariance(self):
""" Initialize the covariances with default values """
self.Q = np.block([[
np.array([[0.00051736, -0.00091164], [-0.00091164, 0.00519678]]),
np.zeros((2, 1))
], [np.zeros(2),
3.54317141e-06]]) # determined from reader.plot_gps_evaluation
self.R = np.diag([
0.01**2, 0.01**2, np.deg2rad(0.01)**2
]) # To tune when knowing the exact distribution of GPS measurements
if self.bias_estimation:
self.P = np.block([[self.R, np.zeros((3, 3))],
[np.zeros((3, 3)), self.R]])
else:
self.P = deepcopy(self.R)
def set_initial_position(self, gps_pos, attitude):
""" Set the initial position of the map in ECEF """
self.position = deepcopy(gps_pos)
self.attitude = deepcopy(attitude)
def add(self, new_data, fusion=True):
""" Add a new radar data on the map
fusion: if false, add raw data to the map
"""
if self.last_data is None:
# use position/attitude custom initialisation
if not (self.position is None):
new_data = RadarData(new_data.id, new_data.img, self.position,
new_data.attitude)
if not (self.attitude is None):
new_data = RadarData(new_data.id, new_data.img,
new_data.gps_pos, self.attitude)
if self.mapping:
self.mapdata.add_data(new_data)
else:
self.mapdata = deepcopy(new_data)
self.last_data = deepcopy(new_data)
self.position = deepcopy(self.mapdata.gps_pos)
self.attitude = deepcopy(self.mapdata.attitude)
else:
if fusion:
self.predict(new_data)
self.update(new_data)
self.position = self.process_position(new_data)
self.attitude = self.process_attitude(new_data)
else:
self.position = new_data.gps_pos
self.attitude = new_data.attitude
self.last_data = RadarData(new_data.id, new_data.img,
self.position, self.attitude)
if self.mapping:
self.mapdata.add_data(self.last_data)
return deepcopy(self.position), deepcopy(self.attitude)
class Kalman:
"""
Class used as a basis for all Kalman filter classes
"""
def __init__(self, mapping=True, name=None):
self.mapping = mapping
if mapping:
self.mapdata = Map(name)
else:
self.mapdata = None
self.last_data = None
self.position = None # in ECEF
self.attitude = None # from ECEF to rbd
self.prev_pos2D = np.zeros(2) # (x,y) position on the map
self.prev_att2D = 0 # orientation in the map frame
self.trans = np.zeros(
2) # translation from the previous image (not in the map frame !)
self.rot = 0 # orientation from the previous image (not in the map frame !)
self.innovation = None
self.init_default_covariance()
def init_default_covariance(self):
""" Initialize the covariances with default values """
gps_pos_std = 0.05 # standard deviation of GPS position measurement
gps_att_std = np.deg2rad(
1) # standard deviation of GPS attitude measurement
cv2_trans_std = 0.04 # standard deviation of CV2 translation measurement
cv2_rot_std = np.deg2rad(
1) # standard deviation of CV2 rotation measurement
self.set_covariances(gps_pos_std, gps_att_std, cv2_trans_std,
cv2_rot_std)
self.P = np.zeros((
6,
6)) # the initial 2D position and orientation is known to be (0,0)
def set_initial_position(self, gps_pos, attitude):
""" Set the initial position of the map in ECEF """
self.position = deepcopy(gps_pos)
self.attitude = deepcopy(attitude)
def add(self, new_data, fusion=True):
""" Add a new radar data on the map
fusion: if false, add raw data to the map
"""
if self.last_data is None:
# use position/attitude custom initialisation
if not (self.position is None):
new_data = RadarData(new_data.id, new_data.img, self.position,
new_data.attitude)
if not (self.attitude is None):
new_data = RadarData(new_data.id, new_data.img,
new_data.position, self.attitude)
if self.mapping:
self.mapdata.add_data(new_data)
else:
self.mapdata = deepcopy(new_data)
self.last_data = deepcopy(new_data)
self.position = deepcopy(self.mapdata.gps_pos)
self.attitude = deepcopy(self.mapdata.attitude)
else:
if fusion:
self.predict(new_data)
self.update(new_data)
self.position = self.process_position(new_data)
self.attitude = self.process_attitude(new_data)
else:
self.position = new_data.gps_pos
self.attitude = new_data.attitude
self.last_data = RadarData(new_data.id, new_data.img,
self.position, self.attitude)
if self.mapping:
self.mapdata.add_data(self.last_data)
return deepcopy(self.position), deepcopy(self.attitude)
