def __init__(self, dt, wheel_base, var_rate_enc, meas_da, var_a_s=4.0, var_a_p=0.25, var_a_a=math.radians(45.0)**2, control_model=None): sp = MerweScaledSigmaPoints(n=self.STATE_LEN, alpha=0.3, beta=2., kappa=-1.) UnscentedKalmanFilter.__init__(self, dim_x=self.STATE_LEN, dim_z=self.MEAS_LEN, dt=dt, fx=self.f_func, residual_x=self.x_residual_fn, x_mean_fn=self.x_mean_fn, hx=self.h_func, residual_z=self.h_residual_fn, z_mean_fn=self.h_mean_fn, points=sp) self.wheel_base = wheel_base self.var_a_s = var_a_s self.var_a_p = var_a_p self.var_a_a = var_a_a # starting point and covariance self.x = np.array(self.STATE_LEN*[0., ]) sig_a_start = math.radians(5.0) self.P = np.diag([0.5**2, 0.5**2, self.var_a_s**2, self.var_a_p**2, sig_a_start**2, self.var_a_a**2]) self.meas_da = meas_da self.prev_x = None self.prev_encoders = [0, 0, 0] self.tank_encoder = TankEncoderTracker(wheel_base, var_rate_enc, var_rate_enc) # control model # calculates linear and angular accelerations from motors self.control_model = control_model return
def __init__(self, dim_x, dim_z, dt, hx, fx, points, sqrt_fn=None, x_mean_fn=None, z_mean_fn=None, residual_x=None, residual_z=None, alpha=0.01): """ Create a Kalman filter. You are responsible for setting the various state variables to reasonable values; the defaults below will not give you a functional filter. """ #pylint: disable=too-many-arguments UnscentedKalmanFilter.__init__(self, dim_x, dim_z, dt, hx, fx, points, sqrt_fn=sqrt_fn, x_mean_fn=x_mean_fn, z_mean_fn=z_mean_fn, residual_x=residual_x, residual_z=residual_z) self.beta_n = chi2.ppf(1.0 - alpha, dim_z)
def __init__(self, dt, wheel_base, var_rate_enc, meas_da): sp = MerweScaledSigmaPoints(n=self.STATE_LEN, alpha=0.3, beta=2., kappa=-1.) UnscentedKalmanFilter.__init__(self, dim_x=self.STATE_LEN, dim_z=self.MEAS_LEN, dt=dt, fx=self.f_func, residual_x=self.x_residual_fn, x_mean_fn=self.x_mean_fn, hx=self.h_func, residual_z=self.h_residual_fn, z_mean_fn=self.h_mean_fn, points=sp) self.wheel_base = wheel_base self.var_a_s = 1.0 self.var_a_p = 0.25 self.var_a_a = math.radians(20.0)**2 # starting point and covariance self.x = np.array(self.STATE_LEN * [ 0., ]) sig_a_start = math.radians(5.0) self.P = np.diag([ 0.5**2, 0.5**2, self.var_a_s**2, self.var_a_p**2, sig_a_start**2, self.var_a_a**2 ]) self.meas_da = meas_da self.prev_x = None self.prev_encoders = [0, 0, 0] self.tank_encoder = TankEncoderTracker(wheel_base, var_rate_enc, var_rate_enc) self.prev_encoder_pos = [0, 0, 0] return
def __init__(self, dt, wheel_base, var_rate_enc, meas_da): sp = MerweScaledSigmaPoints(n=8, alpha=0.3, beta=2., kappa=-1.) UnscentedKalmanFilter.__init__( self, dim_x=8, dim_z=4, dt=dt, fx=TankKalmanDelta3.f_func, residual_x=TankKalmanDelta3.x_residual_fn, x_mean_fn=TankKalmanDelta3.x_mean_fn, hx=TankKalmanDelta3.h_func, residual_z=TankKalmanDelta3.h_residual_fn, z_mean_fn=TankKalmanDelta3.h_mean_fn, points=sp) self.wheel_base = wheel_base self.var_a_s = 4.0 self.var_a_p = 1e-10 self.var_a_a = math.radians(45.0) # starting point and covariance self.x = np.array(8 * [ 0., ]) sig_a_start = math.radians(5.0) self.P = np.diag([ 0.5**2, 0.5**2, dt**2 * self.var_a_s, self.var_a_s, dt**2 * self.var_a_p, sig_a_start**2, dt**2 * self.var_a_a, self.var_a_a ]) self.meas_da = meas_da self.prev_x = None self.prev_encoders = [0, 0, 0] self.tank_encoder = TankEncoderTracker(wheel_base, var_rate_enc, var_rate_enc) return