Python KalmanFilter.em方法代码示例

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
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])

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
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

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
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))

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
 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)

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
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()))

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
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

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
    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']]

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
def main():
	# get the data
	readings, data_format = unpack_binary_data_into_list(BSN_DATA_FILE_NAME)
	# just the data/readings, no timestamps
	bsn_data = np.array([np.array(x[1:]) for x in readings[0:]]) # TODO
			
	# initialize filter
	# (all constructor parameters have defaults, and pykalman supposedly does a
	# good job of estimating them, so we will be lazy until there is a need to
	# define the initial parameters)
	bsn_kfilter = KalmanFilter(
		initial_state_mean = bsn_data[0],
		n_dim_state = len(bsn_data[0]),
		n_dim_obs = len(bsn_data[0]),
		em_vars = 'all'
	)

	# perform parameter estimation and do predictions
	print("Estimating parameters...")
	bsn_kfilter.em(X=bsn_data, n_iter=5, em_vars = 'all')
	print("Creating smoothed estimates...")
	filtered_bsn_data = bsn_kfilter.smooth(bsn_data)[0]

	# re-attach the time steps to observations
	filtered_bsn_data = bind_timesteps_and_data_in_list(
		[x[0:1][0] for x in readings],
		filtered_bsn_data
	)

	# write the data to a new file
	with open(FILTERED_BSN_DATA_FILE_NAME, "wb") as filtered_file:
		filtered_file.write(string.ljust(data_format, 25))
		for filtered_item in filtered_bsn_data:
			print(filtered_item)
			filtered_file.write(struct.pack(data_format, *filtered_item))
		filtered_file.close()

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
 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]

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
 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

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
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

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
def get_interpolated_speed_list(path,kalman=False,smoothFactor=0.01):
	gpx_file = open(path, 'r')
	gpx = gpxpy.parse(gpx_file)
	interpolated_list = []
	previous_speed = 0

	for point in range(len(gpx.tracks[0].segments[0].points)-1):
		p1 = gpx.tracks[0].segments[0].points[point]
		p1_time = time.mktime(p1.time.timetuple())

		p2 = gpx.tracks[0].segments[0].points[point+1]
		p2_time = time.mktime(p2.time.timetuple())

		## average speed between data points
		speed = p1.speed_between(p2)
		if speed is None or speed == 0:
			if previous_speed > 7:
				if speed == 0:
					speed = previous_speed-1
				else:
					speed = previous_speed
			else:
				speed = 0
		previous_speed = speed

		interpolated_list.append(speed)

		## if time difference is greater than one second, the missing seconds are filled with average speed
		if p2_time-p1_time > 1:
			seconds = int(p2_time-p1_time-1)
			for second in range(seconds):
				interpolated_list.append(speed)
	if kalman is True:
		kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]), transition_covariance=smoothFactor*np.eye(2))
		states_pred = kf.em(interpolated_list).smooth(interpolated_list)[0]
		return states_pred[:, 0]
	else:
		return interpolated_list

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
from pykalman import KalmanFilter
import numpy as np
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
kf = kf.em(measurements, n_iter=5)
(filtered_state_means, filtered_state_covariances) = kf.filter(measurements)

print filtered_state_means

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
    accel_z.append((acceleration_z[i-2] + acceleration_z[i-1] + acceleration_z[i]) / n)
'''
'''
for i in range(len(acceleration_x)):
    if abs(acceleration_x[i]) < 2:
        acceleration_x[i] = 0
    if abs(acceleration_y[i]) < 2:
        acceleration_y[i] = 0
    if abs(acceleration_z[i]) < 2:
        acceleration_z[i] = 0
'''
t = np.arange(0, (len(acceleration_x))*dt, dt)

kf = KalmanFilter(transition_matrices=np.array([[1.15, 1], [0, 1]]),
                  transition_covariance=0.01 * np.eye(2))
states_pred = kf.em(acceleration_x).smooth(acceleration_x)[0]

plt.figure(1)
obs_scatter = plt.plot(t[0:len(acceleration_x)], acceleration_x, color='b',
                         label='observations x')
acceleration_x = states_pred[:, 0]
position_line = plt.plot(t[0:len(states_pred[:, 0])], states_pred[:, 0],
                        linestyle='-', color='r',
                        label='filtered x')
plt.legend(loc='lower right')
plt.xlabel('time')
plt.grid()

#dist, cost, path = mlpy.dtw_subsequence(acceleration_x, states_pred[:, 0], dist_only=False)
lines = [' '.join(list(map(str, states_pred[:, 0]))) + "\n"]

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import em [as 别名]
 def apply(self, dataset):
     kf = KalmanFilter(initial_state_mean=0, n_dim_obs=len(dataset.data))
     predicted = kf.em(dataset.data).smooth(dataset.data)
     return data