def main():
# sample from model
kf = UnscentedKalmanFilter(
transition_function, observation_function,
observation_covariance,
)
# states, observations = kf.sample(50, initial_state_mean)
observations = []
import os
dir = os.path.dirname(os.path.realpath(__file__))
with open(dir+'/data.csv', 'rb') as csvfile:
para = csv.reader(csvfile, delimiter=',', quotechar='|')
for row in para:
observations.append(row)
states = []
cov = []
for i in range(len(observations)):
states.append([0, 0])
cov.append([[0, 0], [0, 0]])
states[0] = initial_state_mean
print states[0]
cov[0] = initial_state_covariance
for i in range(len(observations)-1):
states[i], cov[i] = kf.filter(observations[i+1])
states[i+1], cov[i+1] = kf.filter_update(states[i], cov[i])
# estimate state with filtering and smoothing
# filtered_state_estimates = kf.filter(observations)[0]
# smoothed_state_estimates = kf.smooth(observations)[0]
# draw estimates
pl.figure()
lines_true = pl.plot(states, color='b')
# lines_filt = pl.plot(filtered_state_estimates, color='r', ls='-')
# lines_smooth = pl.plot(smoothed_state_estimates, color='g', ls='-.')
#pl.legend((lines_true[0], lines_filt[0], lines_smooth[0]),
# ('true', 'filt', 'smooth'),
# loc='lower left'
#)
pl.show()
class UnscentedKalmanFilterControl:
def __init__(self, dimState, delay, arm, knoiseU, controller):
'''
Initializes parameters to uses the function implemented below
inputs: -dimState: dimension of the state, here the state correspond to the muscular activation vector U, int
-dimObs: dimension of the observation, here the observation is the position of the arm given by the model, int
-delay: the delay with which we give the observation to the filter, int
-arm, armModel, class object
-rs, ReadSetupFile, class object
'''
self.name = "UnscentedKalmanFilter"
self.knoiseU = knoiseU
self.dimState = dimState
self.delay = delay
self.arm = arm
self.controller = controller
#initialization of some parameters for the filter
transition_covariance = np.eye(self.dimState) * 0.01
initial_state_mean = np.zeros(self.dimState)
observation_covariance = 1000 * np.eye(self.dimState)
initial_state_covariance = np.eye(self.dimState)
self.nextCovariance = np.eye(self.dimState) * 0.0001
self.ukf = UnscentedKalmanFilter(self.transitionFunctionUKF,
self.observationFunctionUKF,
transition_covariance,
observation_covariance,
initial_state_mean,
initial_state_covariance)
def initObsStore(self, state):
'''
Initialization of the observation storage
Input: -state: the state stored
'''
self.obsStore = []
for i in range(self.delay):
self.obsStore.append(state)
#print ("InitStore:", self.obsStore)
def storeObs(self, state):
'''
Stores the current state and returns the delayed state
Input: -state: the state to store
'''
for i in range(self.delay - 1):
self.obsStore[self.delay - i - 1] = self.obsStore[self.delay - i -
2]
self.obsStore[0] = state
#print ("After store:", self.obsStore)
#print ("retour:", self.obsStore[self.delay-1])
return self.obsStore[self.delay - 1]
def transitionFunctionUKF(self, state, transitionNoise=0):
'''
Transition function used by the filter, function of the state and the transition noise at time t and produces the state at time t+1
Inputs: -stateU: the state at time t, numpy array
-transitionNoise: transition noise at time t, numpy array
Output: -nextStateUNoise: the next State with noise added, numpy array
'''
#print("UKF trans state :", state)
U = self.controller.computeOutput(state)
nextState = self.arm.computeNextState(U, state)
return nextState + transitionNoise
def observationFunctionUKF(self, state, observationNoise=0):
'''
Observation function used by the filter, function of the state and the observation noise at time t and produces the observation at time t
Inputs: -stateU: the state at time t, numpy array
-observationNoise: the observation noise at time t, numpy array
Output: -nextObsNoise: observation at time t+1
'''
U = self.controller.computeOutput(state)
nextObs = self.arm.computeNextState(U, state)
nextObsNoise = nextObs + observationNoise
return nextObsNoise
def runUKF(self, state):
'''
Function used to compute the next state approximation with the filter
Inputs: -Unoisy: the state to feed the filter, here its the muscular activation vector U, numpy array of dimension (x, 1), here x = 6
-state: the state of the arm
Output: -stateApprox: the next state approximation, numpy array of dimension (x, 1), here x = 4
'''
#store the state of the arm to feed the filter with a delay on the observation
oldstate = self.storeObs(state)
#print("UKF old state :", oldstate)
#compute the nextState approximation ie here the next muscular activation
nextState, nextCovariance = self.ukf.filter_update(
state, self.nextCovariance, oldstate)
#compute the nextState i.e. the next position vector from the approximation of the next muscular activation vector given by the filter
return nextState
def debugStore(self):
state = np.array([1, 2, 3, 4])
self.initObsStore(state)
for i in range(5):
tmp = [rd.random() for x in range(4)]
self.storeObs(tmp)
class UnscentedKalmanFilterControl:
def __init__(self, dimState, delay, arm, knoiseU, controller):
'''
Initializes parameters to uses the function implemented below
inputs: -dimState: dimension of the state, here the state correspond to the muscular activation vector U, int
-dimObs: dimension of the observation, here the observation is the position of the arm given by the model, int
-delay: the delay with which we give the observation to the filter, int
-arm, armModel, class object
-rs, ReadSetupFile, class object
'''
self.name = "UnscentedKalmanFilter"
self.knoiseU = knoiseU
self.dimState = dimState
self.delay = delay
self.arm = arm
self.controller = controller
#initialization of some parameters for the filter
transition_covariance = np.eye(self.dimState)*0.01
initial_state_mean = np.zeros(self.dimState)
observation_covariance = 1000*np.eye(self.dimState)
initial_state_covariance = np.eye(self.dimState)
self.nextCovariance = np.eye(self.dimState)*0.0001
self.ukf = UnscentedKalmanFilter(self.transitionFunctionUKF, self.observationFunctionUKF,
transition_covariance, observation_covariance,
initial_state_mean, initial_state_covariance)
def initObsStore(self, state):
'''
Initialization of the observation storage
Input: -state: the state stored
'''
self.obsStore = []
for i in range(self.delay):
self.obsStore.append(state)
#print ("InitStore:", self.obsStore)
def storeObs(self, state):
'''
Stores the current state and returns the delayed state
Input: -state: the state to store
'''
for i in range (self.delay-1):
self.obsStore[self.delay-i-1]=self.obsStore[self.delay-i-2]
self.obsStore[0]=state
#print ("After store:", self.obsStore)
#print ("retour:", self.obsStore[self.delay-1])
return self.obsStore[self.delay-1]
def transitionFunctionUKF(self, state, transitionNoise = 0):
'''
Transition function used by the filter, function of the state and the transition noise at time t and produces the state at time t+1
Inputs: -stateU: the state at time t, numpy array
-transitionNoise: transition noise at time t, numpy array
Output: -nextStateUNoise: the next State with noise added, numpy array
'''
#print("UKF trans state :", state)
U = self.controller.computeOutput(state)
nextState = self.arm.computeNextState(U, state)
return nextState + transitionNoise
def observationFunctionUKF(self, state, observationNoise = 0):
'''
Observation function used by the filter, function of the state and the observation noise at time t and produces the observation at time t
Inputs: -stateU: the state at time t, numpy array
-observationNoise: the observation noise at time t, numpy array
Output: -nextObsNoise: observation at time t+1
'''
U = self.controller.computeOutput(state)
nextObs = self.arm.computeNextState(U, state)
nextObsNoise = nextObs + observationNoise
return nextObsNoise
def runUKF(self, state):
'''
Function used to compute the next state approximation with the filter
Inputs: -Unoisy: the state to feed the filter, here its the muscular activation vector U, numpy array of dimension (x, 1), here x = 6
-state: the state of the arm
Output: -stateApprox: the next state approximation, numpy array of dimension (x, 1), here x = 4
'''
#store the state of the arm to feed the filter with a delay on the observation
oldstate = self.storeObs(state)
#print("UKF old state :", oldstate)
#compute the nextState approximation ie here the next muscular activation
nextState, nextCovariance = self.ukf.filter_update(state, self.nextCovariance, oldstate)
#compute the nextState i.e. the next position vector from the approximation of the next muscular activation vector given by the filter
return nextState
def debugStore(self):
state = np.array([1,2,3,4])
self.initObsStore(state)
for i in range(5):
tmp = [rd.random() for x in range(4)]
self.storeObs(tmp)
class Fusion(object):
def __init__(self, x, v, w):
self.random_state = np.random.RandomState(0)
self.transition_covariance = np.array([[0.5, 0, 0, 0, 0],\
[0, 1, 0, 0, 0],\
[0, 0, 0.1, 0, 0],\
[0, 0, 0, 0.001, 0],\
[0, 0, 0, 0, 0.001],\
])
self.observation_covariance = np.array([[0.5, 0, 0, 0, 0],\
[0, 1, 0, 0, 0],\
[0, 0, 0.5, 0, 0],\
[0, 0, 0, 0.001, 0],\
[0, 0, 0, 0, 0.001],\
])
self.initial_state_mean = [x, 0.1, v, np.pi / 2, w]
# self.initial_state_mean = [0, 0, 20, 0, np.pi / 180]
self.transition_state = self.initial_state_mean
self.obs = self.initial_state_mean
self.pre_parabola_param = [0, 0, 1.75]
self.initial_state_covariance = np.array([[0.5, 0, 0, 0, 0],\
[0, 0.02, 0, 0, 0],\
[0, 0, 0.1, 0, 0],\
[0, 0, 0, 0.001, 0],\
[0, 0, 0, 0, 0.001],\
])
self.T = 0.5
self.estimate_state = [
self.initial_state_mean, self.initial_state_covariance
]
self.kf = UnscentedKalmanFilter(self.transition_function,
self.observation_function,
self.transition_covariance,
self.observation_covariance,
self.initial_state_mean,
self.initial_state_covariance,
random_state=self.random_state)
self.timestamp = time.time()
def transition_function(self, state, noise):
t = self.T
if state[4] == 0:
a = state[2] * np.cos(state[3]) + state[0]
b = state[2] * np.sin(state[3]) + state[1]
c = state[2]
d = np.pi / 2 #state[4] * t + state[3]
e = state[4]
else:
r = state[2] / state[4]
a = r * np.sin(state[4] * t + state[3]) - r * np.sin(
state[3]) # + state[0]
b = -r * np.cos(state[4] * t + state[3]) + r * np.cos(
state[3]) + state[1]
c = state[2]
d = np.pi / 2 #state[4] * t + state[3]
e = state[4] # + np.sin(debug_index / 10) * 0.01
# e = states_i + np.sin(debug_index / 10) * 10 * np.pi / 180
#adjust x from parabola param(down-right coordinate)
pixl_b = b * (IMAGE_HEI / 40)
parabola_y1 = self.pre_parabola_param[0] * (
-pixl_b)**2 + self.pre_parabola_param[1] * (
-pixl_b) + self.pre_parabola_param[2]
dalta_y = parabola_y1 - self.pre_parabola_param[2]
# a = dalta_y * (3.5 / lane_wid) + a
pixl_y = parabola_y1 * (3.5 / lane_wid)
a = pixl_y - a
b = b - state[1]
if self.obs[0] - a > 3.5 / 2:
a += 3.5
elif self.obs[0] - a < -3.5 / 2:
a -= 3.5
self.transition_state = np.array([a, b, c, d, e]) + noise
# print ("self.transition_state:" + str(self.transition_state))
return self.transition_state
def observation_function(self, state, noise):
# C = np.array([[-1, 0.5], [0.2, 0.1]])
# C = np.array([[1, 0], [0, 1]])
C = np.eye(5)
# return np.dot(C, state) + noise
return state + noise
def update(self, obs):
self.estimate_state = self.kf.filter_update(
self.estimate_state[0], self.estimate_state[1], obs,
self.transition_function, self.transition_covariance,
self.observation_function, self.observation_covariance)
print("estimate X:" + str(self.estimate_state[0]))
return self.estimate_state
def update_step(self, x, v, w, t, parabola_param):
print("update_step1:({},{},{},{},{})".format(x, v, w, t,
str(parabola_param)))
self.T = t
self.parabola_param = parabola_param
# obs = [x, y, v, theta, w]
# we didn't have obs_y so use predict obs_y(we didn't use y)
if w == 0 or self.transition_state[4] == 0:
y = self.transition_state[2] * np.sin(
self.transition_state[3]) + self.transition_state[1]
else:
r = self.transition_state[2] / self.transition_state[4]
y = -r * np.cos(
self.transition_state[4] * t + self.transition_state[3]
) + r * np.cos(self.transition_state[3]) + self.transition_state[1]
self.obs = [x, y, v, np.pi / 2, w]
self.timestamp = time.time()
print("update_step2:({})".format(str(self.obs)))
self.update(self.obs)
self.pre_parabola_param = parabola_param
print("update_step3:({})".format(str(self.estimate_state[0])))
return self.estimate_state[0][0]
def get_estimate(self):
return self.estimate_state[0][0]
def get_predict(self):
return self.transition_state[0]
