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]