def reset(self):
self.kf = AdditiveUnscentedKalmanFilter(n_dim_state=self.n_state_dims)
self.obs_yaw_lin = YawLinear()
self.last_state_mean = None
self.last_state_covar = None
self.last_obs = None
self.initialized = False
self.reject_count = 0
def ukfinit(self):
self.ukf = AdditiveUnscentedKalmanFilter(
n_dim_obs=self.M,
n_dim_state=self.N,
transition_functions=self.transition_function,
observation_functions=self.observation_function,
transition_covariance=Q,
observation_covariance=self.R,
initial_state_mean=self.x,
initial_state_covariance=Q)
class IMULocation:
def __init__(self):
self.N = 10
self.M = 3
self.x = zeros((1,self.N))[0]
self.R = eye(3)*0.02
self.P = Q
self.time = -1
self.ukfinit()
self.state_equation = copy.deepcopy(state_equation)
def ukfinit(self):
self.ukf = AdditiveUnscentedKalmanFilter(n_dim_obs = self.M, n_dim_state = self.N,
transition_functions = self.transition_function,
observation_functions = self.observation_function,
transition_covariance = Q,
observation_covariance = self.R,
initial_state_mean = self.x,
initial_state_covariance = Q)
def locate(self, state, state_cov, delt_time, euler_angle, linear_acc, angular_rate):
self.delt_time = delt_time
self.u = tuple((hstack((linear_acc, angular_rate))))
(self.x, self.P) = self.ukf.filter_update(state, state_cov, euler_angle)
print linalg.norm(euler_angle-self.x[3:6])
return (self.x, self.P)
def transition_function(self, state):
return odeint(self.state_equation, state, [0, self.delt_time], tuple([self.u]))[1]
def observation_function(self, state):
return hstack((state[3:6]))
def ukfinit(self):
self.ukf = AdditiveUnscentedKalmanFilter(n_dim_obs = self.M, n_dim_state = self.N,
transition_functions = self.transition_function,
observation_functions = self.observation_function,
transition_covariance = Q,
observation_covariance = self.R,
initial_state_mean = self.x,
initial_state_covariance = Q)
def kalman_estimate_1D(x,P,va_measurements,x_history):
"""
This function takes the filtered velocity, acceleration data and covariance matrix (from previous filtration),
the filtered position history (for only a single dimension) and the measurements (for velocity
and acceleration). And returns the updated velocity,acceleration data and covariance matrix
and also updates the position history.
It uses an Additive Unscented Kalman Filter and calls "kalman_estimate_v_a" in order to achieve this.
The functions 'lambda x,w' and 'lambda x,v' are respectively the transition and observation functions
(https://pykalman.github.io/#id3).
# measurements for 1 dim in the form [v,a] # x_history is history of in format [x1,x2,x3,...]
"""
(x,P)=kalman_estimate_v_a(x,P,va_measurements)
aukf = AdditiveUnscentedKalmanFilter(lambda x, w: x + w, lambda x, v: x + v, observation_covariance=1)
x_history=x_history[-30:] + x[0]*dt + 0.5 * x[1] * dt * dt
x_filtered,temp = aukf.filter(x_history)
return (x,P,x_filtered)
def kalman_estimate_1D(x, P, va_measurements, x_history):
"""
This function takes the filtered velocity, acceleration data and covariance matrix (from previous filtration),
the filtered position history (for only a single dimension) and the measurements (for velocity
and acceleration). And returns the updated velocity,acceleration data and covariance matrix
and also updates the position history.
It uses an Additive Unscented Kalman Filter and calls "kalman_estimate_v_a" in order to achieve this.
The functions 'lambda x,w' and 'lambda x,v' are respectively the transition and observation functions
(https://pykalman.github.io/#id3).
# measurements for 1 dim in the form [v,a] # x_history is history of in format [x1,x2,x3,...]
"""
(x, P) = kalman_estimate_v_a(x, P, va_measurements)
aukf = AdditiveUnscentedKalmanFilter(lambda x, w: x + w,
lambda x, v: x + v,
observation_covariance=1)
x_history = x_history[-30:] + x[0] * dt + 0.5 * x[1] * dt * dt
x_filtered, temp = aukf.filter(x_history)
return (x, P, x_filtered)
class UWBLocation:
def __init__(self):
self.N = 10
self.M = 4
self.x = zeros((1, self.N))[0]
self.R = eye(4) * 0.0001
self.R[1:4, 1:4] = eye(3) * 0.000001
self.P = Q
self.time = -1
self.ukfinit()
self.state_equation = copy.deepcopy(state_equation)
self.u = tuple([[0, 0, -g, 0, 0, 0]])
def ukfinit(self):
self.ukf = AdditiveUnscentedKalmanFilter(
n_dim_obs=self.M,
n_dim_state=self.N,
transition_functions=self.transition_function,
observation_functions=self.observation_function,
transition_covariance=Q,
observation_covariance=self.R,
initial_state_mean=self.x,
initial_state_covariance=Q)
def locate(self, state, state_cov, delt_time, anchor_dis, anchor_pos,
euler_angle, linear_acc, angular_rate):
self.anchor_pos = anchor_pos
self.delt_time = delt_time
# [phi, theta, psi] = [state[3], state[4], state[5]]
# [cp, sp, ct, st, cs, ss] = [cos(phi), sin(phi), cos(theta),
# sin(theta), cos(psi), sin(psi)]
#self.u = tuple([[g * st, -g * ct * sp, -g * ct * cp,0,0,0]])
self.u = tuple([hstack((linear_acc, angular_rate))])
#print self.u
(self.x,
self.P) = self.ukf.filter_update(state, state_cov,
hstack((anchor_dis, euler_angle)))
return (self.x, self.P)
def transition_function(self, state):
return odeint(self.state_equation, state, [0, self.delt_time],
self.u)[1]
def observation_function(self, state):
return hstack((linalg.norm(state[0:3] - self.anchor_pos), state[3:6]))
class UWBLocation:
def __init__(self):
self.N = 10
self.M = 4
self.x = zeros((1,self.N))[0]
self.R = eye(4)*0.0001
self.R[1:4,1:4] = eye(3)*0.000001
self.P = Q
self.time = -1
self.ukfinit()
self.state_equation = copy.deepcopy(state_equation)
self.u = tuple([[0,0,-g,0,0,0]])
def ukfinit(self):
self.ukf = AdditiveUnscentedKalmanFilter(n_dim_obs = self.M, n_dim_state = self.N,
transition_functions = self.transition_function,
observation_functions = self.observation_function,
transition_covariance = Q,
observation_covariance = self.R,
initial_state_mean = self.x,
initial_state_covariance = Q)
def locate(self, state, state_cov, delt_time, anchor_dis, anchor_pos, euler_angle, linear_acc, angular_rate):
self.anchor_pos = anchor_pos
self.delt_time = delt_time
# [phi, theta, psi] = [state[3], state[4], state[5]]
# [cp, sp, ct, st, cs, ss] = [cos(phi), sin(phi), cos(theta),
# sin(theta), cos(psi), sin(psi)]
#self.u = tuple([[g * st, -g * ct * sp, -g * ct * cp,0,0,0]])
self.u = tuple([hstack((linear_acc, angular_rate))])
#print self.u
(self.x, self.P) = self.ukf.filter_update(state, state_cov, hstack((anchor_dis, euler_angle)))
return (self.x, self.P)
def transition_function(self, state):
return odeint(self.state_equation, state, [0, self.delt_time], self.u)[1]
def observation_function(self, state):
return hstack((linalg.norm(state[0:3] - self.anchor_pos), state[3:6]))
def __init__(self, position, velocity):
"""
Args:
position: Where we are. (X, Y)
velocity: How fast we're going. (X, Y) """
# The transmitter x and y position should technically never change, so it's
# not part of our state.
self.__transmitter_positions = []
# We need an x and y position, an x and y velocity. (LOBs go after that.)
self.__state = np.array([position[self._X], position[self._Y],
velocity[self._X], velocity[self._Y]])
logger.debug("Initial state: %s" % (self.state))
self.__state_size = self.__state.shape[0]
# Default observations.
self.__observations = np.copy(self.__state)
# Build a covariance matrix. We will use our initial uncertainty constants,
# and assume that nothing in the state is correlated with anything else.
self.__state_covariances = np.zeros((self.__state_size, self.__state_size))
# Calculate our diagonal fill, which will repeat, since the state repeats
# for each drone.
diagonal_fill = [self.DRONE_POSITION_UNCERTAINTY,
self.DRONE_POSITION_UNCERTAINTY,
self.DRONE_VELOCITY_UNCERTAINTY,
self.DRONE_VELOCITY_UNCERTAINTY]
diagonal_indices = np.diag_indices(self.__state_size)
self.__state_covariances[diagonal_indices] = diagonal_fill
logger.debug("Initializing state covariances: %s" % \
(self.__state_covariances))
# Since our initial state is directly what's measured from our sensors, it
# seems logical that the initial observation covariances would be about what
# our initial state covariances are.
self.__observation_covariances = np.copy(self.__state_covariances)
# Initialize the Kalman filter.
self.__kalman = \
AdditiveUnscentedKalmanFilter( \
transition_functions=self.__transition_function,
observation_functions=self.__observation_function)
class IMULocation:
def __init__(self):
self.N = 10
self.M = 3
self.x = zeros((1, self.N))[0]
self.R = eye(3) * 0.02
self.P = Q
self.time = -1
self.ukfinit()
self.state_equation = copy.deepcopy(state_equation)
def ukfinit(self):
self.ukf = AdditiveUnscentedKalmanFilter(
n_dim_obs=self.M,
n_dim_state=self.N,
transition_functions=self.transition_function,
observation_functions=self.observation_function,
transition_covariance=Q,
observation_covariance=self.R,
initial_state_mean=self.x,
initial_state_covariance=Q)
def locate(self, state, state_cov, delt_time, euler_angle, linear_acc,
angular_rate):
self.delt_time = delt_time
self.u = tuple((hstack((linear_acc, angular_rate))))
(self.x, self.P) = self.ukf.filter_update(state, state_cov,
euler_angle)
print linalg.norm(euler_angle - self.x[3:6])
return (self.x, self.P)
def transition_function(self, state):
return odeint(self.state_equation, state, [0, self.delt_time],
tuple([self.u]))[1]
def observation_function(self, state):
return hstack((state[3:6]))
# init
stateMean = np.zeros((n + 1, 2))
stateCov = np.zeros((n + 1, 2, 2))
stateMean[0] = param['x0']
stateCov[0] = param['p0']
for i in range(n):
(stateMeanPred, stateCovPred) = filter_predict(f, jf, Q, stateMean[i],
stateCov[i])
(stateMean[i + 1],
stateCov[i + 1]) = filter_update(h, jh, np.array([param['r']]),
stateMeanPred, stateCovPred,
observations[i])
plt.plot(stateMean[:, 0])
plt.plot(states[:, 0])
plt.plot(observations, '.')
## Unscented Kalman Filter
# In[107]:
ukf = AdditiveUnscentedKalmanFilter(f, h, Q, param['r'], param['x0'],
param['p0'])
stateMean = ukf.filter(observations)[0]
plt.plot(stateMean[:, 0])
plt.plot(states[:, 0])
plt.plot(observations, '.')
class FusionUKF:
n_state_dims = 11
n_lidar_obs_dims = 3
n_radar_obs_dims = 4
state_var_map = {
'x': 0,
'vx': 1,
'ax': 2,
'y': 3,
'vy': 4,
'ay': 5,
'z': 6,
'vz': 7,
'az': 8,
'yaw': 9,
'vyaw': 10
}
OK = 0
UNRELIABLE_OBSERVATION = 1
NOT_INITED = 2
RESETTED = 3
def __init__(self, object_radius):
self.initial_state_covariance = FusionUKF.create_initial_state_covariance(
)
self.radar_observation_function = FusionUKF.create_radar_observation_function(
)
self.lidar_observation_covariance = FusionUKF.create_lidar_observation_covariance(
)
self.radar_observation_covariance = FusionUKF.create_radar_observation_covariance(
object_radius)
self.object_radius = object_radius
# how much noisy observations to reject before resetting the filter
self.reject_max = 2
# radar position measurements are coarse at close distance
# discard radar observations closer than this
self.min_radar_radius = 0.
self.max_timejump = 1e9
self.reset()
def set_min_radar_radius(self, min_radius):
self.min_radar_radius = min_radius
def set_max_timejump(self, max_timejump):
self.max_timejump = max_timejump
@staticmethod
def create_initial_state_covariance():
# converges really fast, so don't tweak too carefully
return np.diag([
2., # x
0.8, # x'
0.8, # x''
2., # y
0.8, # y'
0.8, # y''
10., # z
10., # z'
10., # z''
10., # alpha
10. # alpha
])
@staticmethod
def create_transition_function(dt):
dt2 = 0.5 * dt**2
return lambda s: [
s[0] + s[1] * dt + s[2] * dt2,
s[1] + s[2] * dt,
s[2],
s[3] + s[4] * dt + s[5] * dt2,
s[4] + s[5] * dt,
s[5],
# let's don't track z velocity and acceleration, because it's unreliable
# s[6] + s[7] * dt + s[8] * dt2,
s[6],
s[7],
s[8],
s[9],
s[10]
]
@staticmethod
def lidar_observation_function(s):
m = FusionUKF.state_var_map
return [s[m['x']], s[m['y']], s[m['z']], s[m['yaw']]]
@staticmethod
def create_radar_observation_function():
m = FusionUKF.state_var_map
return lambda s: [s[m['x']], s[m['vx']], s[m['y']], s[m['vy']]]
@staticmethod
def create_transition_covariance():
return np.diag([
1e-2, # x
1e-2, # vx
1e-2, # ax
1e-2, # y
1e-2, # vy
1e-2, # ay
1e-5, # z
1e-5, # vz
1e-5, # az
1e-1, # yaw
1e-3 # vyaw
])
@staticmethod
def create_lidar_observation_covariance():
return np.diag([
0.05, # x
0.05, # y
0.05, # z
0.001 # yaw
])
@staticmethod
def create_radar_observation_covariance(object_radius):
cov_x, cov_vx, cov_y, cov_vy = FusionUKF.calc_radar_covariances(
object_radius)
print cov_x, cov_vx, cov_y, cov_vy
return np.diag([cov_x, cov_vx, cov_y, cov_vy])
@staticmethod
def calc_radar_covariances(object_radius):
# object_radius = radius of the circumscribing circle of the tracked object
sigma_xy = object_radius / 1.
cov_xy = sigma_xy**2
# jitter derived from http://www.araa.asn.au/acra/acra2015/papers/pap167.pdf
jitter_v = 0.12
sigma_v = jitter_v / 10.
cov_v = sigma_v**2
return cov_xy, cov_v, cov_xy, cov_v
def obs_as_kf_obs(self, obs):
if isinstance(obs, RadarObservation):
return [obs.x, obs.vx, obs.y, obs.vy]
elif isinstance(obs, LidarObservation):
return [obs.x, obs.y, obs.z, obs.yaw]
elif isinstance(obs, EmptyObservation):
return None
else:
raise ValueError
@staticmethod
def obs_as_state(obs):
if isinstance(obs, RadarObservation):
z = -0.8 # radar doesn't measure Z-coord, so we need an initial estimation of Z.
return [obs.x, 0., 0., obs.y, 0., 0., z, 0., 0., 0., 0.]
elif isinstance(obs, LidarObservation):
return [obs.x, 0., 0., obs.y, 0., 0., obs.z, 0., 0., obs.yaw, 0.]
else:
raise ValueError
def looks_like_noise(self, obs):
if not self.initialized or isinstance(obs, EmptyObservation):
return
state_mean = self.last_state_mean
state_deviation = np.sqrt(np.diag(self.last_state_covar))
mul = 10.
deviation_threshold = mul * state_deviation
oas = np.array(self.obs_as_state(obs))
oas_deviation = np.abs(oas - state_mean)
reject_mask = oas_deviation > deviation_threshold
m = self.state_var_map
bad_x = reject_mask[m['x']]
bad_y = reject_mask[m['y']]
bad_z = reject_mask[m['z']]
who_bad = ''
if bad_x:
who_bad += 'x'
elif bad_y:
who_bad += 'y'
elif bad_z:
who_bad += 'z'
return who_bad if who_bad != '' else None
def check_covar(self):
if not self.initialized:
return
state_deviation = np.sqrt(np.diag(self.last_state_covar))
mul = 1.5
deviation_threshold = mul * np.diag(self.initial_state_covariance)
mask = state_deviation > deviation_threshold
nz = np.nonzero(mask)[0]
var_name = [
'x', 'vx', 'ax', 'y', 'vy', 'ay', 'z', 'vz', 'az', 'yaw', 'vyaw'
]
for i in nz:
print 'Fusion: {} exceeded deviation ({}>{})'.format(
var_name[i], state_deviation[i], deviation_threshold[i])
#print mask
return mask.sum() == 0
# m = self.state_var_map
# bad_x = mask[m['x']]
# bad_y = mask[m['y']]
# bad_z = mask[m['z']]
# if bad_x:
# return 'x', state_deviation[m['x']]
# elif bad_y:
# return 'y', state_deviation[m['y']]
# elif bad_z:
# return 'z', state_deviation[m['z']]
#return None
def reset(self):
self.kf = AdditiveUnscentedKalmanFilter(n_dim_state=self.n_state_dims)
self.obs_yaw_lin = YawLinear()
self.last_state_mean = None
self.last_state_covar = None
self.last_obs = None
self.initialized = False
self.reject_count = 0
def filter(self, obs):
if not self.initialized and isinstance(obs, EmptyObservation):
return self.NOT_INITED
if isinstance(
obs,
RadarObservation) and obs.radius() < self.min_radar_radius:
#print 'rejecting radar observation because its too close'
return self.UNRELIABLE_OBSERVATION
#print obs
# we need initial estimation to feed it to filter_update()
if not self.initialized:
if not self.last_obs:
# need two observations to get a filtered state
self.last_obs = obs
return self.NOT_INITED
last_state_mean = self.obs_as_state(self.last_obs)
last_state_covar = self.initial_state_covariance
else:
last_state_mean = self.last_state_mean
last_state_covar = self.last_state_covar
dt = obs.timestamp - self.last_obs.timestamp
if np.abs(dt) > self.max_timejump:
print 'Fusion: {}s time jump detected, allowed is {}s. Resetting.'.format(
dt, self.max_timejump)
self.reset()
return self.RESETTED
who_is_bad = self.looks_like_noise(obs)
if who_is_bad is not None:
print 'Fusion: rejected noisy observation (bad {}): {}'.format(
who_is_bad, obs)
self.reject_count += 1
if self.reject_count > self.reject_max:
print 'Fusion: resetting filter because too much noise'
self.reset()
return self.RESETTED
return self.UNRELIABLE_OBSERVATION
if isinstance(obs, LidarObservation):
transition_function = self.create_transition_function(dt)
transition_covariance = self.create_transition_covariance()
observation_function = self.lidar_observation_function
observation_covariance = self.lidar_observation_covariance
elif isinstance(obs, RadarObservation):
transition_function = self.create_transition_function(dt)
transition_covariance = self.create_transition_covariance()
observation_function = self.radar_observation_function
observation_covariance = self.radar_observation_covariance
else: # EmptyObservation
transition_function = self.create_transition_function(dt)
transition_covariance = self.create_transition_covariance()
observation_function = None
observation_covariance = None
try:
self.last_state_mean, self.last_state_covar =\
self.kf.filter_update(
last_state_mean,
last_state_covar,
self.obs_as_kf_obs(obs),
transition_function,
transition_covariance,
observation_function,
observation_covariance)
except:
print 'Fusion: ====== WARNING! ====== filter_update() failed!'
#bad_covar = self.check_covar()
#if bad_covar is not None:
#print 'Fusion: ({}) resetting filter because too high deviation in {}={}'.format(obs.timestamp, bad_covar[0], bad_covar[1])
if self.check_covar() == False:
print 'Fusion: resetting filter because too high deviation'
self.reset()
return self.RESETTED
self.last_obs = obs
self.initialized = True
return self.OK
def E_step(self):
# print(self.g_coords.shape)
# print(self.w_cov,self.v_cov)
ukf = AdditiveUnscentedKalmanFilter(self.f_estimator, self.g_estimator, transition_covariance=self.w_cov, observation_covariance=self.v_cov)
self.hidden_sequence = ukf.smooth(self.output_sequence)[0]
def runFilter(observations, params, dname, filterType):
"""
Run Kalman Filter (UKF/KF) and return smoothed means/covariances
observations : nsample x T
params : {'dname'... contains all the necessary parameters for KF/UKF}
filterType : 'KF' or 'UKF'
"""
s1 = set(params[dname].keys())
s2 = set(['trans_fxn','obs_fxn','trans_cov','obs_cov','init_mu','init_cov',
'trans_mult','obs_mult','trans_drift','obs_drift','baseline'])
for k in s2:
assert k in s1,k+' not found in params'
#assert s1.issubset(s2) and s1.issuperset(s2),'Missing in params: '+', '.join(list(s2.difference(s1)))
assert filterType=='KF' or filterType=='UKF','Expecting KF/UKF'
model,mean,var = None,None,None
X = observations.squeeze()
#assert len(X.shape)==2,'observations must be nsamples x T'
if filterType=='KF':
def setupArr(arr):
if type(arr) is np.ndarray:
return arr
else:
return np.array([arr])
model=KalmanFilter(
transition_matrices = setupArr(params[dname]['trans_mult']), #multiplier for z_t-1
observation_matrices = setupArr(params[dname]['obs_mult']).T, #multiplier for z_t
transition_covariance = setupArr(params[dname]['trans_cov_full']), #transition cov
observation_covariance= setupArr(params[dname]['obs_cov_full']), #obs cov
transition_offsets = setupArr(params[dname]['trans_drift']),#additive const. in trans
observation_offsets = setupArr(params[dname]['obs_drift']), #additive const. in obs
initial_state_mean = setupArr(params[dname]['init_mu']),
initial_state_covariance = setupArr(params[dname]['init_cov_full']))
else:
#In this case, the transition and emission function may have other parameters
#Create wrapper functions that are instantiated w/ the true parameters
#and pass them to the UKF
def trans_fxn(z):
return params[dname]['trans_fxn'](z, fxn_params = params[dname]['params'])
def obs_fxn(z):
return params[dname]['obs_fxn'](z, fxn_params = params[dname]['params'])
model=AdditiveUnscentedKalmanFilter(
transition_functions = trans_fxn, #params[dname]['trans_fxn'],
observation_functions = obs_fxn, #params[dname]['obs_fxn'],
transition_covariance = np.array([params[dname]['trans_cov']]), #transition cov
observation_covariance= np.array([params[dname]['obs_cov']]), #obs cov
initial_state_mean = np.array([params[dname]['init_mu']]),
initial_state_covariance = np.array(params[dname]['init_cov']))
#Run smoothing algorithm with model
dim_stoc = params[dname]['dim_stoc']
if dim_stoc>1:
mus = np.zeros((X.shape[0],X.shape[1],dim_stoc))
cov = np.zeros((X.shape[0],X.shape[1],dim_stoc))
else:
mus = np.zeros(X.shape)
cov = np.zeros(X.shape)
ll = 0
for n in range(X.shape[0]):
(smoothed_state_means, smoothed_state_covariances) = model.smooth(X[n,:])
if dim_stoc>1:
mus[n,:] = smoothed_state_means
cov[n,:] = np.concatenate([np.diag(k)[None,:] for k in smoothed_state_covariances],axis=0)
else:
mus[n,:] = smoothed_state_means.ravel()
cov[n,:] = smoothed_state_covariances.ravel()
if filterType=='KF':
ll += model.loglikelihood(X[n,:])
return mus,cov,ll
def main(argv):
config = read_parser(argv, Inputs, InputsOpt_Defaults)
if config['mode'] == 'normal':
print('nothing')
n = 500
x_pure = np.arange(n) * 0.1
x = x_pure + np.random.normal(scale=0.25, size=n)
y = np.cos(x)
# y = 2*x+ 6
kf = KalmanFilter(initial_state_mean=0, n_dim_obs=1, random_state=1)
# measurements = [[x[i], y[i]] for i in range(int(n))]
measurements = [[y[i]] for i in range(int(n / 2))]
# measurements_full = [[y[i]] for i in range(int(n))]
kf.em(measurements)
# print(kf.initial_state_mean)
# print(kf.initial_state_covariance)
# print(type(kf.initial_state_mean))
# print(type(kf.initial_state_covariance))
# exit()
z = kf.filter(measurements)
means = z[0]
covs = z[1]
it_mean = means[len(means) - 1]
it_cov = covs[len(covs) - 1]
h = list(means)
r = h
# kf2 = KalmanFilter(initial_state_mean=it_mean, initial_state_covariance=it_cov, n_dim_obs=1)
# kf2.em(measurements)
f = list(
kf.sample(int(n / 2), initial_state=it_mean, random_state=1)[0])
# f = h + list(np.array(f) + it_mean)
f = h + f
for i in range(int(n / 2)):
it_z = kf.filter_update(filtered_state_mean=it_mean,
filtered_state_covariance=it_cov,
observation=[y[i + int(n / 2)]])
it_mean = it_z[0]
it_cov = it_z[1]
h.append(it_mean)
plt.plot(x_pure, h, 'r')
plt.plot(x_pure, y)
plt.plot(x_pure, f, 'g')
plt.show()
elif config['mode'] == 'ukalman':
print('Ukalman')
n = 1000
x_pure = np.arange(n) * 0.1
x = x_pure + np.random.normal(scale=0.5, size=n)
y = np.cos(x)
# y = 2*x
kf = AdditiveUnscentedKalmanFilter(n_dim_obs=1)
measurements = [[y[i]] for i in range(int(n / 2))]
kf.initial_state_mean = np.array([0.])
kf.initial_state_covariance = np.array([[0.1]])
it_mean = kf.initial_state_mean
it_cov = kf.initial_state_covariance
print(kf.initial_state_mean)
print(kf.initial_state_covariance)
# exit()
h = []
for element in measurements:
it_z = kf.filter_update(filtered_state_mean=it_mean,
filtered_state_covariance=it_cov,
observation=element)
it_mean = it_z[0]
it_cov = it_z[1]
h.append(it_mean)
f = list(kf.sample(int(n / 2), initial_state=it_mean)[0])
# f = h + list(np.array(f) + it_mean)
f = h + f
plt.plot(x_pure, y)
plt.plot(x_pure, f, 'g')
plt.show()
return
from __future__ import division
from pykalman import AdditiveUnscentedKalmanFilter
import numpy as np
import numpy.linalg as la
def AFK(y):
#Inputs: GPS coordinates; nx2 vector
#Outputs: smoothed states; nx2 vector
def TransObsFunc(state): #Same functions for simplified filter
return state
alpha = 0.8 #Tuning parameter for smoothness
akf = AdditiveUnscentedKalmanFilter(
TransObsFunc, TransObsFunc,
np.eye(2)*alpha, np.eye(2),
[y[0,0], y[0,1]], np.eye(2))
return akf.filter(y)[0]
n = int(len(observations))
# init
stateMean = np.zeros((n+1, 2))
stateCov = np.zeros((n+1, 2, 2))
stateMean[0] = param['x0']
stateCov[0] = param['p0']
for i in range(n):
(stateMeanPred, stateCovPred) = filter_predict(f, jf, Q, stateMean[i], stateCov[i])
(stateMean[i+1], stateCov[i+1]) = filter_update(h, jh, np.array([param['r']]), stateMeanPred, stateCovPred, observations[i])
plt.plot(stateMean[:,0])
plt.plot(states[:,0])
plt.plot(observations, '.')
## Unscented Kalman Filter
# In[107]:
ukf = AdditiveUnscentedKalmanFilter(f, h, Q, param['r'], param['x0'], param['p0'])
stateMean = ukf.filter(observations)[0]
plt.plot(stateMean[:,0])
plt.plot(states[:,0])
plt.plot(observations, '.')
random_state = np.random.RandomState(0)
observation_covariance = np.eye(2) + random_state.randn(2, 2) * 0.1
initial_state_mean = [0, 0]
initial_state_covariance = [[1, 0.1], [0.1, 1]]
# sample from model
ukf = UnscentedKalmanFilter(transition_function,
observation_function,
transition_covariance,
observation_covariance,
initial_state_mean,
initial_state_covariance,
random_state=random_state)
akf = AdditiveUnscentedKalmanFilter(additive_transition_function,
additive_observation_function,
transition_covariance,
observation_covariance, initial_state_mean,
initial_state_covariance)
states, observations = ukf.sample(50, initial_state_mean)
# estimate state with filtering
ukf_state_estimates = ukf.filter(observations)[0]
akf_state_estimates = akf.filter(observations)[0]
# draw estimates
pl.figure()
lines_true = pl.plot(states, color='b')
lines_ukf = pl.plot(ukf_state_estimates, color='r')
lines_akf = pl.plot(akf_state_estimates, color='g')
pl.legend((lines_true[0], lines_ukf[0], lines_akf[0]),
('true', 'UKF', 'AddUKF'),
transition_covariance = np.eye(2)
random_state = np.random.RandomState(0)
observation_covariance = np.eye(2) + random_state.randn(2, 2) * 0.1
initial_state_mean = [0, 0]
initial_state_covariance = [[1, 0.1], [-0.1, 1]]
# sample from model
ukf = UnscentedKalmanFilter(
transition_function, observation_function,
transition_covariance, observation_covariance,
initial_state_mean, initial_state_covariance,
random_state=random_state
)
akf = AdditiveUnscentedKalmanFilter(
additive_transition_function, additive_observation_function,
transition_covariance, observation_covariance,
initial_state_mean, initial_state_covariance
)
states, observations = ukf.sample(50, initial_state_mean)
# estimate state with filtering
ukf_state_estimates = ukf.filter(observations)[0]
akf_state_estimates = akf.filter(observations)[0]
# draw estimates
pl.figure()
lines_true = pl.plot(states, color='b')
lines_ukf = pl.plot(ukf_state_estimates, color='r')
lines_akf = pl.plot(akf_state_estimates, color='g')
pl.legend((lines_true[0], lines_ukf[0], lines_akf[0]),
('true', 'UKF', 'AddUKF'),
from pykalman import AdditiveUnscentedKalmanFilter
'Initialize Parameters'
def transition_function(X):
...
def observation_function(X):
...
transition_covariance = np.eye(2)
observation_covariance = np.eye(2) + something
initial_state_mean = [0, 0]
initial_state_covariance = [[1, 0.1], [0.1, 1]]
akf = AdditiveUnscentedKalmanFilter(transition_function,observation_function, \
transition_covariance,observation_covariance,initial_state_mean, \
initial_state_covariance)
akf_state_estimates = akf.filter(timesteps, states)
pl.figure()
lines_true = pl.plot(states, color='b')
lines_akf = pl.plot(akf_state_estimates, color='g', ls='-.')
pl.show()
class Kalman:
""" Handles a Kalman filter for computing drone and transmitter locations. """
# Our initial uncertainty of our drone positions. GPS is not terribly
# innacurate, so there isn't a ton of uncertainty here.
DRONE_POSITION_UNCERTAINTY = 0.001
# Out initial uncertainly of our drone velocities. This is reasonably accurate
# as well.
DRONE_VELOCITY_UNCERTAINTY = 0.05
# Our initial uncertainty on our LOBs.
# TODO(danielp): I have zero idea of what this should be. Figure that out.
LOB_UNCERTAINTY = np.radians(5)
# The indices of various elements in the state.
_POS_X = 0
_POS_Y = 1
_VEL_X = 2
_VEL_Y = 3
# The index of the first LOB.
_LOB = 4
# Indices of x and y in coordinate tuples.
_X = 0
_Y = 1
def __init__(self, position, velocity):
"""
Args:
position: Where we are. (X, Y)
velocity: How fast we're going. (X, Y) """
# The transmitter x and y position should technically never change, so it's
# not part of our state.
self.__transmitter_positions = []
# We need an x and y position, an x and y velocity. (LOBs go after that.)
self.__state = np.array([position[self._X], position[self._Y],
velocity[self._X], velocity[self._Y]])
logger.debug("Initial state: %s" % (self.state))
self.__state_size = self.__state.shape[0]
# Default observations.
self.__observations = np.copy(self.__state)
# Build a covariance matrix. We will use our initial uncertainty constants,
# and assume that nothing in the state is correlated with anything else.
self.__state_covariances = np.zeros((self.__state_size, self.__state_size))
# Calculate our diagonal fill, which will repeat, since the state repeats
# for each drone.
diagonal_fill = [self.DRONE_POSITION_UNCERTAINTY,
self.DRONE_POSITION_UNCERTAINTY,
self.DRONE_VELOCITY_UNCERTAINTY,
self.DRONE_VELOCITY_UNCERTAINTY]
diagonal_indices = np.diag_indices(self.__state_size)
self.__state_covariances[diagonal_indices] = diagonal_fill
logger.debug("Initializing state covariances: %s" % \
(self.__state_covariances))
# Since our initial state is directly what's measured from our sensors, it
# seems logical that the initial observation covariances would be about what
# our initial state covariances are.
self.__observation_covariances = np.copy(self.__state_covariances)
# Initialize the Kalman filter.
self.__kalman = \
AdditiveUnscentedKalmanFilter( \
transition_functions=self.__transition_function,
observation_functions=self.__observation_function)
def __transition_function(self, current_state):
""" Transition function. Tells us how to get the next state from the current
state.
Args:
current_state: The current state.
Returns:
The next state. """
new_state = np.copy(current_state)
# Updating the position is easy, we just add the velocity.
new_state[self._POS_X] += current_state[self._VEL_X]
new_state[self._POS_Y] += current_state[self._VEL_Y]
for i in range(0, len(self.__transmitter_positions)):
position = self.__transmitter_positions[i]
# We can calculate our LOB too, based on our position.
new_state[self._LOB + i] = np.arctan2(position[self._X] - current_state[self._POS_X],
position[self._Y] - current_state[self._POS_Y])
# We use the velocity vector to gauge the drone's heading, and correct the
# LOB accordingly.
heading_correction = np.arctan2(current_state[self._VEL_X],
current_state[self._VEL_Y])
new_state[self._LOB + i] -= heading_correction
logger.debug("New state prediction: %s" % (new_state))
return new_state
def __observation_function(self, current_state):
""" Observation function. Tells us what our sensor readings should look like
if our state estimate were correct and our sensors were perfect.
Returns:
The sensor readings we would expect. """
# We basically measure our state directly, so this isn't all that
# interesting.
return current_state
def set_observations(self, position, velocity, *args):
""" Sets what our observations are.
Args:
position: Where the GPS thinks we are. (X, Y)
velocity: How fast we think we're going. (X, Y)
Additional arguments are the LOBs on any transmitters we are tracking. """
observations = [position[self._X], position[self._Y], velocity[self._X],
velocity[self._Y]]
expecting_lobs = self.__observations.shape[0] - len(observations)
if len(args) != expecting_lobs:
raise ValueError("Expected %d LOBs, got %d." % (expecting_lobs,
len(args)))
observations.extend(args)
self.__observations = np.array(observations)
logger.debug("Setting new observations: %s" % (self.__observations))
def update(self):
""" Updates the filter for one iteration. """
logger.info("Updating kalman filter.")
output = self.__kalman.filter_update(self.__state, self.__state_covariances,
observation=self.__observations,
observation_covariance=
self.__observation_covariances)
self.__state, self.__state_covariances = output
logger.debug("New state: %s, New state covariance: %s" % \
(self.__state, self.__state_covariances))
def state(self):
"""
Returns:
The current state. """
return self.__state
def state_covariances(self):
""" Returns: The current state covariances. """
return self.__state_covariances
def add_transmitter(self, lob, location):
""" Adds a new transmitter for us to track.
Args:
lob: Our LOB to the transmitter.
location: Where we think that the transmitter is located. """
self.__transmitter_positions.append(location)
# Add the LOB to the state.
self.__state = np.append(self.__state, lob)
new_state_size = self.__state.shape[0]
# Make sure the observations vector is the proper size.
self.__observations = np.append(self.__observations, lob)
# Now resize everything else to make it fit. Start by adding an extra row
# and column of zeros to the state covariance matrix.
new_state_cov = _expand_matrix(self.__state_covariances)
# Add in a new default value for the variance of the new LOB.
new_state_cov[new_state_size - 1, new_state_size - 1] = \
self.LOB_UNCERTAINTY
self.__state_covariances = new_state_cov
self.__state_size = new_state_size
logger.debug("New state size: %d" % (self.__state_size))
logger.debug("New state covariances: %s" % (self.__state_covariances))
# Do the same thing for the observation covariances.
new_observation_cov = _expand_matrix(self.__observation_covariances)
new_observation_cov[new_state_size - 1, new_state_size - 1] = \
self.LOB_UNCERTAINTY
self.__observation_covariances = new_observation_cov
logger.debug("New observation covariances: %s" % \
(self.__observation_covariances))
def position_error_ellipse(self, stddevs):
""" Gets a confidence error ellipse for our drone position
measurement.
Args:
stddevs: How many standard deviations we want the ellipse to encompass.
Returns:
The width, the height, and the angle to the x axis of the ellipse,
(radians) in a tuple in that order. """
# Take the subset of the covariance matrix that pertains to our position.
position_covariance = self.__state_covariances[:2, :2]
# Calculate its eigenvalues, and sort them.
eigenvalues, eigenvectors = np.linalg.eigh(position_covariance)
order = eigenvalues.argsort()[::-1]
eigenvalues = eigenvalues[order]
eigenvectors = eigenvectors[:, order]
# Ellipse parameters.
angle = np.arctan2(*eigenvectors[:, 0][::-1])
width, height = 2 * stddevs * np.sqrt(eigenvalues)
logger.debug("Position error: width: %f, height: %f, angle: %f" % \
(width, height, angle))
return (width, height, angle)
def lob_confidence_intervals(self, stddevs):
""" Gets confidence intervals for our LOBs.
Args:
stddevs: How many standard deviations we want the interval to encompass.
Returns:
A list of the margin of errors for each LOB measurement. """
# Get the indices of the LOB covariances.
indices = np.diag_indices(self.__state_size)
# The first four are the position and velocity variances.
indices = (indices[0][self._LOB:], indices[1][self._LOB:])
if not indices[0]:
# We're not tracking any transmitters:
return []
# Get the variances.
variances = self.__state_covariances[indices]
omega = np.sqrt(variances)
margins_of_error = stddevs * omega
logger.debug("Margins of error for LOBs: %s" % (margins_of_error))
return margins_of_error
def transmitter_error_region(self, stddevs):
""" Calculates error ellipses for all the
transmitter positions. It does this by looking at the worst-case scenario for
both the error on the drone position and the error on the LOB.
Returns:
A list of data for each transmitter. Each item in the list is itself
a list of 8 points that define the error region for that transmitter. This
region is roughly fan shaped, and the points are in clockwise order,
starting from the bottom left corner. """
# Basically, we're taking every point on the ellipse and projecting it
# through the LOB to reach a new error region, which is sort of fan-shaped.
# Start by finding both the error regions for our position and lobs.
ellipse_width, ellipse_height, ellipse_angle = \
self.position_error_ellipse(stddevs)
# Turn the error ellipse into a set of points.
center = (self.__state[self._POS_X], self.__state[self._POS_Y])
spread_x = ellipse_width / 2.0
spread_y = ellipse_height / 2.0
low_x = (center[self._X] - spread_x * np.cos(ellipse_angle),
center[self._Y] - spread_x * np.sin(ellipse_angle))
high_x = (center[self._X] + spread_x * np.cos(ellipse_angle),
center[self._Y] + spread_x * np.sin(ellipse_angle))
low_y = (center[self._X] - spread_y * np.sin(ellipse_angle),
center[self._Y] - spread_y * np.cos(ellipse_angle))
high_y = (center[self._X] + spread_y * np.sin(ellipse_angle),
center[self._Y] + spread_y * np.cos(ellipse_angle))
lob_errors = self.lob_confidence_intervals(stddevs)
lobs = self.__state[self._LOB:]
output = []
for i in range(0, len(lobs)):
lob = lobs[i]
lob_error = lob_errors[i]
transmitter_position = self.__transmitter_positions[i]
lob_low = lob - lob_error
lob_high = lob + lob_error
# Figure out what the transmitter position would be for each of these
# scenarios.
radius = np.sqrt((transmitter_position[self._X] - center[self._X]) ** 2 + \
(transmitter_position[self._Y] - center[self._Y]) ** 2)
transmitter_low = (radius * np.cos(lob_low), radius * np.sin(lob_low))
transmitter_high = (radius * np.cos(lob_high), radius * np.sin(lob_high))
# Calculate points on the error ellipse when centered about all three of
# these positions.
recenter_vector_low = (transmitter_low[self._X] - center[self._X],
transmitter_low[self._Y] - center[self._Y])
recenter_vector_mean = (transmitter_position[self._X] - center[self._X],
transmitter_position[self._Y] - center[self._Y])
recenter_vector_high = (transmitter_high[self._X] - center[self._X],
transmitter_high[self._Y] - center[self._Y])
bottom_left = map(add, low_y, recenter_vector_low)
left_middle = map(add, low_x, recenter_vector_low)
top_left = map(add, high_y, recenter_vector_low)
top_middle = map(add, high_y, recenter_vector_mean)
top_right = map(add, high_y, recenter_vector_high)
right_middle = map(add, high_x, recenter_vector_high)
bottom_right = map(add, low_y, recenter_vector_high)
bottom_middle = map(add, low_y, recenter_vector_mean)
# These points define our error region.
error_region = [bottom_left, left_middle, top_left, top_middle, top_right,
right_middle, bottom_right, bottom_middle]
logger.debug("Error region for transmitter at %s: %s" % \
(transmitter_position, error_region))
output.append(error_region)
return output
