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
