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]
def calc_slope_intercept_using_kalman_filter(etfs, prices): """ Utilise the Kalman Filter from the pyKalman package to calculate the slope and intercept of the regressed ETF prices. """ delta = 1e-5 trans_cov = delta / (1 - delta) * np.eye(2) obs_mat = np.vstack( [prices[etfs[0]], np.ones(prices[etfs[0]].shape)] ).T[:, np.newaxis] 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 = kf.filter(prices[etfs[1]].values) return state_means, state_covs
def test_kalman_pickle(): 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, em_vars='all') # train and get log likelihood X = data.observations[0:10] kf = kf.em(X, n_iter=5) loglikelihood = kf.loglikelihood(X) # pickle Kalman Filter store = StringIO() pickle.dump(kf, store) clf = pickle.load(StringIO(store.getvalue())) # check that parameters came out already np.testing.assert_almost_equal(loglikelihood, kf.loglikelihood(X)) # store it as BytesIO as well store = BytesIO() pickle.dump(kf, store) kf = pickle.load(BytesIO(store.getvalue()))
def main(): with open(sys.argv[1], "r") as fin: tracks = cPickle.load(fin) print "%d tracks loaded."%len(tracks) index = 5 measurements = [] track = tracks[index] t0 = track.utm[0][2]/1e6 for pt in track.utm: t = pt[2]/1e6 - t0 measurements.append([pt[0], pt[1]]) measurements = np.asarray(measurements) kf = KalmanFilter(n_dim_obs=2, n_dim_state=2).em(measurements, n_iter=100) results = kf.smooth(measurements)[0] fig = plt.figure(figsize=(9,9)) ax = fig.add_subplot(111, aspect='equal') ax.plot([pt[0] for pt in results], [pt[1] for pt in results], 'r-', linewidth=2) ax.plot([pt[0] for pt in track.utm], [pt[1] for pt in track.utm], 'x-') #ax.set_xlim([const.SF_small_RANGE_SW[0], const.SF_small_RANGE_NE[0]]) #ax.set_ylim([const.SF_small_RANGE_SW[1], const.SF_small_RANGE_NE[1]]) plt.show()
def run_kal(measurements, training_size=40): # count the number of measurements num_measurements = len(measurements) # find the dimension of each row dim = len(measurements[0]) if dim == 3: simple_mode = True elif dim == 9: simple_mode = False else: print "Error: Dimensions for run_kal must be 3 or 9" exit(1) training_set = [] for i in range(min(training_size,len(measurements))): training_set.append(measurements[i]) if simple_mode: # run the filter kf = KalmanFilter(initial_state_mean=[0,0,0], n_dim_obs=3) (smoothed_state_means, smoothed_state_covariances) = kf.em(training_set).smooth(measurements) else: kf = KalmanFilter(initial_state_mean=[0,0,0,0,0,0,0,0,0], n_dim_obs=9) (smoothed_state_means, smoothed_state_covariances) = kf.em(training_set).smooth(measurements) # means represent corrected points return smoothed_state_means, smoothed_state_covariances, simple_mode
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)
def onEnd(self): print("Launching Figures for Estimator") #print(self.xr) #print(self.yr) kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]), transition_covariance=0.01 * np.eye(2)) self.states_pred = kf.em(self.yr).smooth(self.yr)[0] self.signalr = pl.scatter(self.xr, self.yr, linestyle='-', marker='o', color='b', label='Data Received from Sensor') self.position_line = pl.plot(self.xr,self.states_pred[:, 0], linestyle='-', marker='o', color='g', label='Data Estimated from Sensor') self.position_error = pl.plot(self.xr, (self.yr-self.states_pred[:, 0]), linestyle='-', marker='o', color='m', label='Relative Estimation Error') pl.legend(loc='upper right') pl.xlim(xmin=0, xmax=self.counterr) pl.xlabel('time[s]') pl.ylabel('Position [m]') pl.ylim(ymin=-(A+0.25), ymax=(A+0.25)) pl.show() # First, form the AMS AID to inform start self.informAMSr = spade.AID.aid(name="ams.127.0.0.1", addresses=["xmpp://ams.127.0.0.1"]) # Second, build the message self.msg = spade.ACLMessage.ACLMessage() # Instantiate the message self.msg.setPerformative("inform") # Set the "inform" FIPA performative self.msg.setOntology("Estimation") # Set the ontology of the message content self.msg.setLanguage("OWL-S") # Set the language of the message content self.msg.addReceiver(self.informAMSr) # Add the message receiver self.msg.setContent("End of Reception") # Set the message content print(self.msg.content) # Third, send the message with the "send" method of the agent self.myAgent.send(self.msg)
def on_update(self, data): result = {} result["timestamp"] = data["timestamp"] if self.input.size() >= self.length: independent_var = self.input.get_by_idx_range(key=None, start_idx=0, end_idx=-1) symbol_set = set(self.input.keys) depend_symbol = symbol_set.difference(self.input.default_key) depend_var = self.input.get_by_idx_range(key=depend_symbol, start_idx=0, end_idx=-1) obs_mat = np.vstack([independent_var.values, np.ones(independent_var.values.shape)]).T[:, np.newaxis] model = 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=self.trans_cov, ) state_means, state_covs = model.filter(depend_var.values) slope = state_means[:, 0][-1] result[Indicator.VALUE] = slope result[KalmanFilteringPairRegression.SLOPE] = slope result[KalmanFilteringPairRegression.SLOPE] = state_means[:, 1][-1] self.add(result) else: result[Indicator.VALUE] = np.nan result[KalmanFilteringPairRegression.SLOPE] = np.nan result[KalmanFilteringPairRegression.SLOPE] = np.nan self.add(result)
def KFilt(sample,fs=25): """Input (sample) is fill_in_data output. Outputs two lists of color data """ #kalman filter inputs # Dimensions of parameters: # 'transition_matrices': 2, # 'transition_offsets': 1, # 'observation_matrices': 2, # 'observation_offsets': 1, # 'transition_covariance': 2, # 'observation_covariance': 2, # 'initial_state_mean': 1, # 'initial_state_covariance': 2, n_timesteps = len(sample) trans_mat = [] #mask missing values observations = np.ma.array(sample,mask=np.zeros(sample.shape)) missing_loc = np.where(np.isnan(sample)) observations[missing_loc[0][:],missing_loc[1][:]] = np.ma.masked #Import Kalman filter, inerpolate missing points and get 2nd, 3rd orde kinematics dt = 1./25 #Length of each frame (should be iether 1/25 or 1/30) n_timesteps = len(sample) observation_matrix = np.array([[1,0,0,0], [0,1,0,0]])#np.eye(4) t = np.linspace(0,len(observations)*dt,len(observations)) q = np.cov(observations.T[:2,:400]) qdot = np.cov(np.diff(observations.T[:2,:400]))#np.cov(observations[:1,:400]) h=(t[-1]-t[0])/t.shape[0] A=np.array([[1,0,h,.5*h**2], [0,1,0,h], [0,0,1,0], [0,0,0,1]]) init_mean = [sample[0],0,0] #initial mean should be close to the first point, esp if first point is human-picked and tracking starts at the beginning of a video observation_covariance = q*500 #ADJUST THIS TO CHANGE SMOOTHNESS OF FILTER init_cov = np.eye(4)*.001#*0.0026 transition_matrix = A transition_covariance = np.array([[q[0,0],q[0,1],0,0], [q[1,0],q[1,1],0,0], [0,0,qdot[0,0],qdot[0,1]], [0,0,qdot[1,0],qdot[1,1]]]) kf = KalmanFilter(transition_matrix, observation_matrix,transition_covariance,observation_covariance,n_dim_obs=2) kf = kf.em(observations,n_iter=1,em_vars=['transition_covariance','transition_matrix','observation_covariance']) #pdb.set_trace() global trans_mat, trans_cov, init_cond x_filt = kf.filter(observations[0])[0]#observations.T[0])[0] kf_means = kf.smooth(observations[0])[0] return kf_means,x_filt #np.column_stack((color_x[:,0],color_y[:,0],color_x[:,1],color_y[:,1])),frames
def smooth(dr): '''Smoothing with Kalman Filter''' kf = KalmanFilter() for t in range(1,201): if not os.path.exists('drivers/%d/%d_smooth.csv'%(dr,t)): d = np.genfromtxt('drivers/%d/%d.csv'%(dr,t), delimiter=',', skip_header=True) d[:,0] = kf.smooth(d[:,0])[0].T[0] d[:,1] = kf.smooth(d[:,1])[0].T[0] np.savetxt('drivers/%d/%d_smooth.csv'%(dr,t), d, delimiter=',', header='x,y', fmt='%.1f')
def kalman_smooth(observations, **kwargs): ''' Smooth shit ''' kwargs.setdefault('initial_state_mean', BASE_SCORE) kwargs.setdefault('transition_covariance', 0.01 * np.eye(1)) kf = KalmanFilter(**kwargs) states_pred = kf.smooth(observations)[0] return states_pred[:, 0]
def df_kalman(self): """ Smooths trip using Kalman method * https://github.com/pykalman/pykalman * http://pykalman.github.io * https://ru.wikipedia.org/wiki/Фильтр_Калмана * http://bit.ly/1Dd1bhn """ df = self.trip_data.copy() df['_v_'] = self.distance(self.trip_data, '_x_', '_y_') df['_a_'] = df._v_.diff() # Маскируем ошибочные точки df._x_ = np.where( (df._a_ > MIN_A) & (df._a_ < MAX_A), df._x_, np.ma.masked) df._y_ = np.where( (df._a_ > MIN_A) & (df._a_ < MAX_A), df._y_, np.ma.masked) df['_vx_'] = df._x_.diff() df['_vy_'] = df._y_.diff() # Параметры физической модели dx = v * dt 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]] xinit, yinit = df._x_[0], df._y_[0] vxinit, vyinit = df._vx_[1], df._vy_[1] initcovariance = 1.0e-4 * np.eye(4) transistionCov = 1.0e-3 * np.eye(4) observationCov = 1.0e-2 * np.eye(2) # Фильтр Калмана kfilter = KalmanFilter( transition_matrices=transition_matrix, observation_matrices=observation_matrix, initial_state_mean=[xinit, yinit, vxinit, vyinit], initial_state_covariance=initcovariance, transition_covariance=transistionCov, observation_covariance=observationCov ) measurements = df[['_x_', '_y_']].values kfilter = kfilter.em(measurements, n_iter=5) (state_means, state_covariances) = kfilter.smooth(measurements) kdf = pd.DataFrame(state_means, columns=('x', 'y', 'vx', 'vy')) kdf['v'] = np.sqrt(np.square(kdf.vx) + np.square(kdf.vy)) self.trip_data[['x', 'y', 'v']] = kdf[['x', 'y', 'v']]
def test_kalman_fit(): # check against MATLAB dataset kf = KalmanFilter( data.transition_matrix, data.observation_matrix, data.initial_transition_covariance, data.initial_observation_covariance, data.transition_offsets, data.observation_offset, data.initial_state_mean, data.initial_state_covariance, em_vars=['transition_covariance', 'observation_covariance']) loglikelihoods = np.zeros(5) for i in range(len(loglikelihoods)): loglikelihoods[i] = kf.loglikelihood(data.observations) kf.em(X=data.observations, n_iter=1) assert_true(np.allclose(loglikelihoods, data.loglikelihoods[:5])) # check that EM for all parameters is working kf.em_vars = 'all' n_timesteps = 30 for i in range(len(loglikelihoods)): kf.em(X=data.observations[0:n_timesteps], n_iter=1) loglikelihoods[i] = kf.loglikelihood(data.observations[0:n_timesteps]) for i in range(len(loglikelihoods) - 1): assert_true(loglikelihoods[i] < loglikelihoods[i + 1])
def test_kalman_sampling(): 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) (x, z) = kf.sample(100) assert_true(x.shape == (100, data.transition_matrix.shape[0])) assert_true(z.shape == (100, data.observation_matrix.shape[0]))
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 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 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]
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 __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
def __init__(self, df, dependent=None, independent=None, delta=None, trans_cov=None, obs_cov=None): if not dependent: dependent = df.columns[1] if not independent: independent = df.columns[2] self.x = df[independent] self.x.index = df.index self.y = df[dependent] self.y.index = df.index self.dependent = dependent self.independent = independent self.delta = delta or 1e-5 self.trans_cov = trans_cov or self.delta / (1 - self.delta) * np.eye(2) self.obs_mat = np.expand_dims( np.vstack([[self.x.values], [np.ones(len(self.x))]]).T, axis=1 ) self.obs_cov = obs_cov or 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=self.obs_mat, observation_covariance=self.obs_cov, transition_covariance=self.trans_cov) self.state_means, self.state_covs = self.kf.filter(self.y.values)
def pkm(): #https://pykalman.github.io #READ Basic Usage print("Beginning Kalman Filtering....") kf = KalmanFilter(initial_state_mean=0, n_dim_obs=9) #measurements - array of 9x1 vals #each a_x, a_y, a_z, g_x, g_y, g_z, m_x, m_y, m_z measurements = getMeasurementsFromDB() smooth_inputs = [[2,0], [2,1], [2,2]] #traditional assumed params are known, but we can get from EM on the measurements #we get better predictions of hidden states (true position) with smooth function kf.em(measurements).smooth(smooth_inputs) print(measurements) print(kf.em(measurements, n_iter=5))
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]]), )
def _KalmanFilterRegression( self ): """ Use Kalman Filter to obtain first-order auto-regression parameters r_t = beta_0 + beta_1 * r_(t-1) """ returns = self.returns; _trans_cov_delta = 1e-3; # Transition matrix and covariance trans_mat = np.eye(2); # Assume beta is not to change from t-1 to t _delta = _trans_cov_delta; trans_cov = _delta / (1 - _delta) * np.eye(2); # This _delta and trans_cov seem to have great impact on the result # form Observation Matrix data = returns.values[:-1,:]; _, num_stocks = data.shape; data = np.expand_dims( data, axis = 2 ); # T-by-2-by-1 array obs_mat = np.insert( data, 1, 1, axis = 2 ); # Insert column of ones T-2-2 array obs_cov = np.eye( num_stocks ); # assume zero correlation among noises in observed stock returns #print "Shape of observation matrix is ", obs_mat.shape; #print "Example of obs_mat is ", obs_mat[:2,:,:]; # Observed stock returns: r_t index = returns.index[1:]; # index for beta_1(t) observations = returns.values[1:,:] # matrix of r_t # Form Kalman Filter and then filter! kf = KalmanFilter( n_dim_obs = num_stocks, n_dim_state = 2, # 2 regression parameters initial_state_mean = np.zeros(2), initial_state_covariance = np.ones((2,2)), transition_matrices = trans_mat, transition_covariance = trans_cov, observation_matrices = obs_mat, observation_covariance = obs_cov, ); state_means, state_covs = kf.filter( observations ); slope = pd.Series( state_means[:,0], index ); intercept = pd.Series( state_means[:,1], index ); kf_coefficients_df = pd.DataFrame( [ intercept, slope ], index = [ "coeff_0", "coeff_1" ] ); self.kf_coefficients_df = kf_coefficients_df.transpose(); return (intercept, slope);
def check_dims(n_dim_state, n_dim_obs, kwargs): kf = KalmanFilter(**kwargs) (transition_matrices, transition_offsets, transition_covariance, observation_matrices, observation_offsets, observation_covariance, initial_state_mean, initial_state_covariance) = ( kf._initialize_parameters() ) assert_true(transition_matrices.shape == (n_dim_state, n_dim_state)) assert_true(transition_offsets.shape == (n_dim_state,)) assert_true(transition_covariance.shape == (n_dim_state, n_dim_state)) assert_true(observation_matrices.shape == (n_dim_obs, n_dim_state)) assert_true(observation_offsets.shape == (n_dim_obs,)) assert_true(observation_covariance.shape == (n_dim_obs, n_dim_obs)) assert_true(initial_state_mean.shape == (n_dim_state,)) assert_true( initial_state_covariance.shape == (n_dim_state, n_dim_state) )
def test_kalman_predict(): 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) x_smooth = kf.smooth(X=data.observations)[0] assert_array_almost_equal( x_smooth[:501], data.smoothed_state_means[:501], decimal=7 )
def weigh_data_point(self, map_number): """ Determine a point's weight """ # Look @ how many points on that X,Y spot through various Z positions # If most recent positions show obstacle, probably obstacle #for range(map_number-5, map_number): # Find X, Y for that map number... # pass # Load up pickles X, Y, Z = self.get_pickles() # Perform Kalman filters # Initial state = 0 because we're starting off at coords 0,0 kf = KalmanFilter(initial_state_mean = 0, n_dim_obs=2) measurements = kf.em(measurements.smooth(measurements)) # TODO: Deal w/ updating locations # means, covars = self.update_data_points(measurements) # measurements = means # Return data points return measurements
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__(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 KFilt(sample): """Input (sample) is fill_in_data output. Outputs two lists of color data """ #kalman filter inputs n_timesteps = len(sample) trans_mat = [] trans_cov = [] init_cond = [],[] #TODO: set up specific parameters (observation matrix, etc) #mask missing values observations = np.ma.array(sample,mask=np.zeros(sample.shape)) missing_loc = np.where(np.isnan(sample)) observations[missing_loc[0][:],missing_loc[1][:]] = np.ma.masked #Import Kalman filter, inerpolate missing points and get 2nd, 3rd orde kinematics dt = 1./25 #Length of each frame (should be iether 1/25 or 1/30) #trans_mat = np.array([[1, 0 ,1, 0],[0, 1, 0, 1],[0,0,1,0],[0,0,0,1]]) #trans_cov = 0.01*np.eye(4) if not trans_mat: #if there's not a global variable defining tranisiton matrices and covariance, make 'em and optimize trans_mat = np.array([[1,1],[0,1]]) trans_cov = 0.01*np.eye(2) kf = KalmanFilter(transition_matrices = trans_mat, transition_covariance=trans_cov) kf = kf.em(observations.T[0],n_iter=5) #optimize parameters trans_mat = kf.transition_matrices trans_cov = kf.transition_covariance init_mean = kf.initial_state_mean init_cov = kf.initial_state_covariance else: kf = KalmanFilter(transition_matrices = trans_mat, transition_covariance=trans_cov,\ initial_state_mean = init_mean,initial_state_covariance = init_cov) global trans_mat, trans_cov, init_cond color_x = kf.smooth(observations.T[0])[0] color_y = kf.smooth(observations.T[1])[0] return color_x,color_y #np.column_stack((color_x[:,0],color_y[:,0],color_x[:,1],color_y[:,1])),frames
def makeKalman(self, pts): 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]] xinit=pts[0,0] yinit=pts[0,1] vxinit=pts[1,0]-pts[0,0] vyinit=pts[1,1]-pts[0,1] initstate=[xinit,yinit,vxinit,vyinit] initcovariance=1.0e-3*np.eye(4) transistionCov=1.0e-4*np.eye(4) observationCov=1.0e-1*np.eye(2) self.kf=KalmanFilter(transition_matrices=Transition_Matrix, observation_matrices =Observation_Matrix, initial_state_mean=initstate, initial_state_covariance=initcovariance, transition_covariance=transistionCov, observation_covariance=observationCov) self.means, self.covariances = self.kf.filter(pts) self.v = self.means[-1][2:]
def plot_with_kalman_smoothing(cpu_data): kalman_data = cpu_data[['temperature', 'cpu_percent', 'sys_load_1']] # Preparing Kalman filter parameters initial_state = kalman_data.iloc[0] observation_covariance = np.diag([0.5, 0.5, 0.5]) ** 2 transition_covariance = np.diag([0.2, 0.2, 0.2]) ** 2 transition = [[1, -1, 0.7], [0, 0.6, 0.03], [0, 1.3, 0.8]] # Creating Kalman filter kf = KalmanFilter( initial_state_mean=initial_state, initial_state_covariance=observation_covariance, observation_covariance=observation_covariance, transition_covariance=transition_covariance, transition_matrices=transition ) kalman_smoothed, _ = kf.smooth(kalman_data) plt.plot(cpu_data['timestamp'], kalman_smoothed[:, 0], 'g-')
def getPredictedValuesVer2(measurements, standard_deviation = None): #this method use PyKalman module #Apply this method only for Gains. if standard_deviation == None: #if standard deviation is not specified, then get it only from measurements list standard_deviation = getStandardDeviation(measurements) #adapt the transition_covariance. Temporary if standard_deviation < 1e-9: transition_covariance = 1e-21 else: transition_covariance = 5e-17 kf = KalmanFilter(transition_matrices = [1], observation_matrices = [1], transition_covariance = transition_covariance, observation_covariance = math.pow(standard_deviation, 2)) tmp= kf.filter(measurements)[0].tolist() for i in range(0, len(tmp)): tmp[i] = tmp[i][0] return tmp
def Spread_KalmanFilterRegression(df,pair1,pair2): x=KalmanFilterAverage(df[pair1]) y=KalmanFilterAverage(df[pair2]) delta = 1e-3 trans_cov = delta / (1 - delta) * np.eye(2) # How much random walk wiggles obs_mat = np.expand_dims(np.vstack([[x], [np.ones(len(x))]]).T, axis=1) kf = KalmanFilter(n_dim_obs=1, n_dim_state=2, # y is 1-dimensional, (alpha, beta) is 2-dimensional initial_state_mean=[0,0], initial_state_covariance=np.ones((2, 2)), transition_matrices=np.eye(2), observation_matrices=obs_mat, observation_covariance=2, transition_covariance=trans_cov) # Get running estimates and errors for the state parameters state_means, state_covs = kf.filter(y.values) #Build the spread spread=pd.DataFrame() spread['{}_{}'.format(pair1,pair2)]=df[pair1]-state_means[:,0]*df[pair2] spread.dropna(inplace=True) return(spread)
def kalman_filter(data_1, niter): kalman_param = np.zeros((data_1.shape[0], 2)) kalman_smooth = np.zeros((data_1.shape[0], data_1.shape[1])) """ EM algorithm is slow for Kalman initialization""" for i in range(data_1.shape[0]): kf = KalmanFilter(em_vars=[ 'transition_matrices', 'observation_matrices', 'transition_covariance', 'observation_covariance', 'observation_offsets', 'initial_state_mean', 'initial_state_covariance' ]) kf = kf.em(X=data_1[i], n_iter=niter) kalman_smooth[i, ] = np.ravel(kf.smooth(data_1[i])[0]) kalman_param[i, 0] = np.max(kalman_smooth[i, ]) kalman_param[i, 1] = np.std(kalman_smooth[i, ]) """ Eigen Value for correlated smoothed series """ eigen_corr, t = np.linalg.eig(np.corrcoef(kalman_smooth)) eigen_corr.sort() eigen_corr = eigen_corr[::-1] return eigen_corr, kalman_param
def calc_slope_intercept_kalman(etfs, prices): """ Utilise the Kalman Filter from the PyKalman package to calculate the slope and intercept of the regressed ETF prices. """ delta = 1e-5 trans_cov = delta / (1 - delta) * np.eye(2) obs_mat = np.vstack([prices[etfs[0]], np.ones(prices[etfs[0]].shape)]).T[:, np.newaxis] 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 = kf.filter(prices[etfs[1]].values) return state_means, state_covs
def init_filter(self): random_state = np.random.RandomState(0) transition_matrix = [[1, 0.1], [0, 1]] transition_offset = [-0.1, 0.1] observation_matrix = np.eye(2) + random_state.randn(2, 2) * 0.1 observation_offset = [1.0, -1.0] transition_covariance = np.eye(2) observation_covariance = np.eye(2) + random_state.randn(2, 2) * 0.1 initial_state_mean = [5, -5] initial_state_covariance = [[1, 0.1], [-0.1, 1]] # sample from model self.kf = KalmanFilter(transition_matrix, observation_matrix, transition_covariance, observation_covariance, transition_offset, observation_offset, initial_state_mean, initial_state_covariance, random_state=random_state)
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 kalman(x, n_dim_state): #parameter: time series, kf = KalmanFilter(n_dim_state=n_dim_state, n_dim_obs=x.shape[0]) x = x.T #loglikelihoods = range(1, 51) #estimates = np.ones(50) #for i in range(len(loglikelihoods)): # # kf = kf.em(X=x, n_iter= 1, em_vars = 'all') # estimates[i] = kf.loglikelihood(x) # print(i) converged = False tol = 1e-8 LL = [] i = 0 while converged == False: kf = kf.em(X=x, n_iter=1, em_vars='all') LL.append(kf.loglikelihood(x)) LLold = LL[i] if i <= 2: LLbase = LL[i] elif ((LL[i] - LLbase) < (1 + tol) * (LLold - LLbase)): converged = True i = i + 1 return i - 1
def KF_filter(full_data): data_i=[] filter_i=[] for i in range(16): data_i.append(full_data[:,i*3:(i+1)*3]) for d_i in data_i: d=np.asarray(d_i) a=1/50 A=np.asarray([[1, 0, 0, a, 0, 0, 0.5*(a**2), 0, 0], [0, 1, 0, 0, a, 0, 0, 0.5*(a**2), 0], [0, 0, 1, 0, 0, a, 0, 0, 0.5*(a**2)], [0, 0, 0, 1, 0 ,0, a, 0, 0], [0, 0, 0, 0, 1, 0, 0, a, 0], [0, 0, 0, 0, 0, 1, 0, 0, a], [0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1]]) C=C = np.asarray([ [1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0]]) R=np.identity(3,dtype=int) Q=np.identity(9,dtype=int) velocity=np.gradient(d)[1] acceleration=np.gradient(velocity)[1] tot=np.concatenate((d,velocity,acceleration),axis=1) mean=np.mean(tot,axis=0) covariance=np.cov(tot,rowvar = False) kf = KalmanFilter(transition_matrices = A, observation_matrices = C, transition_covariance = Q, observation_covariance = R, initial_state_mean = mean, initial_state_covariance = covariance) filter=kf.filter(d)[0] filter_i.append(filter[:,:3]) return np.concatenate(filter_i,axis=1)
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 KalmanFilterRegression(x, y): delta = 1e-3 trans_cov = delta / (1 - delta) * np.eye( 2) # How much random walk wiggles obs_mat = np.expand_dims(np.vstack([[x], [np.ones(len(x))]]).T, axis=1) kf = KalmanFilter( n_dim_obs=1, n_dim_state=2, # y is 1-dimensional, (alpha, beta) is 2-dimensional initial_state_mean=[0, 0], initial_state_covariance=np.ones((2, 2)), transition_matrices=np.eye(2), observation_matrices=obs_mat, observation_covariance=2, transition_covariance=trans_cov) # Use the observations y to get running estimates and errors for the state parameters state_means, state_covs = kf.filter(y.values) slope = state_means[:, 0] intercept = state_means[:, 1] std_q = np.array([np.sqrt(cov[0][0]) for cov in state_covs]) return slope, intercept, std_q
def hedge_ratio(Y, X): """ Use the Kalman Filter from the pyKalman package to calculate the slope and intercept of the regressed prices """ delta = 1e-5 # To Optimize trans_cov = delta / (1 - delta) * np.eye(2) obs_mat = np.vstack([X, np.ones(X.shape)]).T[:, np.newaxis] 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=2.0, transition_covariance=trans_cov) state_means, state_covs = kf.filter(Y) return state_means[:, 1]
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 kalman_filter(data, error): data = np.ma.masked_less(data, -900) 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]] xinit = data[0, 0] yinit = data[0, 1] vxinit = data[1, 0] - data[0, 0] vyinit = data[1, 1] - data[0, 1] initstate = [xinit, yinit, vxinit, vyinit] initcovariance = error * np.eye(4) transistionCov = error * np.eye(4) observationCov = error * 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) (filtered_state_means, _) = kf.filter(data) return np.array(filtered_state_means)
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, # H observation_covariance=self.observation_covariance, # Q )
def setup_kf(imean, a=[[1, 0, 0.5, 0], [0, 1, 0, 0.5], [0, 0, 1, 0], [0, 0, 0, 1]], o=[[1, 0, 0, 0], [0, 1, 0, 0]]): """ Initialize Kalman filter object for each new tracks. The transfermation matrix (a) and observation matrix (o) can be tuned to better suit with a specific motion pattern, but to preserve generality we are using a simple constant speed model here. Args: imean (2x1 array): 1x2 array or list of the location of centroid. a (array): transformation matrix that governs state transition to the next time step. Size varies with model. o (array): observation matrix that defines the observable states. Size varies with model. """ return KalmanFilter(transition_matrices=a, observation_matrices=o, initial_state_mean=[*imean,0,0])
def smooth_measurements(measurements): measurements = np.asarray(measurements) initial_state_mean = [measurements[0][0], 0, measurements[0][1], 0] transition_matrix = [[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]] observation_matrix = [[1, 0, 0, 0], [0, 0, 1, 0]] kf1 = KalmanFilter(transition_matrices=transition_matrix, observation_matrices=observation_matrix, initial_state_mean=initial_state_mean) kf1 = kf1.em(measurements, n_iter=10) (smoothed_state_means, smoothed_state_covariances) = kf1.smooth(measurements) return smoothed_state_means
def __init__(self): self.pos = [0,0,0] self.predicted = False self.idLocal = -1 self.idGlobal = -1 self.timestamp = time() self.idCam = -1 self.ocurrencias = 0 self.vol = -1 # Volumen self.seen = False # Flag update kalman self.kf = KalmanFilter( transition_matrices=transition_matrix, observation_matrices=observation_matrix, transition_offsets=transition_offset, observation_offsets=observation_offset, random_state=0 ) self.Vx = 0 self.Vy = 0 self.maskMultiDel = False self.dist = 0
def update_smoo_hists(self): """ update self.smoo_X_hist, self.smoo_Y_hist, self.smoo_R3_hist according to filter_type """ if self.dps_settings.filter_type == 'none': self.smoo_X_hist = self.X_hist self.smoo_Y_hist = self.Y_hist self.smoo_R3_hist = self.R3_hist self.smoo_velX_hist = self.velX_hist self.smoo_velY_hist = self.velY_hist self.smoo_velR3_hist = self.velR3_hist elif self.dps_settings.filter_type == 'kalman': self.update_sampled_hists() kf = KalmanFilter( initial_state_mean=0, n_dim_obs=1, ) self.smoo_X_hist, _ = kf.filter(self.sampled_X_hist) self.smoo_Y_hist, _ = kf.filter(self.sampled_Y_hist) self.smoo_R3_hist, _ = kf.filter(self.sampled_R3_hist)
def pyKalman4DTest(params, greater, samples): kp = params['kparams'] #kp['initial_state_mean']=[quadsummary([s['unusual_packet'] for s in samples]), # quadsummary([s['other_packet'] for s in samples]), # numpy.mean([s['unusual_tsval'] for s in samples]), # numpy.mean([s['other_tsval'] for s in samples])] kf = KalmanFilter(n_dim_obs=4, n_dim_state=4, **kp) smooth, covariance = kf.smooth([(s['unusual_packet'], s['other_packet'], s['unusual_tsval'], s['other_tsval']) for s in samples]) m = numpy.mean(smooth) if greater: if m > params['threshold']: return 1 else: return 0 else: if m < params['threshold']: return 1 else: return 0
def _log_likelihood(self, theta): Gamma0, Gamma1, Psi, Pi, C_in, obs_matrix, obs_offset = self._eval_matrix( theta, to_array=True) for mat in [Gamma0, Gamma1, Psi, Pi, C_in]: if isnan(mat).any() or isinf(mat).any(): return -inf G1, C_out, impact, fmat, fwt, ywt, gev, eu, loose = gensys( Gamma0, Gamma1, C_in, Psi, Pi) if eu[0] == 1 and eu[1] == 1: # TODO add observation covariance to allow for measurment errors kf = KalmanFilter(G1, obs_matrix, impact @ impact.T, None, C_out.reshape(self.n_state), obs_offset.reshape(self.n_obs)) L = kf.loglikelihood(self.data) else: L = -inf return L
def __init__(self, dt, conv_speed, x0=np.zeros((1, 4)), covar0=5 * np.eye(4).reshape((1, 4, 4))): self.dt = dt self.conv_speed = conv_speed self.state_dim = 4 # --- parameters --- self.A = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 0, 0], [0, 0, 0, 0]]) self.B = np.array([[0, 0], [0, 0], [1, 0], [0, 1]]) self.u = np.array([ conv_speed[1] * np.cos(conv_speed[0]), conv_speed[1] * np.sin(conv_speed[0]) ]) #conveyer speed factored into x and y self.C = np.eye(4) self.d = np.zeros(4, ) self.R = 0.7 * np.eye(4) # state (process) noise covariance self.Q = 1.0 * np.eye(4) # observation noise covariance self.x0 = x0 self.covar0 = covar0 transition_matrix = self.A transition_offset = np.matmul(self.B, self.u) # B*u = b*v_conv observation_matrix = self.C observation_offset = self.d transition_covariance = self.R observation_covariance = self.Q initial_state_mean = self.x0 initial_state_covariance = self.covar0 self.kf = KalmanFilter(transition_matrix, observation_matrix, transition_covariance, observation_covariance, transition_offset, observation_offset, initial_state_mean, initial_state_covariance) self.states_means = self.x0 # list of predictions of states means from KF self.states_covars = self.covar0 # list of predictions of states covariances from KF self.observations = self.x0 # list of the observations
def run_kal(measurements, training_size=40): # count the number of measurements num_measurements = len(measurements) # find the dimension of each row dim = len(measurements[0]) if dim == 3: simple_mode = True elif dim == 9: simple_mode = False else: print "Error: Dimensions for run_kal must be 3 or 9" exit(1) training_set = [] for i in range(min(training_size, len(measurements))): training_set.append(measurements[i]) if simple_mode: # run the filter kf = KalmanFilter(initial_state_mean=[0, 0, 0], n_dim_obs=3) (smoothed_state_means, smoothed_state_covariances) = kf.em(training_set).smooth(measurements) else: kf = KalmanFilter(initial_state_mean=[0, 0, 0, 0, 0, 0, 0, 0, 0], n_dim_obs=9) (smoothed_state_means, smoothed_state_covariances) = kf.em(training_set).smooth(measurements) # means represent corrected points return smoothed_state_means, smoothed_state_covariances, simple_mode
def _process_update(self, source: str, timestamp: int, data: Dict[str, float]): result = {} if input.size() >= self.length: independent_var = self.first_input.get_by_idx_range(key=None, start_idx=0, end_idx=-1) symbol_set = set(self.first_input.keys) depend_symbol = symbol_set.difference(self.first_input.default_key) depend_var = self.first_input.get_by_idx_range(key=depend_symbol, start_idx=0, end_idx=-1) obs_mat = np.vstack([ independent_var.values, np.ones(independent_var.values.shape) ]).T[:, np.newaxis] model = 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=self.trans_cov) state_means, state_covs = model.filter(depend_var.values) slope = state_means[:, 0][-1] result[Indicator.VALUE] = slope result[KalmanFilteringPairRegression.SLOPE] = slope result[KalmanFilteringPairRegression.SLOPE] = state_means[:, 1][-1] self.add(timestamp=timestamp, data=result) else: result[Indicator.VALUE] = np.nan result[KalmanFilteringPairRegression.SLOPE] = np.nan result[KalmanFilteringPairRegression.SLOPE] = np.nan self.add(timestamp=timestamp, data=result)
def _process_kalman(df): kf1 = KalmanFilter(transition_matrices=[1], observation_matrices=[1], initial_state_mean=0, initial_state_covariance=1, observation_covariance=1, transition_covariance=.01) x_state_means, _ = kf1.filter(df.x.values) y_state_means, _ = kf1.filter(df.y.values) df['smooth_x'] = x_state_means df['smooth_y'] = y_state_means obs_mat = np.vstack([df.smooth_x, np.ones(df.smooth_x.shape)]).T[:, np.newaxis] delta = 1e-5 trans_cov = delta / (1 - delta) * np.eye(2) kf2 = 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) reg_state, _ = kf2.filter(df.smooth_y.values) df['beta'], df['alpha'] = reg_state[:, 0], reg_state[:, 1] df['new_x'] = df['smooth_x'] * df['beta'] + df['alpha'] df['spread'] = df['smooth_y'] - df['new_x'] df['stdev'] = df.expanding().spread.std() df['z'] = df['spread'] / df['stdev']
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)
def __init__(self, init_state, transition_matrix, observation_matrix, n_iter=5, train_size=15): """ Create the Kalman filter. :param init_state: Initial state of the Kalman filter. Should be equal to first element in first_train_batch. :param transition_matrix: adjacency matrix representing state transition from t to t+1 for any time t Example: http://campar.in.tum.de/Chair/KalmanFilter Most likely, this will be an NxN eye matrix the where N is the number of variables being estimated :param observation_matrix: translation matrix from measurement coordinate system to desired coordinate system See: https://dsp.stackexchange.com/a/27488 Most likely, this will be an NxN eye matrix the where N is the number of variables being estimated :param n_iter: Number of times to repeat the parameter estimation function (estimates noise) """ init_state = np.array(init_state) transition_matrix = np.array(transition_matrix) observation_matrix = np.array(observation_matrix) self._expected_shape = init_state.shape self._filter = KalmanFilter( transition_matrices=transition_matrix, observation_matrices=observation_matrix, initial_state_mean=init_state, ) self._calibration_countdown = 0 self._calibration_observations = [] self._em_iter = n_iter self.calibrate(train_size, n_iter) self._x_now = np.zeros(3) self._p_now = np.zeros(3)
def KalmanSequence(size, a, rand): # X_{t+1} = a*X_t + noise ########## # specify parameters random_state = np.random.RandomState(rand) transition_matrix = [[a]] transition_offset = [0] observation_matrix = np.eye(1) #+ random_state.randn(2, 2) * 0.1 observation_offset = [0] transition_covariance = np.eye(1) observation_covariance = np.eye(1) #+ random_state.randn(2, 2) * 0.1 initial_state_mean = [0] initial_state_covariance = [[1]] # for 2-dim # random_state = np.random.RandomState(0) # transition_matrix = [[a, 0], [0, a]] # transition_offset = [0, 0] # observation_matrix = np.eye(2) #+ random_state.randn(2, 2) * 0.1 # observation_offset = [0, 0] # transition_covariance = np.eye(2) # observation_covariance = np.eye(2) #+ random_state.randn(2, 2) * 0.1 # initial_state_mean = [5, -5] # initial_state_covariance = [[1, 0], [0, 1]] # sample from model kf = KalmanFilter( transition_matrix, observation_matrix, transition_covariance, observation_covariance, transition_offset, observation_offset, initial_state_mean, initial_state_covariance, random_state=random_state ) states, observations = kf.sample(n_timesteps=size, initial_state=initial_state_mean) # estimate state with filtering and smoothing filtered_state_estimates = kf.filter(states)[0] # filtered_state_estimates = kf.filter(observations)[0] # smoothed_state_estimates = kf.smooth(observations)[0] return states, observations, filtered_state_estimates
def checkMLDS(T, A, C, Q, S): """ Quick sanity check to test multivariate LDS Generate Dx latent dynamics and fit Dy observation """ Dx = A.shape[0] Dy = C.shape[0] obs = np.zeros([T, Dy]) hid = np.zeros([T, Dx]) for i in range(1, T): hid[i] = np.dot(A, hid[i - 1]) + np.dot(Q, np.random.randn(Dx)) obs[i] = np.dot(C, hid[i]) + np.dot(S, np.random.randn(Dy)) plt.figure() plt.title("Simulated Data") plt.plot(hid, c='blue') plt.plot(obs, c='red') plt.legend(['hidden', 'observed']) plt.show() mykf = KalmanFilter(initial_state_mean=np.zeros(Dx), n_dim_state=Dx, n_dim_obs=Dy, em_vars=[ 'transition_matrices', 'observation_matrices', 'transition_covariance', 'observation_covariance' ]) mykf.em(obs, n_iter=100) plt.figure() myZ, mySig = mykf.smooth(obs) plt.title("Estimated States vs Ground Truth") plt.plot(myZ, c='red') plt.plot(hid, c='blue') plt.legend(['smoothed', 'true']) plt.show() return mykf.transition_matrices, mykf.observation_matrices, mykf.transition_covariance, mykf.observation_covariance
def next(obj, field): objvals = self._message_df[self._message_df['object_id'] == obj][field] if len(self._message_df[self._message_df['object_id'] == obj]) < 2: self._result = message.get(field, None) else: self.initial_state_mean = objvals.iat[0] self.kf = KalmanFilter( transition_matrices=self.F, transition_covariance=self.Q, observation_matrices=self.H, observation_covariance=self.R, initial_state_mean=self.initial_state_mean, initial_state_covariance=self.P) state_means, _ = self.kf.filter(objvals.values) state_means = pd.Series(state_means.flatten()) self._result = state_means.iloc[-1] if len(objvals) < 10: pass else: self._message_df[self._message_df['object_id'] == obj].drop( self._message_df[self._message_df['object_id'] == obj].head(1).index) return self._result