def __init__(self, generated_dir):
self.dim_state = self.initial_x.shape[0]
self.dim_state_err = self.initial_P_diag.shape[0]
self.obs_noise = {
ObservationKind.ODOMETRIC_SPEED:
np.atleast_2d(0.2**2),
ObservationKind.PHONE_GYRO:
np.diag([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL:
np.diag([.5**2, .5**2, .5**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.NO_ROT:
np.diag([0.00025**2, 0.00025**2, 0.00025**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)
def __init__(self, generated_dir, N=4, max_tracks=3000):
name = f"{self.name}_{N}"
self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**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.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
# MSCKF stuff
self.N = N
self.dim_main = LocKalman.x_initial.shape[0]
self.dim_main_err = LocKalman.P_initial.shape[0]
self.dim_state = self.dim_main + self.dim_augment * self.N
self.dim_state_err = self.dim_main_err + self.dim_augment_err * self.N
if self.N > 0:
x_initial, P_initial, Q = self.pad_augmented(self.x_initial, self.P_initial, self.Q)
self.computer = LstSqComputer(generated_dir, N)
self.max_tracks = max_tracks
# init filter
self.filter = EKF_sym(generated_dir, name, Q, x_initial, P_initial, self.dim_main, self.dim_main_err,
N, self.dim_augment, self.dim_augment_err, self.maha_test_kinds)
def __init__(self, generated_dir, N=4, max_tracks=3000):
name = f"{self.name}_{N}"
self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**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.NO_ROT: np.diag([0.0025**2, 0.0025**2, 0.0025**2]),
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]),
ObservationKind.NO_ACCEL: np.diag([0.0025**2, 0.0025**2, 0.0025**2])}
# MSCKF stuff
self.N = N
self.dim_main = LocKalman.x_initial.shape[0]
self.dim_main_err = LocKalman.P_initial.shape[0]
self.dim_state = self.dim_main + self.dim_augment * self.N
self.dim_state_err = self.dim_main_err + self.dim_augment_err * self.N
if self.N > 0:
x_initial, P_initial, Q = self.pad_augmented(self.x_initial, self.P_initial, self.Q) # lgtm[py/mismatched-multiple-assignment] pylint: disable=unbalanced-tuple-unpacking
self.computer = LstSqComputer(generated_dir, N)
self.max_tracks = max_tracks
self.quaternion_idxs = [3, ] + [(self.dim_main + i * self.dim_augment + 3)for i in range(self.N)]
# init filter
self.filter = EKF_sym(generated_dir, name, Q, x_initial, P_initial, self.dim_main, self.dim_main_err,
N, self.dim_augment, self.dim_augment_err, self.maha_test_kinds, self.quaternion_idxs)
def __init__(self, generated_dir):
dim_state = self.initial_x.shape[0]
dim_state_err = self.initial_P_diag.shape[0]
# init filter
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x,
np.diag(self.initial_P_diag), dim_state,
dim_state_err)
def __init__(self, generated_dir):
self.dim_state = self.x_initial.shape[0]
# init filter
self.filter = EKF_sym(generated_dir,
self.name,
self.Q,
self.x_initial,
self.P_initial,
self.dim_state,
self.dim_state,
maha_test_kinds=self.maha_test_kinds)
def __init__(self,
generated_dir,
steer_ratio=15,
stiffness_factor=1,
angle_offset=0):
dim_state = self.initial_x.shape[0]
dim_state_err = self.P_initial.shape[0]
x_init = self.initial_x
x_init[States.STEER_RATIO] = steer_ratio
x_init[States.STIFFNESS] = stiffness_factor
x_init[States.ANGLE_OFFSET] = angle_offset
# init filter
self.filter = EKF_sym(generated_dir,
self.name,
self.Q,
self.initial_x,
self.P_initial,
dim_state,
dim_state_err,
global_vars=self.global_vars)