def __init__(self, sensors):
self.sensors = sensors
# initialize ukf
trans_cov = numpy.eye(2) * 20
obs_cov = numpy.eye(sensors.shape[0]) * 100
self.mean = numpy.random.normal(256, 256, 2)
self.cov = numpy.eye(2) * 128
# self.ukf = pykalman.UnscentedKalmanFilter(self.transition, self.observation, trans_cov, obs_cov, self.mean, self.cov)
self.ukf = pykalman.AdditiveUnscentedKalmanFilter(
self.transition_, self.observation_, trans_cov, obs_cov, self.mean,
self.cov)
def __init__(self, sensors):
self.sensors = sensors
# initialize ukf
trans_cov = numpy.eye(2) * 20 # transition noise covariance matrix
obs_cov = numpy.eye(
sensors.shape[0]) * 100 # observation noise covariance matrix
self.mean = numpy.random.normal(256, 256, 2) # initial mean
self.cov = numpy.eye(2) * 128 # initial covariance
# In this system, we assume simple additive noises. So, AdditiveUnscentedKalmanFilter is more suitable (fast and robust).
# But, you can use the usual UKF which doesn't assume such additive noises as well.
# self.ukf = pykalman.UnscentedKalmanFilter(self.transition, self.observation, trans_cov, obs_cov, self.mean, self.cov)
self.ukf = pykalman.AdditiveUnscentedKalmanFilter(
self.transition_, self.observation_, trans_cov, obs_cov, self.mean,
self.cov)
def __init__(self, param, mode):
self.transition_fun = self.transition_lower
if mode == 'lower':
self.observation_fun = self.observation_lower
else:
self.observation_fun = self.observation_upper
self.trans_cov = param.trans_cov
self.obs_cov = param.obs_cov
self.mean = param.mean
self.cov = param.init_trans_cov
self.trnas_matrix = param.trans_matrx
self.cur_state = []
self.new_state = []
self.ukf = pykalman.AdditiveUnscentedKalmanFilter(
self.transition_fun, self.observation_fun, self.trans_cov,
self.obs_cov, self.mean, self.cov)
def unscented_kalman_filter_sm(foh,
fhv,
fvh,
o_array,
v_array,
smooth_param=300.0):
filter_type = 'additive'
#filter_type = 'normal'
trans_mat = get_state_transition_matrix(foh, fhv, fvh)
obser_mat = get_observation_matrix()
trans_cov = 1.0 * numpy.diag(numpy.ones((trans_mat.shape[0], )))
# DEVEL
# -------------------------------------------
for i in range(1, trans_mat.shape[0]):
trans_cov[i] = 0.0
# -------------------------------------------
obser_cov = smooth_param * numpy.diag(numpy.ones((obser_mat.shape[0], )))
init_state = numpy.zeros((trans_mat.shape[0], ))
init_state_cov = numpy.diag(numpy.ones((trans_mat.shape[0], )))
if filter_type == 'additive':
def state_transition_func(curr_state):
next_state = numpy.dot(trans_mat, curr_state)
next_state[next_state < 0] = 0.0
return next_state
def observation_func(curr_state):
obser = numpy.dot(obser_mat, curr_state)
obser[obser < 0] = 0.0
return obser
kalman = pykalman.AdditiveUnscentedKalmanFilter(
transition_functions=state_transition_func,
observation_functions=observation_func,
transition_covariance=trans_cov,
observation_covariance=obser_cov,
initial_state_mean=init_state,
initial_state_covariance=init_state_cov)
else:
def state_transition_func(curr_state, noise):
next_state = numpy.dot(trans_mat, curr_state)
mask = next_state < 0
next_state[mask] = 0.0
next_state += noise
return next_state
def observation_func(curr_state, noise):
obser = numpy.dot(obser_mat, curr_state)
mask = obser < 0
obser[mask] = 0.0
obser += noise
return obser
kalman = pykalman.UnscentedKalmanFilter(
transition_functions=state_transition_func,
observation_functions=observation_func,
transition_covariance=trans_cov,
observation_covariance=obser_cov,
initial_state_mean=init_state,
initial_state_covariance=init_state_cov)
n = o_array.shape[0]
data = numpy.zeros((n, 2))
data[:, 0] = o_array
data[:, 1] = v_array
state_filt, state_cov = kalman.smooth(data)
return extract_kalman_data(foh, state_filt, state_cov)
#Obs_Covariance = np.array([[1e-2,0,0],[0,1e-2,0],[0,0,1e-8]])
Trans_Variance = np.ones(States.shape[0])
Trans_Variance[:-5] = (dt**2) * 1e-2
Trans_Variance[-5] = (dt**2) * 1e-2 #Transition Uncertainty in incomingTMass
Trans_Variance[-4] = (dt**2) * 1e-1 #Transition Uncertainty in tau_a
Trans_Variance[-3] = (dt**2) * 1e-1 #Transition Uncertainty in tau_s
Trans_Variance[-2] = (dt**2) * 1e-10 #Transition Uncertainty in Alpha
Trans_Variance[-1] = (dt**2) * 1e-3 #Transition Uncertainty in Beta
Trans_Covariance = +np.diag(
Trans_Variance) #np.ones((States.shape[0],States.shape[0]))/100000.
AUSKF = pk.AdditiveUnscentedKalmanFilter(
PODDS_Advect,
PODDS_Measure,
transition_covariance=Trans_Covariance,
observation_covariance=Obs_Covariance,
initial_state_mean=States[:, 0],
initial_state_covariance=Covariances[:, :, 0])
#State,Covariance = AUSKF.filter_update(State,Covariance,np.array([0.1,0.1]))
#T[::2] = np.ma.masked
#T[::3] = np.ma.masked
Mask = np.ones(times.size)
#TMeasured = np.ma.masked_array(T+ np.random.normal(0,1e-2,times.size),TMask)
#QMeasured = np.ma.masked_array(Q+ np.random.normal(0,5e-4,times.size),QMask)
TMeasured = T + np.random.normal(0, 1e-2, times.size)
T2Measured = T2 + np.random.normal(0, 1e-2, times.size)
tau_Measured = tau_a + np.random.normal(0, 1e-2, times.size)