def __init__(self,
init: Union[ObjectTarget3D, TrackingTarget3D],
Q: np.ndarray = np.eye(6)):
'''
:param init: Initial state for the target
:param Q: System process noise
'''
sigma_points = kf.JulierSigmaPoints(6)
self._filter = kf.UnscentedKalmanFilter(
dim_x=6,
dim_z=3,
dt=None,
fx=motion_CTRA,
hx=lambda s: s[:3],
points=sigma_points,
x_mean_fn=self._state_mean,
z_mean_fn=self._state_mean,
residual_x=self._state_diff,
residual_z=self._state_diff,
)
self._filter.Q = np.asarray(Q).reshape(6, 6)
# feed initial state
yaw, pitch, roll = init.orientation.as_euler("ZYX")
self._filter.x = np.array(
[init.position[0], init.position[1], yaw, 0, 0, 0])
self._filter.P = np.copy(self._filter.Q)
self._filter.P[:2, :2] = init.position_var[:2, :2]
self._filter.P[2, 2] = init.orientation_var
self._save_z = init.position[2]
self._save_z_var = init.position_var[2, 2]
self._save_pitch = pitch # TODO: use simple bayesian filter for pitch and roll values
self._save_roll = roll
self.check_valid("initialize")
def __init__(self,
init: Union[ObjectTarget3D, TrackingTarget3D],
Q: np.ndarray = np.eye(4)):
'''
:param init: initial state for the target
:param Q: system process noise
'''
# create filter
sigma_points = kf.JulierSigmaPoints(6)
self._filter = kf.UnscentedKalmanFilter(dim_x=4,
dim_z=2,
dt=None,
fx=motion_CV,
hx=lambda s: s[:2],
points=sigma_points)
self._filter.Q = np.asarray(Q).reshape(4, 4)
# feed initial state
self._filter.x = np.array([init.position[0], init.position[1], 0, 0])
self._filter.P = np.copy(self._filter.Q)
self._filter.P[:2, :2] = init.position_var[:2, :2]
self._save_z = init.position[
2] # TODO: use simple bayesian filter for z values
self._save_z_var = init.position_var[2, 2]
self._save_ori = init.orientation # TODO: use simple bayesian filter for orientation
self._save_ori_var = init.orientation_var
def __init__(self, Q=None):
self._filter = kf.UnscentedKalmanFilter(
dim_x=4,
dim_z=2,
dt=None,
fx=dm.motion_CV,
hx=lambda s: s[:2],
points=kf.MerweScaledSigmaPoints(4, alpha=.1, beta=.2, kappa=-1))
if Q is None:
self._filter.Q = np.diag([1., 1., 5., 5.])
else:
assert Q.shape == (4, 4)
self._filter.Q = Q
self._inited = False
def __init__(self, model, x0, measurement_eqn=None, **kwargs):
super().__init__(model, x0, measurement_eqn, **kwargs)
self._input = None
self.t = self.parameters['t0']
if measurement_eqn is None:
def measure(x):
x = {key: value for (key, value) in zip(model.states, x)}
z = model.output(x)
return array(list(z.values()))
else:
def measure(x):
x = {key: value for (key, value) in zip(model.states, x)}
z = measurement_eqn(x)
return array(list(z.values()))
if 'Q' not in self.parameters:
self.parameters['Q'] = diag([1.0e-1 for i in model.states])
if 'R' not in self.parameters:
self.parameters['R'] = diag(
[1.0e-1 for i in range(len(measure(x0.values())))])
def state_transition(x, dt):
x = {key: value for (key, value) in zip(model.states, x)}
x = model.next_state(x, self._input, dt)
return array(list(x.values()))
num_states = len(model.states)
num_measurements = len(model.outputs)
points = kalman.MerweScaledSigmaPoints(num_states,
alpha=self.parameters['alpha'],
beta=self.parameters['beta'],
kappa=self.parameters['kappa'])
self.filter = kalman.UnscentedKalmanFilter(num_states,
num_measurements,
self.parameters['dt'],
measure, state_transition,
points)
self.filter.x = array(list(x0.values()))
self.filter.Q = self.parameters['Q']
self.filter.R = self.parameters['R']
def __init__(self, Q=None):
self._filter = kf.UnscentedKalmanFilter(
dim_x=6,
dim_z=3,
dt=None,
fx=dm.motion_CTRA,
hx=lambda s: s[:3],
points=kf.MerweScaledSigmaPoints(6, alpha=.1, beta=.2, kappa=-1),
x_mean_fn=self._state_mean,
z_mean_fn=self._state_mean,
residual_x=self._state_diff,
residual_z=self._state_diff,
)
if Q is None:
self._filter.Q = np.diag([1., 1., 1., 3., 10., 3.])
else:
assert Q.shape == (6, 6)
self._filter.Q = Q
self._inited = False
xout[1] = x[1]
return
# measurement model dengan RNN
def hx(x):
return x[:1] # return position [x]. Slicing semua kolom 1 menjadi row of array
'''
dim_x = trainX.ndim
dim_z = 1
points = kalman.MerweScaledSigmaPoints(
Ldim, alpha=.3, beta=2., kappa=0
) #makin besar alpha, makin menyebar data[:train_data], range(train_data)
kf = kalman.UnscentedKalmanFilter(dim_x=Xdim,
dim_z=n_hx_output,
dt=.1,
hx=None,
fx=None,
points=points) # sigma = points
kf.x = np.array(np.zeros(kf.x.shape)) # initial shape
kf.P = .2 # inisial uncertainty
kf.R = .5
z_std = 0.1
##WEIGHTS
def bobotUKF(data, X, k):
lambda_ = points.alpha**2 * (points.n + points.kappa) - points.n
Wc = np.full(2 * points.n + 1, 1. / (2 * (points.n + lambda_)))
Wm = np.full(2 * points.n + 1, 1. / (2 * (points.n + lambda_)))
Wc[0] = lambda_ / (points.n + lambda_) + (1. - points.alpha**2 +
points.beta)
sigma_range = 0.3 sigma_bearing = 0.1 ellipse_step = 1 step = 10 print(landmarks) print(ref_landmarks) landmarks = ref_landmarks points = kalman.MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0, subtract=residual_x) ukf = kalman.UnscentedKalmanFilter(dim_x=3, dim_z=2*len(landmarks), fx=move, hx=Hx, dt=dt, points=points, x_mean_fn=state_mean, z_mean_fn=z_mean, residual_x=residual_x, residual_z=residual_h) ukf.x = np.array([2, 6, .3]) ukf.P = np.diag([.1, .1, .05]) ukf.R = np.diag([sigma_range**2, sigma_bearing**2]*len(landmarks)) ukf.Q = np.eye(3)*0.0001 sim_pos = ukf.x.copy() plt.figure() # plot landmarks if len(landmarks) > 0: