class KalmanMovingAverage(object): """ Estimates the moving average of a price process via Kalman Filtering, using pykalman """ def __init__(self, asset, observation_covariance=1.0, initial_value=0, initial_state_covariance=1.0, transition_covariance=0.05, initial_window=30, maxlen=300, freq='1d'): self.asset = asset self.freq = freq self.initial_window = initial_window self.kf = KalmanFilter(transition_matrices=[1], observation_matrices=[1], initial_state_mean=initial_value, initial_state_covariance=initial_state_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance) self.state_means = pd.Series([self.kf.initial_state_mean], name=self.asset) self.state_covs = pd.Series([self.kf.initial_state_covariance], name=self.asset) def update(self, observations): for dt, observation in observations[self.asset].iterkv(): self._update(dt, observation) def _update(self, dt, observation): mu, cov = self.kf.filter_update(self.state_means.iloc[-1], self.state_covs.iloc[-1], observation) self.state_means[dt] = mu.flatten()[0] self.state_covs[dt] = cov.flatten()[0]
class AngleKalman: def __init__(self, start_angle, time_delta): angle_stdev = 0.05 A = numpy.asarray([[1, time_delta, 0.5 * time_delta * time_delta], [0, 1, time_delta], [0, 0, 1]]) H = numpy.asarray([[1, 0, 0]]) r = angle_stdev r2 = r * r R = numpy.asarray([ [r], ]) Q = numpy.asarray([[r2, 0, 0], [0, r2, 0], [0, 0, r2]]) initial_state = numpy.asarray([start_angle, 0, 0]) self._kf = KalmanFilter(transition_matrices=A, observation_matrices=H, transition_covariance=Q, observation_covariance=R, initial_state_mean=initial_state) self._means = initial_state self._covs = numpy.zeros((3, 3)) self._last_a = start_angle def step(self, a): diff = abs(a - self._last_a) if diff > math.pi: corr = math.copysign(2 * math.pi, a) self._means[0] += corr self._last_a += corr (self._means, self._covs) = \ self._kf.filter_update(self._means, self._covs, observation=numpy.asarray([a])) self._last_a = a def missing_step(self): (self._means, self._covs) = \ self._kf.filter_update(self._means, self._covs, observation=None) def get_means(self): (a, o, e) = self._means new_a = (a + math.pi) % (2 * math.pi) - math.pi return float(new_a), float(o), float(e)
class _KalmanFilter_(object): """Kalman Filter""" def __init__(self, xinit, yinit): self.Transition_Matrix = [[1, 0, 1, 0, 0.5, 0], [0, 1, 0, 1, 0, 0.5], [0, 0, 1, 0, 1, 0], [0, 0, 0, 1, 0, 1], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]] self.Observation_Matrix = [[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]] self.xinit = xinit self.yinit = yinit self.vxinit = 0.0 self.vyinit = 0.0 self.axinit = 0.0 self.ayinit = 0.0 self.initstate = [ self.xinit, self.yinit, self.vxinit, self.vyinit, self.axinit, self.ayinit ] self.initcovariance = 1.0e-3 * np.eye(6) self.transistionCov = 1.0e-4 * np.eye(6) self.observationCov = 1.0e-1 * np.eye(2) self.mean = None self.covariance = None self.kf = KalmanFilter(transition_matrices=self.Transition_Matrix, observation_matrices=self.Observation_Matrix, initial_state_mean=self.initstate, initial_state_covariance=self.initcovariance, transition_covariance=self.transistionCov, observation_covariance=self.observationCov) def _filterUpdate_(self, new_x_measurement, new_y_meansurement): if (self.mean is None) or (self.covariance is None): self.mean, self.covariance = self.kf.filter_update( self.initstate, self.initcovariance, [new_x_measurement, new_y_meansurement]) else: self.mean, self.covariance = self.kf.filter_update( self.mean, self.covariance, [new_x_measurement, new_y_meansurement]) return self.mean[0], self.mean[1]
class KalmanSensorNode(object): def __init__(self, node_id, state_dims, process, fusion_algorithm, transition_matrix, transition_covariance, measurement_covariance): self.node_id = node_id self.state_dims = state_dims self.measurement_covariance = measurement_covariance self.mean = np.zeros(shape=(state_dims, )) self.cov = np.identity(state_dims) self.kf = KalmanFilter(transition_matrices=transition_matrix, transition_covariance=transition_covariance, observation_covariance=measurement_covariance) self.process = process self.fusion_algorithm = fusion_algorithm self.local_estimates = [] self.fused_estimates = [] def estimate(self): measurement_noise = np.random.multivariate_normal( [0 for dim in range(self.state_dims)], self.measurement_covariance) noisy_measurement = np.squeeze(np.asarray( self.process.current_state)) + measurement_noise self.mean, self.cov = self.kf.filter_update(self.mean, self.cov, noisy_measurement) self.local_estimates.append((self.mean, self.cov)) return self.mean, self.cov def fuse_in(self, mean, cov): self.mean, self.cov = self.fusion_algorithm.fuse( self.mean, self.cov, mean, cov) self.fused_estimates.append((self.mean, self.cov)) return self.mean, self.cov
class OurFilter(): def __init__(self, newX, prevX, newCaptureTime, prevCaptureTime): _, Q = getTransitionMats(dtForInitCov) dt = (newCaptureTime - prevCaptureTime) if dt > nominalDt * 3: print("dt was huge", dt) #pdb.set_trace() initV = (newX - prevX) / dt self.prevStateMean = np.array([newX, initV]) self.prevStateCovariance = Q self.prevStateTime = newCaptureTime self.filter = KalmanFilter() def predict(self, predTime): A, Q = getTransitionMats(predTime - self.prevStateTime) return A @ self.prevStateMean, A @ self.prevStateCovariance @ A.T + Q def update(self, measX, measTime): dt = measTime - self.prevStateTime A, Q = getTransitionMats(dt) self.prevStateMean, self.prevStateCovariance = self.filter.filter_update( \ self.prevStateMean, self.prevStateCovariance, observation = measX, transition_matrix = A, transition_covariance = Q, observation_matrix = np.array([[1,0]]), observation_covariance=np.array([[measurementVariance]]) ) self.prevStateTime = measTime
class PointMassTracker(object): def __init__(self, frequency, transition_variances, observation_variances, init_pose, init_variances, change_thresholds): delta_t = 1.0 / frequency transition_mat = np.array([[1, 0, delta_t, 0], [0, 1, 0, delta_t], [0, 0, 1, 0], [0, 0, 0, 1]]) transition_cov = np.array([[transition_variances[0], 0, 0, 0], [0, transition_variances[1], 0, 0], [0, 0, transition_variances[2], 0], [0, 0, 0, transition_variances[3]]]) observation_mat = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) observation_cov = np.array([[observation_variances[0], 0], [0, observation_variances[1]]]) init_state = np.array([init_pose[0], init_pose[1], 0, 0]) init_cov = np.array([[init_variances[0], 0, 0, 0], [0, init_variances[1], 0, 0], [0, 0, init_variances[2], 0], [0, 0, 0, init_variances[3]]]) self.change_thresholds = change_thresholds self.kf = KalmanFilter(transition_matrices=transition_mat, observation_matrices=observation_mat, transition_covariance=transition_cov, observation_covariance=observation_cov, initial_state_mean=init_state, initial_state_covariance=init_cov) self.filtered_state_mean = init_state self.filtered_state_cov = init_cov def update_and_get_estimation(self, measurement): if measurement is not None: if abs(self.filtered_state_mean[0] - measurement[0]) > self.change_thresholds[0] or \ abs(self.filtered_state_mean[1] - measurement[1]) > self.change_thresholds[1]: measurement = None self.filtered_state_mean, self.filtered_state_cov = self.kf.filter_update(self.filtered_state_mean, self.filtered_state_cov, measurement) return self.filtered_state_mean
def test_kalman_filter_update(): kf = KalmanFilter( data.transition_matrix, data.observation_matrix, data.transition_covariance, data.observation_covariance, data.transition_offsets, data.observation_offset, data.initial_state_mean, data.initial_state_covariance) # use Kalman Filter (x_filt, V_filt) = kf.filter(X=data.observations) # use online Kalman Filter n_timesteps = data.observations.shape[0] n_dim_state = data.transition_matrix.shape[0] x_filt2 = np.zeros((n_timesteps, n_dim_state)) V_filt2 = np.zeros((n_timesteps, n_dim_state, n_dim_state)) for t in range(n_timesteps - 1): if t == 0: x_filt2[0] = data.initial_state_mean V_filt2[0] = data.initial_state_covariance (x_filt2[t + 1], V_filt2[t + 1]) = kf.filter_update( x_filt2[t], V_filt2[t], data.observations[t + 1], transition_offset=data.transition_offsets[t] ) assert_array_almost_equal(x_filt, x_filt2) assert_array_almost_equal(V_filt, V_filt2)
class StateEstimationAltitudeCam: def __init__(self): self.state = [0, 0] self.covariance = np.eye(2) self.observation_covariance = np.array([[1]]) self.transition_covariance = np.array([[0.001, 0.001], [0.001, 0.001]]) self.kf = KalmanFilter( transition_covariance=self.transition_covariance, # H observation_covariance=self.observation_covariance, # Q ) self.previous_update = None def update(self, observations): if not self.previous_update: self.previous_update = time.time() dt = time.time() - self.previous_update self.state, self.covariance = self.kf.filter_update( self.state, self.covariance, observations, transition_matrix=np.array([[1, dt], [0, 1]]), observation_matrix=np.array([[1, 0]]), # observation_offset = np.array([, 0, 0]) # observation_covariance=np.array(0.1*np.eye(1)) ) self.previous_update = time.time() def getAltitude(self): return self.state[0]
def fitKCA(t,z,q,fwd=0): # #Inputs: # t: Iterable with time indices # z: Iterable with measurements # q: Scalar that multiplies the seed states covariance # fwd: number of steps to forecast (optional, default=0) #Output: #x[0]: smoothed state means of position velocity and acceleration #x[1]: smoothed state covar of position velocity and acceleration #Dependencies: numpy, pykalman # #1) Set up matrices A,H and a seed for Q h=(t[-1]-t[0])/t.shape[0] A=np.array([[1,h,.5*h**2], [0,1,h], [0,0,1]]) Q=q*np.eye(A.shape[0]) #2) Apply the filter kf=KalmanFilter(transition_matrices=A,transition_covariance=Q) kf=kf.em(z) #4) Smooth x_mean,x_covar=kf.smooth(z) #5) Forecast for fwd_ in range(fwd): x_mean_,x_covar_=kf.filter_update(filtered_state_mean=x_mean[-1], filtered_state_covariance=x_covar[-1]) x_mean=np.append(x_mean,x_mean_.reshape(1,-1),axis=0) x_covar_=np.expand_dims(x_covar_,axis=0) x_covar=np.append(x_covar,x_covar_,axis=0) #6) Std series x_std=(x_covar[:,0,0]**.5).reshape(-1,1) for i in range(1,x_covar.shape[1]): x_std_=x_covar[:,i,i]**.5 x_std=np.append(x_std,x_std_.reshape(-1,1),axis=1) return x_mean,x_std,x_covar
def kalman_filter_one_factor_trained(x_series, y_series, train_period): x_train = x_series[:train_period] y_train = y_series[:train_period] x_train = np.vstack([x_train]).T[:, np.newaxis] # shape = (N,1,1) y_train = np.vstack([y_train]) # shape = (1, N) kf = KalmanFilter(transition_matrices=[1], observation_matrices=x_train, initial_state_mean=[1]) kf.em(y_train) state_mean, state_cov = kf.filter(y_train) x_predict = x_series[train_period:] y_predict = y_series[train_period:] predict = {} i = 0 state_mean_cache, state_cov_cache = state_mean[-1], state_cov[-1] for ind, x, y in zip(x_predict.index, x_predict, y_predict): res = kf.filter_update([state_mean_cache], state_cov_cache, observation=y, observation_matrix=np.array([[x]])) state_mean_cache = res[0][0] state_cov_cache = res[1] predict[i] = { x_series.index.name: ind, "state_mean": float(state_mean_cache), "state_cov": float(state_cov_cache) } i = i + 1 return kf, pd.DataFrame(predict).T.set_index(x_series.index.name)
class KalmanRegression(object): def __init__(self, x_state_mean, y_state_mean, delta=1e-5): trans_cov = delta / (1 - delta) * np.eye(2) obs_mat = np.expand_dims(np.vstack([[x_state_mean], [np.ones(len(x_state_mean))]]).T, axis=1) self.kf = KalmanFilter(n_dim_obs=1, n_dim_state=2, initial_state_mean=np.zeros(2), initial_state_covariance=np.ones((2, 2)), transition_matrices=np.eye(2), observation_matrices=obs_mat, observation_covariance=1.0, transition_covariance=trans_cov) means, cov = self.kf.filter(y_state_mean) self.mean = means[-1] self.cov = cov[-1] def update(self, x, y): self.mean, self.cov = self.kf.filter_update( self.mean, self.cov, y, observation_matrix=np.array([[x, 1.0]])) @property def state_mean(self): return self.mean
class KalmanMovingAverage(object): def __init__(self, asset, observation_covariance=1.0, initial_value=0, initial_state_covariance=1.0, transition_covariance=0.05): self.asset = asset self.kf = KalmanFilter( transition_matrices=[1], observation_matrices=[1], initial_state_mean=initial_value, initial_state_covariance=initial_state_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance) self.state_means = [self.kf.initial_state_mean] self.state_vars = [self.kf.initial_state_covariance] def update_all(self, observations): for observation in observations: self.update(observation) def update(self, observation): mu, cov = self.kf.filter_update(self.state_means[-1], self.state_vars[-1], observation) self.state_means.append(mu.flatten()[0]) self.state_vars.append(cov.flatten()[0])
class StateEstimationAccel: def __init__(self): """ # x,y,z, Xvelo,Yvelo, Zvelo Xaccel, Yaccel, Zaccel, roll, pitch, yaw """ self.state = [0, 0, 0, 0, 0] # x, y ,z self.covariance = np.eye(5) transition_covariance = np.array( [[0.01, 0, 0, 0, 0], [0, 0.1, 0, 0, 0], [0, 0, 0.001, 0, 0], [0, 0, 0, 0.001, 0], [0, 0, 0, 0, 0.001]] ) self.observation_covariance = np.eye(3) * 0.1 # self.transition_covariance = np.eye(5) * 0.001 print transition_covariance self.kf = KalmanFilter( transition_covariance=transition_covariance, observation_covariance=self.observation_covariance # H # Q ) def update(self, dt, observations): self.state, self.covariance = self.kf.filter_update( self.state, self.covariance, observations, transition_matrix=np.array( [[1, dt, 0.5 * (dt ** 2), 0, 0], [0, 1, dt, 0, 0], [0, 0, 1, 0, 0], [0, 0, 0, 1, 0], [0, 0, 0, 0, 1]] ), observation_matrix=np.array([[0, 0, 1, 0, 0], [0, 0, 0, 1, 0], [0, 0, 0, 0, 1]]), # observation_offset=np.array([[0]]), )
class anglefilter(object): def __init__(self): self.A_a = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] self.C_a = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] self.Q_a = np.asarray([[1e-3, 1e-4, 1e-4], [1e-4, 1e-2, 1e-4], [1e-4, 1e-4, 1e-2]]) / 50 self.R_a = np.asarray([[0.1, 1e-3, 1e-3], [1e-3, 0.1, 1e-3], [1e-3, 1e-3, 0.1]]) / 50 self.state_mean = np.zeros(3) self.state_covariance = self.Q_a self.filter = KalmanFilter(transition_matrices=self.A_a, observation_matrices=self.C_a, transition_covariance=self.Q_a, observation_covariance=self.R_a, initial_state_mean=np.asarray([0, 0, 0])) def update(self, angle_list): measurement = np.asarray(angle_list[:3]) self.state_mean, self.state_covariance = self.filter.filter_update( filtered_state_mean=self.state_mean, filtered_state_covariance=self.state_covariance, observation=measurement) new_spot_list = [ self.state_mean[0], self.state_mean[1], self.state_mean[2] ] + angle_list[3:] return new_spot_list
def kf_measure_with_intercept(): initial_state_mean = 0.8 initial_state_covariance = 1 # 构建一个卡尔曼滤波器 kf = KalmanFilter(transition_matrices=np.eye(2), observation_matrices=[0.01, 1], initial_state_mean=[initial_state_mean, 0], initial_state_covariance=np.ones((2, 2)), observation_covariance=1, transition_covariance=np.eye(2) * 0.0001) n_timesteps, n_dim_state, n_dim_obs = len(df), 2, 1 filtered_state_means = np.zeros((n_timesteps, n_dim_state)) filtered_state_covariances = np.zeros( (n_timesteps, n_dim_state, n_dim_state)) filtered_state_means[0] = initial_state_mean filtered_state_covariances[0] = initial_state_covariance for i in tqdm(range(n_timesteps - 1)): filtered_state_means[i + 1], filtered_state_covariances[ i + 1] = kf.filter_update(filtered_state_means[i], filtered_state_covariances[i], df.ic[i], observation_matrix=np.array( [[df.ih[i], 1]]))
def testKalmanFilter(self, data, data2): #d2 = [math.degrees(r) for r d2in ] xCorr = self.getCorrelation(data[:, 0], data2[:, 4]) #self.plotTwoSets(data, data2, m) print(xCorr) w, h = 1920, 1080 mns = [0, 0, 700, 0, 0, 0] cs = 6 * [[192, 107, 100, math.pi, math.pi, math.pi]] #print(cs) #print(mns.shape, cs.shape) kf = KalmanFilter(initial_state_mean=mns, initial_state_covariance=cs) #m, v = kf.filter(data2) #print(m) m = np.zeros_like(data2) mf = [[0], [0], [700], [0], [0], [0]] cf = 6 * [[ 192, 107, 100, math.pi / 1800, math.pi / 180000, math.pi / 1800 ]] for i, y in enumerate(data2): mf, cf = kf.filter_update(mf, cf, y) m[i] = mf[0] print(m.shape) data = data[:, 0] data2 = [math.degrees(r) for r in data2[:, 4]] m = [math.degrees(r) for r in m[:, 4]] xCorr = self.getCorrelation(data, m) print(xCorr) self.plotTwoSets(data2, m)
class AccelerometerReader(Reader): def __init__(self, type, model): super(AccelerometerReader, self).__init__(type, model) self.last_point = None self.points = [] self.mag_threshold2 = pow(config.ACCEL_MAG_THRESHOLD, 2) self.kf = KalmanFilter(initial_state_mean=0, n_dim_obs=1) self.point = None self.covariance = [ [0.0], [0.0], [0.0] ] def _handle_point(self, raw_point): if not self.point: self.point = raw_point if config.ACCEL_FILTER: (self.point[0], self.covariance[0]) = self.kf.filter_update(self.point[0], self.covariance[0], raw_point[0]) (self.point[1], self.covariance[1]) = self.kf.filter_update(self.point[1], self.covariance[1], raw_point[1]) (self.point[2], self.covariance[2]) = self.kf.filter_update(self.point[2], self.covariance[2], raw_point[2]) point = [self.point[0][0].data[0], self.point[1][0].data[0], self.point[2][0].data[0]] else: point = raw_point diff = ( abs(self.last_point[0] - point[0]), abs(self.last_point[1] - point[1]), abs(self.last_point[2] - point[2]) ) mag2 = pow(diff[0], 2) + pow(diff[1], 2) + pow(diff[2], 2) s = sqrt(mag2) # print mag2, self.mag_threshold2 if mag2 > self.mag_threshold2: self._save_data("sleep", { 'mag' : sqrt(mag2) - config.ACCEL_MAG_THRESHOLD}) self.last_point = point def read(self): pt = list(self.sensor.read()) if not self.last_point: self.last_point = pt return self._handle_point(pt)
class KLF(object): def __init__(self): self._trans_mat = np.eye(6) self._trans_conv = scipy.linalg.block_diag( np.eye(3) * 0.05, np.eye(3) * 0.2) self._trans_conv[2, 2] = 0.0872665 self._trans_conv[5, 5] = 0.349066 self._observation_mat = np.block([np.eye(3), np.eye(3) * 0]) self._observation_conv = np.eye(3) * 0.1 self._observation_conv[2, 2] = 0.0872665 self._filter = None #self._initialize() def _initialize(self, cur_state): #reinitialize kalman filter self._filter = KalmanFilter( observation_matrices=self._observation_mat, transition_covariance=self._trans_conv, observation_covariance=self._observation_conv, transition_offsets=np.array([0, 0, 0, 0, 0, 0]), observation_offsets=np.array([0, 0, 0]), n_dim_state=6, n_dim_obs=3) self._filtered_state = np.hstack([cur_state, [0, 0, 0]]) self._filtered_covariance = np.eye(6) * 0.1 def filter(self, cur_state, dt): #first time, we just initialize stuff if (self._filter is None): self._initialize(cur_state) return cur_state #if dt is certain time, restart filter if (dt > 1.25): #reinitialize print("KLF timed out, dt{}".format(dt)) self._initialize(cur_state) return cur_state #do kalman update self._trans_mat[:3, 3:] = np.eye(3) * dt #update nxt_state, nxt_conv = self._filter.filter_update( self._filtered_state, self._filtered_covariance, cur_state, transition_matrix=self._trans_mat) self._filtered_state = nxt_state self._filtered_covariance = nxt_conv return nxt_state
def kalmanFilterPositionChange(initial_state_mean, initial_state_covariance, initial_AccX_Value, new_AccX_Value): AccX_Value = np.array([initial_AccX_Value, new_AccX_Value]) AccX_Variance = 0.0007 # time step dt = 1 # transition_matrix F = [[1, dt, 0.5 * dt**2], [0, 1, dt], [0, 0, 1]] # observation_matrix H = [0, 0, 1] # transition_covariance Q = [[0.2, 0, 0], [0, 0.1, 0], [0, 0, 10e-4]] # observation_covariance R = AccX_Variance # initial_state_mean X0 = [0, 0, AccX_Value[0]] # initial_state_covariance P0 = [[0, 0, 0], [0, 0, 0], [0, 0, AccX_Variance]] n_timesteps = AccX_Value.shape[0] n_dim_state = 3 filtered_state_means = np.zeros((n_timesteps, n_dim_state)) filtered_state_covariances = np.zeros( (n_timesteps, n_dim_state, n_dim_state)) kf = KalmanFilter(transition_matrices=F, observation_matrices=H, transition_covariance=Q, observation_covariance=R, initial_state_mean=X0, initial_state_covariance=P0) # iterative estimation for the new measurement for t in range(n_timesteps): if t == 0: filtered_state_means[t] = initial_state_mean filtered_state_covariances[t] = initial_state_covariance else: filtered_state_means[t], filtered_state_covariances[t] = ( kf.filter_update(filtered_state_means[t - 1], filtered_state_covariances[t - 1], AccX_Value[t])) return (filtered_state_means[1], filtered_state_covariances[1], new_AccX_Value, filtered_state_means[1, 0] - filtered_state_means[0, 0])
def test_online_update(self): kf = KalmanFilter(transition_matrices = [[1, 1], [0, 1]], observation_matrices = [[0.1, 0.5], [-0.3, 0.0]]) measurements = np.asarray([[1,0], [0,0], [0,1]]) # 3 observations measurements = np.ma.asarray(measurements) measurements[1] = np.ma.masked # measurement at timestep 1 is unobserved kf = kf.em(measurements, n_iter=5) (filtered_state_means, filtered_state_covariances) = kf.filter(measurements) for t in range(1, 3): filtered_state_means[t], filtered_state_covariances[t] = \ kf.filter_update(filtered_state_means[t-1], filtered_state_covariances[t-1], measurements[t]) return filtered_state_means
def missingDataKalman(X, t): # Filter Configuration X = ma.where(X == -1, ma.masked, X) # t step dt = t[2] - t[1] # transition_matrix F = [[1, dt, 0.5 * dt * dt], [0, 1, dt], [0, 0, 1]] # observation_matrix H = [1, 0, 0] # transition_covariance Q = [[1, 0, 0], [0, 1e-4, 0], [0, 0, 1e-6]] # observation_covariance R = [0.01] # max error = 0.6m # initial_state_mean if (X[0] == ma.masked): X0 = [0, 0, 0] else: X0 = [X[0], 0, 0] # initial_state_covariance P0 = [[10, 0, 0], [0, 1, 0], [0, 0, 1]] n_tsteps = len(t) n_dim_state = 3 filtered_state_means = np.zeros((n_tsteps, n_dim_state)) filtered_state_covariances = np.zeros((n_tsteps, n_dim_state, n_dim_state)) # Kalman-Filter initialization kf = KalmanFilter(transition_matrices=F, observation_matrices=H, transition_covariance=Q, observation_covariance=R, initial_state_mean=X0, initial_state_covariance=P0) # iterative estimation for each new measurement for t in range(n_tsteps): if t == 0: filtered_state_means[t] = X0 filtered_state_covariances[t] = P0 else: filtered_state_means[t], filtered_state_covariances[t] = ( kf.filter_update(filtered_state_means[t - 1], filtered_state_covariances[t - 1], observation=X[t])) return filtered_state_means
class KFMulti(): """ Tracks multiple objects (without association) using pykalman """ def __init__(self, measurements): #shared KF (same dynamics used by all objects, we update tr and obs) self.num_tracks = len(measurements) self.observation_matrix = [[1, 0, 0, 0], [0, 0, 1, 0]] self.transition_matrix = [[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]] self.xs = [] #state estimates self.Ps = [] #cov matrices self.kf = KalmanFilter(transition_matrices=self.transition_matrix, observation_matrices=self.observation_matrix) self.createKFs(measurements) def createKFs(self, meas_all): #create a KF object for each tracked object, sharing tr and obs, but using individual states for meas in meas_all: init_state = [meas[0][0], 0, meas[0][1], 0] #in form [x,xv,y,yv] kf = KalmanFilter(transition_matrices=self.transition_matrix, observation_matrices=self.observation_matrix, initial_state_mean=init_state) kf = kf.em(np.array(meas), n_iter=5) x, P = kf.smooth(np.array(meas)) self.xs.append(x[-1, :]) #save the last state estimate self.Ps.append(P[-1, :]) #save last cov matrix estimate def update(self, obs_all=None): #update each state and cov estimate using latest obs (or None for Constant Vel) #if obs_all is set, length must be same as objects we have init on #return just the [x,y] for each tracked object (ie no vel estimate) if obs_all is not None: if len(obs_all) != self.num_tracks: ##Incorrect length of observations return None for i in range(self.num_tracks): obs = None if obs_all is not None: obs = obs_all[i] (x_new, P_new) = self.kf.filter_update( filtered_state_mean=self.xs[i], filtered_state_covariance=self.Ps[i], observation=obs) self.xs[i] = x_new self.Ps[i] = P_new new_pred = [] for obj in self.xs: new_pred.append([obj[0], obj[2]]) return new_pred
def trackKalman(): l_publ = rospy.Publisher("tracking", tracking, queue_size=10) rate = rospy.Rate(10) # 10hz initstate = [current_measurement[0], current_measurement[1], 0, 0] Transition_Matrix = [[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]] Observation_Matrix = [[1, 0, 0, 0], [0, 1, 0, 0]] initcovariance = 1.0e-3 * np.eye(4) transistionCov = 1.0e-4 * np.eye(4) observationCov = 1.0e-1 * np.eye(2) kf = KalmanFilter(transition_matrices=Transition_Matrix, observation_matrices=Observation_Matrix, initial_state_mean=initstate, initial_state_covariance=initcovariance, transition_covariance=transistionCov, observation_covariance=observationCov) start = 1 t = 1 while not rospy.is_shutdown(): #cv2.rectangle(image,(face[0]-50,face[1]-50),(face[0]+50,face[1]+50),(255,0,0),2) ##cv2.rectangle(image,(face_center_pixels[0]-50,face_center_pixels[1]-50),(face_center_pixels[0]+50,face_center_pixels[1]+50),(255,0,0),2) #cv2.imshow("Calibrated Boundary", image) #cv2.waitKey(1) if (start == 1): start = 0 filtered_state_means = initstate filtered_state_covariances = initcovariance print 'current measurement: ', current_measurement (pred_filtered_state_means, pred_filtered_state_covariances) = kf.filter_update( filtered_state_means, filtered_state_covariances, current_measurement) t += 1 filtered_state_means = pred_filtered_state_means filtered_state_covariances = pred_filtered_state_covariances print 'predicted: ', filtered_state_means[0], filtered_state_means[1] #print type(current_measurement[0]) #print type(filtered_state_means[0]) location = tracking() location.x0 = current_measurement[0] location.y0 = current_measurement[1] location.x1 = filtered_state_means[0] location.y1 = filtered_state_means[1] l_publ.publish(location) print '\n' rate.sleep()
def kalman_estimation(self, array, index): dt = self.time_step(index) X = array.copy() n_timesteps = len(index) n_dim_state = 3 # transition_matrix F = [[1, dt, 0.5 * dt * dt], [0, 1, dt], [0, 0, 1]] # observation_matrix H = [1, 0, 0] # transition_covariance Q = [[1, 0, 0], [0, 1e-4, 0], [0, 0, 1e-6]] # observation_covariance R = [0.04] # initial_state_mean X0 = [0, 0, 0] # initial_state_covariance P0 = [[10, 0, 0], [0, 1, 0], [0, 0, 1]] self.filtered_state_means = np.zeros((n_timesteps, n_dim_state)) self.filtered_state_covariances = np.zeros( (n_timesteps, n_dim_state, n_dim_state)) # Kalman-Filter initialization kf = KalmanFilter(transition_matrices=F, observation_matrices=H, transition_covariance=Q, observation_covariance=R, initial_state_mean=X0, initial_state_covariance=P0) # iterative estimation for each new measurement for t in range(n_timesteps): if t == 0: self.filtered_state_means[t] = X0 self.filtered_state_covariances[t] = P0 else: self.filtered_state_means[t], self.filtered_state_covariances[ t] = (kf.filter_update(self.filtered_state_means[t - 1], self.filtered_state_covariances[t - 1], observation=X[t])) filtered_values = self.filtered_state_means[:, 0] sigma = np.sqrt(self.filtered_state_covariances[:, 0, 0]) return filtered_values, sigma
def kf_predict(meassurements): global i newMeasurement = np.ma.asarray(-1) initial_state_mean = [measurements[0, 0], 0, measurements[0, 1], 0] #transition_matrix = [[1, 0, 0.4, 0],[1, 0, 0.1, 0],[0, 0.2, 1, 1],[0.2, 0.5, 0.3, 1]] #TWEAK THIS BASED ON EXPERIMENTATION transition_matrix = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]] observation_matrix = [[1, 0, 0, 0], [0, 1, 0, 0]] kf1 = KalmanFilter(transition_matrices=transition_matrix, observation_matrices=observation_matrix, initial_state_mean=initial_state_mean) kf1 = kf1.em(measurements, n_iter=5) (smoothed_state_means, smoothed_state_covariances) = kf1.smooth(measurements) #PREDICTION x_now = smoothed_state_means[-1, :] P_now = smoothed_state_covariances[-1, :] #newMeasurement = np.ma.asarray(measurements[measurements.shape[0]-1]) #print "++++++++" #print x_now #print P_now #print "+++++++++" (x_now, P_now) = kf1.filter_update(filtered_state_mean=x_now, filtered_state_covariance=P_now, observation=None) #PLOTS - Comment out later plt.figure(i) i = i + 1 plt.hold(True) times = range(measurements.shape[0]) plt.plot(times, measurements[:, 0], 'bo', times, measurements[:, 1], 'ro', times, smoothed_state_means[:, 0], 'b--', times, smoothed_state_means[:, 2], 'r--') #print np.array(smoothed_state_means.shape) #print x_now[0] #print x_now[1] #print np.array(times).shape[0] + 1 plt.plot([np.array(times).shape[0] + 1], x_now[0], 'xb', [np.array(times).shape[0] + 1], x_now[1], 'xr') return (x_now)
class KalmanRegression(object): """ Uses a Kalman Filter to estimate regression parameters in an online fashion. Estimated model: y ~ beta * x + alpha """ def __init__(self, initial_y, initial_x, delta=1e-5, maxlen=3000): self._x = initial_x.name self._y = initial_y.name self.maxlen = maxlen trans_cov = delta / (1 - delta) * np.eye(2) obs_mat = np.expand_dims(np.vstack([[initial_x], [np.ones(initial_x.shape[0])]]).T, axis=1) self.kf = KalmanFilter(n_dim_obs=1, n_dim_state=2, initial_state_mean=np.zeros(2), initial_state_covariance=np.ones((2, 2)), transition_matrices=np.eye(2), observation_matrices=obs_mat, observation_covariance=1.0, transition_covariance=trans_cov) state_means, state_covs = self.kf.filter(initial_y.values) self.means = pd.DataFrame(state_means, index=initial_y.index, columns=['beta', 'alpha']) self.state_cov = state_covs[-1] def update(self, observations): x = observations[self._x] y = observations[self._y] mu, self.state_cov = self.kf.filter_update(self.state_mean, self.state_cov, y, observation_matrix=np.array( [[x, 1.0]])) mu = pd.Series(mu, index=['beta', 'alpha'], name=observations.name) self.means = self.means.append(mu) if self.means.shape[0] > self.maxlen: self.means = self.means.iloc[-self.maxlen:] def get_spread(self, observations): x = observations[self._x] y = observations[self._y] return y - (self.means.beta[-1] * x + self.means.alpha[-1]) @property def state_mean(self): return self.means.iloc[-1]
class LocationKalman: def __init__(self, start_x, start_y, time_delta): position_stdev = 0.05 A = numpy.asarray([[1, 0, time_delta, 0], [0, 1, 0, time_delta], [0, 0, 1, 0], [0, 0, 0, 1]]) H = numpy.asarray([[1, 0, 0, 0], [0, 1, 0, 0]]) r = position_stdev r2 = r * r R = numpy.asarray([[r, 0], [0, r]]) Q = numpy.asarray([[r2, 0, 0, 0], [0, r2, 0, 0], [0, 0, r2, 0], [0, 0, 0, r2]]) initial_state = numpy.asarray([start_x, start_y, 0, 0]) self._kf = KalmanFilter(transition_matrices=A, observation_matrices=H, transition_covariance=Q, observation_covariance=R, initial_state_mean=initial_state) self._means = initial_state self._covs = numpy.zeros((4, 4)) def step(self, x, y): (self._means, self._covs) = \ self._kf.filter_update(self._means, self._covs, observation=numpy.asarray([x, y])) def missing_step(self): (self._means, self._covs) = \ self._kf.filter_update(self._means, self._covs, observation=None) def get_means(self): return tuple(self._means)
class KalmanRegression(object): """ Uses a Kalman Filter to estimate regression parameters in an online fashion. Estimated model: y ~ beta * x + alpha """ def __init__(self, initial_y, initial_x, delta=1e-5, maxlen=3000): self._x = initial_x.name self._y = initial_y.name self.maxlen = maxlen trans_cov = delta / (1 - delta) * np.eye(2) obs_mat = np.expand_dims( np.vstack([[initial_x], [np.ones(initial_x.shape[0])]]).T, axis=1) self.kf = KalmanFilter(n_dim_obs=1, n_dim_state=2, initial_state_mean=np.zeros(2), initial_state_covariance=np.ones((2, 2)), transition_matrices=np.eye(2), observation_matrices=obs_mat, observation_covariance=1.0, transition_covariance=trans_cov) state_means, state_covs = self.kf.filter(initial_y.values) self.means = pd.DataFrame(state_means, index=initial_y.index, columns=['beta', 'alpha']) self.state_cov = state_covs[-1] def update(self, observations): x = observations[self._x] y = observations[self._y] mu, self.state_cov = self.kf.filter_update( self.state_mean, self.state_cov, y, observation_matrix=np.array([[x, 1.0]])) mu = pd.Series(mu, index=['beta', 'alpha'], name=observations.name) self.means = self.means.append(mu) if self.means.shape[0] > self.maxlen: self.means = self.means.iloc[-self.maxlen:] def get_spread(self, observations): x = observations[self._x] y = observations[self._y] return y - (self.means.beta[-1] * x + self.means.alpha[-1]) @property def state_mean(self): return self.means.iloc[-1]
def applyKalmanFilter(accelData): # Time = [i * 0.015 for i in range(len(accelData))] # time step dt = 0.015 # transition_matrix F = [[1, dt, 0.5 * dt ** 2], [0, 1, dt], [0, 0, 1]] # observation_matrix H = [0, 0, 1] # transition_covariance Q = [[0.5, 0, 0], [0, 0.5, 0], [0, 0, 0.5]] # observation_covariance R = np.var(accelData) # initial_state_mean X0 = [0, 0, 0] # initial_state_covariance P0 = [[0, 0, 0], [0, 0, 0], [0, 0, R]] n_timesteps = len(accelData) n_dim_state = 3 filtered_state_means = np.zeros((n_timesteps, n_dim_state)) filtered_state_covariances = np.zeros((n_timesteps, n_dim_state, n_dim_state)) kf = KalmanFilter( transition_matrices=F, observation_matrices=H, transition_covariance=Q, observation_covariance=R, initial_state_mean=X0, initial_state_covariance=P0, ) # iterative estimation for each new measurement for t in range(n_timesteps): if t == 0: filtered_state_means[t] = X0 filtered_state_covariances[t] = P0 else: filtered_state_means[t], filtered_state_covariances[t] = kf.filter_update( filtered_state_means[t - 1], filtered_state_covariances[t - 1], accelData[t], ) return filtered_state_means[:, 2]
class Filter(object): def __init__(self, first_obs): self.transition_covariance = 0.8 self.observation_covariance = 0.8 self.state = first_obs self.measurements = [first_obs] self.covariance = None self.transition_matrices = [0.8] self.observation_matrices = [0.8] self.initial_state_mean = [first_obs] self.kf = KalmanFilter( # self.transition_matrices, # observation_matrices=self.observation_matrices, initial_state_mean=0, n_dim_state=1, # self.transition_covariance, observation_covariance=self.observation_covariance) # self.state, self.covariance = self.kf.filter(self.measurements) # self.state = self.state[0] # self.covariance = self.covariance[0][0] def update(self, rssi): # self.state = self.kf.filter([rssi])[0][0][0] self.measurements = self.measurements[-5:] self.measurements.append(rssi) means, covariances = self.kf.filter(self.measurements) self.state, self.covariance = self.kf.filter_update( means[-1], covariances[-1], [rssi]) self.state = self.state[0] self.covariance = self.covariance[0][0] # def update(self, rssi): # self.state, self.covariance = self.kf.filter_update( # [self.state], [self.covariance], [rssi]) # try: # self.state = self.state[0][0] # except: # self.state = self.state[0] # self.covariance = self.covariance[0][0] def transition_function(self): return def observation_functions(self): return
def trackKalman(): l_publ = rospy.Publisher("tracking", tracking, queue_size = 10) rate = rospy.Rate(10) # 10hz initstate = [current_measurement[0], current_measurement[1], 0, 0] Transition_Matrix=[[1,0,1,0],[0,1,0,1],[0,0,1,0],[0,0,0,1]] Observation_Matrix=[[1,0,0,0],[0,1,0,0]] initcovariance=1.0e-3*np.eye(4) transistionCov=1.0e-4*np.eye(4) observationCov=1.0e-1*np.eye(2) kf=KalmanFilter(transition_matrices=Transition_Matrix, observation_matrices =Observation_Matrix, initial_state_mean=initstate, initial_state_covariance=initcovariance, transition_covariance=transistionCov, observation_covariance=observationCov) start = 1 t = 1 while not rospy.is_shutdown(): #cv2.rectangle(image,(face[0]-50,face[1]-50),(face[0]+50,face[1]+50),(255,0,0),2) ##cv2.rectangle(image,(face_center_pixels[0]-50,face_center_pixels[1]-50),(face_center_pixels[0]+50,face_center_pixels[1]+50),(255,0,0),2) #cv2.imshow("Calibrated Boundary", image) #cv2.waitKey(1) if (start == 1): start = 0 filtered_state_means = initstate filtered_state_covariances = initcovariance print 'current measurement: ', current_measurement (pred_filtered_state_means, pred_filtered_state_covariances) = kf.filter_update(filtered_state_means, filtered_state_covariances, current_measurement); t += 1 filtered_state_means = pred_filtered_state_means filtered_state_covariances = pred_filtered_state_covariances print 'predicted: ', filtered_state_means[0], filtered_state_means[1] #print type(current_measurement[0]) #print type(filtered_state_means[0]) location = tracking() location.x0 = current_measurement[0] location.y0 = current_measurement[1] location.x1 = filtered_state_means[0] location.y1 = filtered_state_means[1] l_publ.publish(location) print '\n' rate.sleep()
def lambda_handler(event, context): # Log the received event # implement me! try: variance = get_variance( event ) if (PREVIOUS_STATE_KEY in event): prev = event[PREVIOUS_STATE_KEY]; prevMean = np.array([ prev[LATITUDE_KEY], prev[LONGITUDE_KEY]]) prevCovar = np.multiply( prev[VARIANCE_KEY], COVARIANCE_MATRIX ) kf = KalmanFilter( initial_state_mean=prevMean, initial_state_covariance=prevCovar, em_vars=['transition_covariance', 'observation_covariance'], transition_matrices=TRANSITION_MATRIX) curObs = np.array([ event[LATITUDE_KEY], event[LONGITUDE_KEY]]); curCovar = np.multiply( event[VARIANCE_KEY], np.eye( len(curObs))) nextMean, nextCovar = kf.filter_update( prevMean, prevCovar, observation_matrix=OBSERVATION_MATRIX, observation=curObs, observation_covariance=curCovar) return { LATITUDE_KEY: nextMean[0], LONGITUDE_KEY: nextMean[1], VARIANCE_KEY: nextCovar[0][0], } else: # if first point in sequence... # just return the single location point, with corresponding accuracy return { LATITUDE_KEY: event[LATITUDE_KEY], LONGITUDE_KEY: event[LONGITUDE_KEY], VARIANCE_KEY: variance } except Exception as e: print(e) return { 'message': str(e) }
class KalmanSmoother: def __init__(self, orice): self.kf = KalmanFilter(transition_matrices=np.matrix(''' 1. 0.; 0. 1. '''), observation_matrices=np.matrix(''' 1. 0.; 0. 1. ''')) self.filtered_state_means = np.zeros((2)) self.filtered_state_covariances = np.matrix(np.eye(2))*1000 def get_distance_estimation(self, observation): self.filtered_state_means, self.filtered_state_covariances = self.kf.filter_update(self.filtered_state_means, self.filtered_state_covariances, observation) return self.filtered_state_means[0]
class StateEstimationAltitude2(): def __init__(self): self.state = [0, 0] self.covariance = np.eye(2) # self.transition_covariance = np.array([ # [0.0000025, 0.000005], # [0.0000005, 0.0000001], # ]) self.observation_covariance = np.array([ [0.01, 0], [0, 0.5], ]) self.transition_covariance = np.array([ [0.003, 0.05], [0, 0.02], ]) self.kf = KalmanFilter( transition_covariance=self.transition_covariance, # H observation_covariance=self.observation_covariance, # Q ) self.previous_update = None def update(self, dt, observations): if not self.previous_update: self.previous_update = time.time() self.state, self.covariance = ( self.kf.filter_update( self.state, self.covariance, observations, transition_matrix=np.array([ [1, dt], [0, 1], ]), observation_matrix=np.array([ [1, 0], [0, 1], ]), # observation_offset = np.array([, 0, 0]) # o1bservation_covariance=np.array(0.1*np.eye(1)) )) self.previous_update = time.time() def getAltitude(self): return self.state[0]
class KalmanMovingAverage(object): """ Estimates the moving average of a price process via Kalman Filtering. See http://pykalman.github.io/ for docs on the filtering process. """ def __init__(self, asset, observation_covariance=1.0, initial_value=0, initial_state_covariance=1.0, transition_covariance=0.05, initial_window=20, maxlen=3000, freq='1d'): self.asset = asset self.freq = freq self.initial_window = initial_window self.maxlen = maxlen self.kf = KalmanFilter( transition_matrices=[1], observation_matrices=[1], initial_state_mean=initial_value, initial_state_covariance=initial_state_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance) self.state_means = pd.Series([self.kf.initial_state_mean], name=self.asset) self.state_vars = pd.Series([self.kf.initial_state_covariance], name=self.asset) def update(self, observations): for dt, observation in observations[self.asset].iteritems(): self._update(dt, observation) def _update(self, dt, observation): mu, cov = self.kf.filter_update(self.state_means.iloc[-1], self.state_vars.iloc[-1], observation) self.state_means[dt] = mu.flatten()[0] self.state_vars[dt] = cov.flatten()[0] if self.state_means.shape[0] > self.maxlen: self.state_means = self.state_means.iloc[-self.maxlen:] if self.state_vars.shape[0] > self.maxlen: self.state_vars = self.state_vars.iloc[-self.maxlen:]
class LightSensorReader(Reader): def __init__(self, type, model): super(LightSensorReader, self).__init__(type, model) self.kf = KalmanFilter(initial_state_mean=0, n_dim_obs=1) self.point = None self.covariance = [0.0] def read(self): raw_point = [float(self.sensor.read())] if not self.point: self.point = raw_point (self.point, self.covariance) = self.kf.filter_update(self.point, self.covariance, raw_point) self._save_data("light", {'light': int(self.point[0])})
class Regression(object): """ y = beta * x + alpha """ def __init__(self, initial_x, initial_y, date, delta=1e-5): trans_cov = delta / (1 - delta) * np.eye(2) xList = [] for x in initial_x: xList.append([[x,1.]]) obs_mat = np.vstack([xList]) self.kf = KalmanFilter(n_dim_obs=1, n_dim_state=2, initial_state_mean=np.zeros(2), initial_state_covariance=np.ones((2, 2)), transition_matrices=np.eye(2), observation_matrices=obs_mat, observation_covariance=1.0, transition_covariance=trans_cov) state_means, state_covs = self.kf.filter(np.array(initial_y)) self.means = pd.DataFrame(state_means, columns=['beta', 'alpha']) self.state_cov = state_covs[-1] def update(self, observations, date): x = observations[0] y = observations[1] mu, self.state_cov = self.kf.filter_update(self.state_mean, self.state_cov, y, observation_matrix=np.array([[x, 1.0]])) mu = pd.Series(mu, index=['beta', 'alpha'], name=date) self.means = self.means.append(mu) def get_spread(self, observations): x = observations[0] y = observations[1] return y - (self.means.beta * x + self.means.alpha) @property def state_mean(self): return self.means.iloc[-1]
def kalman_filtering(price_sequence): h = 1 #time step A = np.array([[1,h,.5*h**2], [0,1,h], [0,0,1]]) Q = np.eye(A.shape[0]) #2) Apply the filter kf = KalmanFilter(transition_matrices = A , transition_covariance = Q) means, covariances = kf.filter([price_sequence[0]]) filtered_price_sequence = [means[0,0]] for i in range(1,len(price_sequence)): #to track it (streaming) new_mean, new_covariance = kf.filter_update(means[-1], covariances[-1], price_sequence[i]) means = np.vstack([means,new_mean]) covariances = np.vstack([covariances,new_covariance.reshape((1,3,3))]) filtered_price_sequence.append(means[i,0]) return filtered_price_sequence
def kalman_track(tser): #get the tracking segments segments = trigger_and_hold(tser,5,10) #delta t ts = 0.0002 #get the first time series slice sig_slice = tser[:,0]/np.max(tser[:,0]) #the kalman filter to smooth in the angle domain #array of angle indecies (change to actual dimentions?) #set up initial conditions #filter for time series kf2 = KalmanFilter(transition_matrices=np.array([[1., ts, .5*ts**2, 0., 0, 0, ], [0., 1., ts, 0., 0, 0, ], [0., 0., 1., 0., 0, 0, ], [0., 0., 0., 1., ts, 0.5*ts**2 ], [0., 0., 0., 0., 1, ts ], [0., 0., 0., 0., 0 , 1. ]]), transition_covariance = np.array([[ts**2., 0., 0., 0., 0, 0.], [0., 1., 0., 0., 0., 0.], [0., 0, 1/ts**2, 0., 0., 0.], [0., 0., 0., ts**2., 0., 0.], [0., 0., 0., 0., 1., 0.], [0., 0, 0., 0., 0., 1/ts**2]]), observation_matrices= np.array([[1,0,0,0,0,0], [0,0,0,1,0,0]]), observation_covariance = np.array([[1, 0.,], [0.,1]])) #how much of the time series to filter - for debugging testrange = len(segments)-1 #allocate memory filtered_state_means = np.zeros((testrange+1,6)) filtered_state_cov = np.zeros((testrange+1,6,6)) #x = arange(len(sig_slice),dtype = float) #kf1 = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]),observation_covariance = [[100000.0]]) #sm_slice = kf1.smooth(sig_slice)[0][:,0] #p = [p_m1, p_m2, sd1, sd2, sc1, sc2] #plsq = leastsq(res, p, args = (sm_slice/np.sum(sm_slice), x)) filtered_state_means[0,:] = [m1,0,0,m2,0,0] filtered_state_cov[0,:] = np.zeros((6,6)) #get the valid tracking segments #itteratively track for t in range(testrange): print t if segments[t]: sig_slice = tser[:,t]/np.max(tser[:,t]) observation = observe(sig_slice,filtered_state_means[t,0],filtered_state_means[t,3]) filtered_state_means[t+1,:],filtered_state_cov[t+1,:] = kf2.filter_update(filtered_state_means[t,:], filtered_state_cov[t,:,:], observation) else: filtered_state_means[t+1,:] = [m1,0,0,m2,0,0] filtered_state_cov[t+1,:] = filtered_state_cov[t,:] return filtered_state_means
def kf_metric_plot(metric): assert isinstance(metric, Metric), "Not a metric: {0!s}".format(metric) assert hasattr(metric, 'data'), "No Data" import numpy as np from bounos import DataPackage from pykalman import KalmanFilter data = DataPackage("/dev/shm/dat-2013-02-01-13-58-48.aietes.npz") rnd = np.random.RandomState(0) # generate a noisy sine wave to act as our fake observations n_timesteps = data.tmax x = range(0, n_timesteps) records = metric.data try: obs_dim = len(records[0]) except TypeError as e: obs_dim = 1 observations = np.ma.array(records) # to put it as(tmax,3) masked = 0 for i in x: try: if rnd.normal(2, 2) >= 0: observations[i] = np.ma.masked masked += 1 except BaseException as e: print(i) raise e print("{0:f}% Masked".format(((masked * 100.0) / data.tmax))) print("Records: Shape: {0!s}, ndim: {1!s}, type: {2!s}".format(records.shape, records.ndim, type(records))) # create a Kalman Filter by hinting at the size of the state and observation # space. If you already have good guesses for the initial parameters, put them # in here. The Kalman Filter will try to learn the values of all # variables. kf = KalmanFilter(n_dim_obs=obs_dim, n_dim_state=obs_dim) # You can use the Kalman Filter immediately without fitting, but its estimates # may not be as good as if you fit first. # states_pred = kf.em(observations, n_iter=data.tmax).smooth(observations) # print 'fitted model: %s' % (kf,) # Plot lines for the observations without noise, the estimated position of the # target before fitting, and the estimated position after fitting. fig = plt.figure(figsize=(16, 6)) ax1 = fig.add_subplot(111) filtered_state_means = np.zeros((n_timesteps, kf.n_dim_state)) filtered_state_covariances = np.zeros( (n_timesteps, kf.n_dim_state, kf.n_dim_state)) for t in x: if t == 0: tmp = np.zeros(kf.n_dim_state) tmp.fill(500) filtered_state_means[t] = tmp print(filtered_state_means[t]) continue if masked and not observations.mask[t].any(): ax1.axvline(x=t, linestyle='-', color='r', alpha=0.1) try: filtered_state_means[t], filtered_state_covariances[t] = \ kf.filter_update( filtered_state_means[t - 1], filtered_state_covariances[t - 1], observations[t]) except IndexError as e: print(t) raise e (p, v) = (filtered_state_means, filtered_state_covariances) errors = map(np.linalg.norm, p[:] - records[:]) pred_err = ax1.plot(x, p[:], marker=' ', color='b', label='predictions-x') obs_scatter = ax1.plot(x, records, linestyle='-', color='r', label='observations-x', alpha=0.8) ax1.set_ylabel(metric.label) ax2 = ax1.twinx() error_plt = ax2.plot( x, errors, linestyle=':', color='g', label="Error Distance") ax2.set_yscale('log') ax2.set_ylabel("Error") lns = pred_err + obs_scatter + error_plt labs = [l.get_label() for l in lns] plt.legend(lns, labs, loc='upper right') plt.xlim(0, 500) ax1.set_xlabel('time') fig.suptitle(data.title) print("Predicted ideal {0!s}: {1!s}".format(metric.label, str(p[-1]))) plt.show()
def main(): #Paramètres du modèle #Indiquer à quoi correspond chaque variable osigma=0.1; transition_matrix = np.array([[1., 0.,0.],[1., 1.,0.],[0.,0,0.9]]) transition_covariance = np.zeros((3,3)); observation_matrix = np.array([[0., 1.,0.],[0., 0.,1.]]) observation_covariance = np.eye(2)*osigma initial_state_mean = np.array([1,0,10]) initial_state_covariance = np.eye(3); #Observations observations=np.array([ [1.1,9.2], [1.9,8.1], [2.8,7.2], [4.2,6.6], [5.0,5.9], [6.1,5.32], [7.2,4.7], [8.1,4.3], [9.0,3.9]]) # Filtrage à la main # Quels sont les paramètres du constructeur ? kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, ) # Que conserverons les variables suivantes ? hand_state_estimates=[initial_state_mean] hand_state_cov_estimates=[initial_state_covariance] for anObs in observations: # Quelles étapes du filtrage sont réalisées par la ligne suivante (aMean,aCov) = kf.filter_update(hand_state_estimates[-1],hand_state_cov_estimates[-1],anObs) hand_state_estimates.append(aMean) hand_state_cov_estimates.append(aCov) hand_state_estimates=np.array(hand_state_estimates) # A quoi sert la ligne suivante ? hand_positions=np.dot(hand_state_estimates,observation_matrix.T ) plt.figure(1) plt.plot(observations[:,0],observations[:,1], 'r+') plt.plot(hand_positions[:,0],hand_positions[:,1], 'b') plt.savefig('illustration_filtrage_main.pdf') plt.close() # Filtrage complet # Que fait cette section ? # Quels sont les paramètres supplémentaires donnés au constructeur ? # Quels sont les autres paramètres possibles ? kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, initial_state_mean=initial_state_mean, initial_state_covariance=initial_state_covariance, ) (filtered_state_estimates,filtered_state_cov_estimates) = kf.filter(observations) filtered_positions=np.dot(filtered_state_estimates,observation_matrix.T ) plt.figure(1) plt.plot(observations[:,0],observations[:,1], 'r+') plt.plot(filtered_positions[:,0],filtered_positions[:,1], 'b') plt.savefig('illustration_filtrage.pdf') plt.close() # Lissage # Que fait cette section ? # Quel est la différence avec le filtrage ? # Puis-je faire un lissage "à la main" observation par observation ? kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, initial_state_mean=initial_state_mean, initial_state_covariance=initial_state_covariance, ) (smoothed_state_estimates,smoothed_state_cov_estimates) = kf.smooth(observations) smoothed_positions=np.dot(smoothed_state_estimates,observation_matrix.T ) plt.figure(2) plt.plot(observations[:,0],observations[:,1], 'r+') plt.plot(smoothed_positions[:,0],smoothed_positions[:,1], 'b') plt.savefig('illustration_lissage.pdf') plt.close()
class Main: def __init__(self): self._observations = pickle.load(open("dumps/marker_observations.dump")) transition_covariance = np.array([[0.025, 0.005], [0.0005, 0.01]]) self.kf = KalmanFilter( transition_covariance=transition_covariance, observation_covariance=np.eye(1) * 1 # H # Q ) self.altitude = [] self.co = [] self.state = [0, 0] self.covariance = np.eye(2) test = "test_9" self._baro = pickle.load(open("data/duedalen/%s/barometer.dump" % test)) self._throttle = pickle.load(open("data/duedalen/%s/throttle.dump" % test)) self._sonar = pickle.load(open("data/12.11.13/%s/sonar.dump" % test)) self self.acceleration = pickle.load(open("data/12.11.13/%s/acceleration.dump" % test)) self.z_velocity = [a.z_velocity for a in self.acceleration] self.baro = [i[1] for i in self._baro] self.throttle = [(i[1] - 1000.0) / (1000.0) for i in self._throttle] print self.throttle self.sonar = [i[1] for i in self._sonar] self.cam_alt = [marker.z if marker else np.ma.masked for marker in self._observations] for i in xrange(len(self._baro) - 1): dt = self._baro[i + 1][0] - self._baro[i][0] print dt # self.zvelo = [i.value for i in self._zvelo] # print self.zvelo # exit() def learn(self): dt = 0.10 kf = KalmanFilter( em_vars=["transition_covariance", "observation_covariance"], observation_covariance=np.eye(2) * 1, transition_covariance=np.array( [[0.000025, 0, 0.0005, 0], [0, 0.000025, 0, 0.0005], [0.0005, 0, 0.001, 0], [0, 0.000025, 0, 0.001]] ), transition_matrices=np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]), observation_matrices=np.array([[1, 0, 0, 0], [0, 1, 0, 0]]), ) states, co = kf.em(self.offline_data).smooth(self.offline_data) print kf.transition_covariance self.lx = [s[0] for s in states] self.ly = [s[1] for s in states] # print kf.transition_covariance # print kf.observation_covariance def update_filter(self, value, control): dt = 0.10 # if abs(value[0] - self.state[0]) > 10: # value = Non control = control / 200 print value self.state, self.covariance = self.kf.filter_update( self.state, self.covariance, value, transition_matrix=np.array([[1, dt], [0, 1]]), observation_matrix=np.array([[1, 0]]), transition_offset=np.array([control]), ) self.co.append(self.covariance[0]) self.altitude.append(self.state[0]) def run(self): self.i = 0 for c in self.throttle: self.update_filter([self.cam_alt[self.i]], c) self.i += 1 def draw_fig(self): pl.figure(dpi=80) pl.plot(self.altitude, color="b") pl.plot(self.cam_alt, color="r") pl.plot(self.baro, color="g") pl.show()
class KalmanFilterPairsTradingStrategy(StrategyBase): """ MeanReversionSpreadStrategy """ def __init__( self, events_engine, data_board ): super(KalmanFilterPairsTradingStrategy, self).__init__(events_engine, data_board) self.symbols = ['EWA US Equity', 'EWC US Equity'] self.bollinger_scaler = 1.0 self.state_cov_multiplier = np.power(0.01, 2) # 0.1: spread_std=2.2, cov=16 ==> 0.01: 0.22, 0.16 self.observation_cov = 0.001 self.current_ewa_size = 0 self.current_ewc_size = 0 self._kf = None # Kalman Filter self._current_state_means = None self._current_state_covs = None def on_bar(self, bar_event): print(bar_event.bar_start_time) A = self._data_board.get_hist_price(self.symbols[0], bar_event.bar_start_time) B = self._data_board.get_hist_price(self.symbols[1], bar_event.bar_start_time) x = A['Price'].iloc[-1] y = B['Price'].iloc[-1] observation_matrix_stepwise = np.array([[x, 1]]) observation_stepwise = y spread = None spread_std = None if self._kf is None: self._kf = KalmanFilter(n_dim_obs=1, n_dim_state=2, initial_state_mean=np.ones(2), # initial value initial_state_covariance=np.ones((2, 2)), # initial value transition_matrices=np.eye(2), # constant observation_matrices=observation_matrix_stepwise, # depend on x observation_covariance=self.observation_cov, # constant transition_covariance=np.eye(2) * self.state_cov_multiplier) # constant P = np.ones((2, 2)) + np.eye(2)*self.state_cov_multiplier spread = y - observation_matrix_stepwise.dot(np.ones(2))[0] spread_std = np.sqrt(observation_matrix_stepwise.dot(P).dot(observation_matrix_stepwise.transpose())[0][0] + self.observation_cov) state_means_stepwise, state_covs_stepwise = self._kf.filter(observation_stepwise) # depend on y self._current_state_means = state_means_stepwise[0] self._current_state_covs = state_covs_stepwise[0] else: state_means_stepwise, state_covs_stepwise = self._kf.filter_update( self._current_state_means, self._current_state_covs, observation=observation_stepwise, observation_matrix=observation_matrix_stepwise) P = self._current_state_covs + np.eye(2)*self.state_cov_multiplier # This has to be small enough spread = y - observation_matrix_stepwise.dot(self._current_state_means)[0] spread_std = np.sqrt(observation_matrix_stepwise.dot(P).dot(observation_matrix_stepwise.transpose())[0][0] + self.observation_cov) self._current_state_means = state_means_stepwise self._current_state_covs = state_covs_stepwise # residual is assumed to be N(0, spread_std) bollinger_ub = self.bollinger_scaler * spread_std bollinger_lb = - self.bollinger_scaler * spread_std coeff = self._current_state_means[0] print(spread, spread_std) if (spread > bollinger_ub) and (self.current_ewa_size >= 0): print ('Hit upper band, short spread.') o = OrderEvent() o.full_symbol = self.symbols[0] o.order_type = OrderType.MARKET o.order_size = int(-1000*coeff) - self.current_ewa_size self.place_order(o) self.current_ewa_size = int(-1000*coeff) o = OrderEvent() o.full_symbol = self.symbols[1] o.order_type = OrderType.MARKET o.order_size = 1000 - self.current_ewc_size self.place_order(o) self.current_ewc_size = 1000 elif (spread < bollinger_lb) and (self.current_ewa_size <= 0): print('Hit lower band, long spread.') o = OrderEvent() o.full_symbol = self.symbols[0] o.order_type = OrderType.MARKET o.order_size = int(1000 * coeff) - self.current_ewa_size self.place_order(o) self.current_ewa_size = int(1000 * coeff) o = OrderEvent() o.full_symbol = self.symbols[1] o.order_type = OrderType.MARKET o.order_size = -1000 - self.current_ewc_size self.place_order(o) self.current_ewc_size = -1000 elif (spread < 0) and (spread > bollinger_lb) and (self.current_ewa_size < 0): print('spread crosses below average.cover short spread') o = OrderEvent() o.full_symbol = self.symbols[0] o.order_type = OrderType.MARKET o.order_size = - self.current_ewa_size self.place_order(o) self.current_ewa_size = 0 o = OrderEvent() o.full_symbol = self.symbols[1] o.order_type = OrderType.MARKET o.order_size = - self.current_ewc_size self.place_order(o) self.current_ewc_size = 0 elif (spread > 0) and (spread < bollinger_ub) and (self.current_ewa_size > 0): print('spread crosses above average.cover long spread') o = OrderEvent() o.full_symbol = self.symbols[0] o.order_type = OrderType.MARKET o.order_size = - self.current_ewa_size self.place_order(o) self.current_ewa_size = 0 o = OrderEvent() o.full_symbol = self.symbols[1] o.order_type = OrderType.MARKET o.order_size = - self.current_ewc_size self.place_order(o) self.current_ewc_size = 0
from pykalman import KalmanFilter kf = KalmanFilter(n_dim_state=1, n_dim_obs=1) # print y # output=kf.em(y).smooth(normalizedVals)[0] ##print output # plot('kalman', output) # plt.show() # means, covariances = kf.filter(y[0]) import numpy as np means = np.zeros((1, 1)) covariances = np.zeros((1, 1)) output = [] next_mean, next_covariance = kf.filter_update(means[-1], covariances[-1], y[0]) for new_measurement in y[1:]: next_mean, next_covariance = kf.filter_update(next_mean, next_covariance, new_measurement) print next_mean, next_covariance output.append(next_mean[0]) print output plot("Filtered", output) plt.show()
class NeedleTracker(object): def __init__(self): rospy.loginfo("Starting matlab engine...") self.eng = matlab.engine.start_matlab() rospy.loginfo("Done") self.DEBUG = True # needle points in each image self.left_points = None self.right_points = None # used to convert ROS image messages to format usable by opencv self.cv_bridge = cv_bridge.CvBridge() self.left_image = None self.right_image = None self.init_subscribers() self.init_publishers() self.info = {'l': None, 'r': None, 'b': None, 'd': None} if self.DEBUG: self.init_debug_publishers() rospy.loginfo("Initialized needle tracker.") # generate models for needle self.model = models.generate_2d_unit_arc(12, 3.0/8.0, "needle_registration/input/arc_2d") # more densely interpolated model self.dense_model = models.generate_2d_unit_arc(100, 3.0/8.0, "needle_registration/input/arc_2d") # setup kalman filter update self.left_needle_pose = np.zeros(6) self.left_needle_covar = np.identity(6) self.left_kf = KalmanFilter(initial_state_mean=self.left_needle_pose, n_dim_obs=6) self.right_needle_pose = np.zeros(6) self.right_needle_covar = np.identity(6) self.right_kf = KalmanFilter(initial_state_mean=self.left_needle_pose, n_dim_obs=6) # set up exit handler signal.signal(signal.SIGINT, self.exit_handler) def init_subscribers(self): # image subscribers rospy.Subscriber(topics.LEFT_IMAGE, Image, self.left_image_callback, queue_size=1) rospy.Subscriber(topics.RIGHT_IMAGE, Image, self.right_image_callback, queue_size=1) # TODO: move topics to separate file rospy.Subscriber("/AD/left/camera_info", CameraInfo, self.left_info_callback) rospy.Subscriber("/AD/right/camera_info", CameraInfo, self.right_info_callback) def init_publishers(self): # needle pose publishers self.needle_pose_left_pub = rospy.Publisher("/needle_tracker/needle_pose_left", PoseStamped) self.needle_pose_right_pub = rospy.Publisher("/needle_tracker/needle_pose_right", PoseStamped) def init_debug_publishers(self): self.registered_points_pub = rospy.Publisher("/needle_tracker/debug/registered_points", Image) self.color_seg_pub = rospy.Publisher("/needle_tracker/debug/color_seg_pub", Image) self.points_3d_pub = rospy.Publisher('/needle_tracker/debug/points_3d', MarkerArray) def left_info_callback(self, msg): if self.info['l']: return self.info['l'] = msg def right_info_callback(self, msg): if self.info['r']: return self.info['r'] = msg def image_publisher(self, image, image_publisher): img_msg = self.cv_bridge.cv2_to_imgmsg(image) image_publisher.publish(img_msg) def left_image_callback(self, msg): self.left_image = self.cv_bridge.imgmsg_to_cv2(msg, "bgr8") self.left_points = self.process_image(self.left_image) # do not continue until we have points from both images if self.left_points is None or self.right_points is None: return # check the order of points and reverse if necessary if self.left_points[0][0] < self.left_points[-1][0]: self.left_points = self.left_points[::-1] if self.right_points[0][0] < self.right_points[-1][0]: self.right_points = self.right_points[::-1] max_height = int(np.max(self.left_points[:,0])) min_height = int(np.min(self.left_points[:,0])) points_3d = [] for i in range(len(self.left_points)): pt_left = self.left_points[i] pt_right = self.right_points[i] # if abs(pt_left[0] - pt_right[0]) > 20: # continue x = pt_left[0] y = pt_left[1] disparity = y - pt_right[1] pt = Util.convertStereo(y,x,disparity, self.info) points_3d.append(tfx.pose(pt)) # publish 3d point if self.DEBUG: markers = MarkerArray() for i in range(len(points_3d)): point = points_3d[i] marker = Util.createMarker(point.msg.PoseStamped(), id=i+1, lifetime=2) markers.markers.append(marker) self.points_3d_pub.publish(markers) # points_3d = [[a.point.x, a.point.y, a.point.z] for a in points_3d] if points_3d[0].position[0] < points_3d[-1].position[0]: pt_a = points_3d[0].position pt_b = points_3d[1].position pt_c = points_3d[-1].position else: pt_a = points_3d[-1].position pt_b = points_3d[-2].position pt_c = points_3d[0].position x = np.hstack(np.array(pt_a - pt_b)) z = np.hstack(np.array(pt_a - pt_c)) y = np.cross(z,x) z = np.cross(x,y) x /= np.linalg.norm(x) y /= np.linalg.norm(y) z /= np.linalg.norm(z) rotation_matrix = np.transpose(np.array([x,y,z])) obs = tfx.pose(pt_a, rotation_matrix) observation = [obs.position.x, obs.position.y, obs.position.z, obs.tb_angles.yaw_deg, obs.tb_angles.pitch_deg, obs.tb_angles.roll_deg] trans_covar = 0.01*np.eye(6) (self.left_needle_pose, self.left_needle_covar) = self.left_kf.filter_update(self.left_needle_pose, self.left_needle_covar, transition_covariance = trans_covar, observation=observation) self.left_needle_pose = self.left_needle_pose.data rotation = tfx.tb_angles(*(self.left_needle_pose[3:])) position = self.left_needle_pose[:3] needle_pose = tfx.pose(position, rotation) needle_pose.frame = 'left_AD' # TODO: dont' hard code this self.needle_pose_left_pub.publish(needle_pose.msg.PoseStamped()) if points_3d[0].position[0] >= points_3d[-1].position[0]: pt_a = points_3d[0].position pt_b = points_3d[1].position pt_c = points_3d[-1].position else: pt_a = points_3d[-1].position pt_b = points_3d[-2].position pt_c = points_3d[0].position x = np.hstack(np.array(pt_a - pt_b)) z = np.hstack(np.array(pt_a - pt_c)) y = np.cross(z,x) z = np.cross(x,y) x /= np.linalg.norm(x) y /= np.linalg.norm(y) z /= np.linalg.norm(z) rotation_matrix = np.transpose(np.array([x,y,z])) obs = tfx.pose(pt_a, rotation_matrix) observation = [obs.position.x, obs.position.y, obs.position.z, obs.tb_angles.yaw_deg, obs.tb_angles.pitch_deg, obs.tb_angles.roll_deg] trans_covar = 0.01*np.eye(6) (self.right_needle_pose, self.right_needle_covar) = self.right_kf.filter_update(self.right_needle_pose, self.right_needle_covar, transition_covariance = trans_covar, observation=observation) self.right_needle_pose = self.right_needle_pose.data rotation = tfx.tb_angles(*(self.right_needle_pose[3:])) position = self.right_needle_pose[:3] needle_pose = tfx.pose(position, rotation) needle_pose.frame = 'left_AD' # TODO: dont' hard code this self.needle_pose_right_pub.publish(needle_pose.msg.PoseStamped()) def right_image_callback(self, msg): self.right_image = self.cv_bridge.imgmsg_to_cv2(msg, "bgr8") if self.right_points is None: self.right_points = self.process_image(self.right_image) def process_image(self, image): return self.get_needle_points(image) def get_needle_points(self, image): """ Returns points along an ellipse that lines up the needle in the image """ erosion_kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3,3)) image = cv2.erode(image, erosion_kernel) image = cv2.erode(image, erosion_kernel) # image = cv2.erode(image, erosion_kernel) # image = cv2.dilate(image, erosion_kernel) # image = cv2.dilate(image, erosion_kernel) # segment the needle from the background hsv_image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) # segment needle based on its hue segmented_image = cv2.inRange(hsv_image, np.array([20,90,50]),np.array([40,180,255])) # publish segmented image if self.DEBUG: color_image = cv2.cvtColor(segmented_image,cv2.COLOR_GRAY2RGB) self.image_publisher(color_image, self.color_seg_pub) # create a file containing all 2d positions of segemented points points = np.transpose(np.nonzero(segmented_image)) # np.savetxt("needle_registration/input/needle_seg.txt", points, delimiter=" ") # result = gmmreg._core.run_ini("needle_registration/needle.ini") # after_tps = result[2] scene = matlab.double(points.tolist()) model = matlab.double(self.model.tolist()) result = self.eng.get_cpd_affine(model, scene) # affine transformation A*x+b A = np.array(result['R']) b = np.array(result['t']) x = np.transpose(self.dense_model) transformed_pts = np.transpose(np.dot(A, x) + b) # draw ellipse if self.DEBUG: color_image = image.copy() for coord in np.asarray(transformed_pts, dtype=np.uint32): coord = tuple(coord)[::-1] # color_image[coord] = np.array([0, 0, 255]) cv2.circle(color_image, coord, 1, [0,0,255]) self.image_publisher(color_image, self.registered_points_pub) return transformed_pts def exit_handler(self, signal, frame): rospy.loginfo("Shutting down needle tracking node...") sys.exit(0)
print '' #t = 0 mu = mu[-1] sig = sig[-1] for i in range(2, len(measurements)): # starting with third measurement... if rospy.is_shutdown(): # easy interruption break t = i * 1/10. #t += 1/10. measurement = measurements[i] #print measurement #print mu #print sig mu, sig = kf.filter_update(mu, sig, observation=measurement) #print mu #print sig #print '' image_temp = image.copy() cv2.circle(image_temp, (measurement.data[0], measurement.data[1]), 2, (255,0,0), -1) cv2.circle(image_temp, (int(mu[0]), int(mu[1])), 5, (0,0,255)) spread = numpy.sqrt(sig.diagonal()) * 10 cv2.rectangle(image_temp, (int(mu[0] - spread[0]), int(mu[1] - spread[1])), (int(mu[0] + spread[0]), int(mu[1] + spread[1])), (0,0,255), 2) cv2.imshow('2D Position & Variance', image_temp) cv2.waitKey(1000/100) # display at 100Hz
n_dim_state = 2#kf.transition_matrix.shape[0] filtered_state_means = np.zeros((n_timesteps, n_dim_state)) filtered_state_covariances = np.zeros((n_timesteps, n_dim_state, n_dim_state)) #filtered_state_means=measurementsRT #filtered_state_covariances=measurementsRT for t in range(n_timesteps - 1): measurement=readbgblob() measurementsRT[t,:]=measurement if t == 0: filtered_state_means[t,] = measurements[i,]#kf.initial_state_mean filtered_state_covariances[t,] = [[70,70],[70,70]]#kf.initial_state_covariance filtered_state_means[t + 1,], filtered_state_covariances[t + 1,] = ( kf.filter_update( filtered_state_means[t,], filtered_state_covariances[t,], measurementsRT[t,]#, #transition_offset=kf.transition_offsets[0], # originally data.transition_offsets[t] #observation_offset=kf.observation_offsets ) ) print filtered_state_means[t+1] print measurementsRT[t] #%% draw estimates pl.figure() lines_true = pl.plot(measurementsRT[:,0],measurementsRT[:,1], '-*',color='b') lines_filt = pl.plot(filtered_state_means[:,0],filtered_state_means[:,1], color='r') pl.legend((lines_true[0], lines_filt[0]), ('measured', 'filtered')) pl.show() #%% sockets for OE
def main(): #Paramètres du modèle #Indiquer à quoi correspond chaque variable osigma=0.1; transition_matrix = np.array([[1., 0.,0.,0.,0.,0.],[1., 1.,0.,0.,0.,0.],[0.,0,1,0.,0.,0.]]) transition_covariance = np.zeros((6,6)); observation_matrix = np.array([[0., 1.,0.],[0., 0.,1.]]) observation_covariance = np.eye(2)*osigma initial_state_mean = np.array([1,0,10]) # m(0) initial_state_covariance = np.eye(3);# p(0) #Observations observations = csv.reader(open('voitureObservations.csv','r'),delimiter=' ') # Filtrage à la main # Quels sont les paramètres du constructeur ? kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, ) # Que conserverons les variables suivantes ? hand_state_estimates=[initial_state_mean] hand_state_cov_estimates=[initial_state_covariance] for anObs in observations: # Quelles étapes du filtrage sont réalisées par la ligne suivante (aMean,aCov) = kf.filter_update(hand_state_estimates[-1],hand_state_cov_estimates[-1],anObs) hand_state_estimates.append(aMean) hand_state_cov_estimates.append(aCov) hand_state_estimates=np.array(hand_state_estimates) # A quoi sert la ligne suivante ? hand_positions=np.dot(hand_state_estimates,observation_matrix.T ) # obtenir les observation (H*Mk-1 dans le cours, rouge) projet mon espace etat ver mon espace observation plt.figure(1) plt.plot(observations[:,0],observations[:,1], 'r+') plt.plot(hand_positions[:,0],hand_positions[:,1], 'b') plt.savefig('illustration_filtrage_main_voiture.pdf') plt.close() # Filtrage complet # Que fait cette section ? # Quels sont les paramètres supplémentaires donnés au constructeur ? # Quels sont les autres paramètres possibles ? kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, initial_state_mean=initial_state_mean, initial_state_covariance=initial_state_covariance, ) (filtered_state_estimates,filtered_state_cov_estimates) = kf.filter(observations) filtered_positions=np.dot(filtered_state_estimates,observation_matrix.T ) plt.figure(1) plt.plot(observations[:,0],observations[:,1], 'r+') plt.plot(filtered_positions[:,0],filtered_positions[:,1], 'b') plt.savefig('illustration_filtrage_voiture.pdf') plt.close() # Lissage # Que fait cette section ? # Quel est la différence avec le filtrage ? # Puis-je faire un lissage "à la main" observation par observation ? kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, initial_state_mean=initial_state_mean, initial_state_covariance=initial_state_covariance, ) (smoothed_state_estimates,smoothed_state_cov_estimates) = kf.smooth(observations) # au lieu de filtre, on appel smooth smoothed_positions=np.dot(smoothed_state_estimates,observation_matrix.T ) plt.figure(2) plt.plot(observations[:,0],observations[:,1], 'r+') plt.plot(smoothed_positions[:,0],smoothed_positions[:,1], 'b') plt.savefig('illustration_lissage_voiture.pdf') plt.close()
data.initial_state_covariance, random_state=0 ) # Estimate mean and covariance of hidden state distribution iteratively. This # is equivalent to # # >>> (filter_state_means, filtered_state_covariance) = kf.filter(data) n_timesteps = data.observations.shape[0] n_dim_state = data.transition_matrix.shape[0] filtered_state_means = np.zeros((n_timesteps, n_dim_state)) filtered_state_covariances = np.zeros((n_timesteps, n_dim_state, n_dim_state)) for t in range(n_timesteps - 1): if t == 0: filtered_state_means[t] = data.initial_state_mean filtered_state_covariances[t] = data.initial_state_covariance filtered_state_means[t + 1], filtered_state_covariances[t + 1] = ( kf.filter_update( filtered_state_means[t], filtered_state_covariances[t], data.observations[t + 1], transition_offset=data.transition_offsets[t], ) ) # draw estimates pk.figure() lines_true = pk.plot(data.states, color='b') lines_filt = pk.plot(filtered_state_means, color='r') pk.legend((lines_true[0], lines_filt[0]), ('true', 'filtered')) pk.show()
# spread_std = np.sqrt(observation_matrix_stepwise.dot(P).dot(observation_matrix_stepwise.transpose())[0][0] + observation_cov) # print(spread, spread_std) state_means_stepwise, state_covs_stepwise = kf.filter(observation_stepwise) # depend on y # print(state_means_stepwise, state_covs_stepwise) means_trace.append(state_means_stepwise[0]) covs_trace.append(state_covs_stepwise[0]) for step in range(1, data.shape[0]): # print(step) x = data[sym_a][step] y = data[sym_b][step] observation_matrix_stepwise = np.array([[x, 1]]) observation_stepwise = y state_means_stepwise, state_covs_stepwise = kf.filter_update( means_trace[-1], covs_trace[-1], observation=observation_stepwise, observation_matrix=observation_matrix_stepwise) # print(state_means_stepwise, state_covs_stepwise) # P = covs_trace[-1] + np.eye(2)*state_cov_multiplier # This has to be small enough # spread = y - observation_matrix_stepwise.dot(means_trace[-1])[0] # spread_std = np.sqrt(observation_matrix_stepwise.dot(P).dot(observation_matrix_stepwise.transpose())[0][0] + observation_cov) # print(spread, spread_std) means_trace.append(state_means_stepwise.data) covs_trace.append(state_covs_stepwise) # ------------------------------------ line evolvement --------------------------------------------# # colormap cm = plt.get_cmap('jet') colors = np.linspace(0.1, 1, data.shape[0]) sc = plt.scatter(data[sym_a], data[sym_b], s=10, c=colors, cmap=cm, edgecolors='k', alpha=0.7)