Python KalmanFilter.smooth方法代码示例

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

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

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

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

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

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

# Estimate the hidden states using observations up to and including
# time t for t in [0...n_timesteps-1].  This method outputs the mean and
# covariance characterizing the Multivariate Normal distribution for
#   P(x_t | z_{1:t})
filtered_state_estimates = kf.filter(data.observations)[0]

# Estimate the hidden states using all observations.  These estimates
# will be 'smoother' (and are to be preferred) to those produced by
# simply filtering as they are made with later observations in mind.
# Probabilistically, this method produces the mean and covariance
# characterizing,
#    P(x_t | z_{1:n_timesteps})
smoothed_state_estimates = kf.smooth(data.observations)[0]

# Draw the true, blind,e filtered, and smoothed state estimates for all 5
# dimensions.
pl.figure(figsize=(16, 6))
lines_true = pl.plot(data.states, linestyle='-', color='b')
# lines_blind = pl.plot(blind_state_estimates, linestyle=':', color='m')
lines_observations = pl.plot(data.observations, linestyle=':', color='m')
lines_filt = pl.plot(filtered_state_estimates, linestyle='--', color='g')
lines_smooth = pl.plot(smoothed_state_estimates, linestyle='-.', color='r')
pl.legend(
    (lines_true[0], lines_filt[0], lines_smooth[0], lines_observations[0]),
    ('true', 'filtered', 'smoothed', 'observations')
)
pl.xlabel('time')
pl.ylabel('state')

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

# The Kalman Filter is parameterized by 3 arrays for state transitions, 3 for measurements, and 2 more for initial conditions.
## This worked
kf = KalmanFilter(transition_matrices=transition_matrix,
                  observation_matrices=observation_matix,
				  transition_covariance=0.01 * np.eye(3)
)

kf = kf.em(X, n_iter=5)

(filtered_state_means, filtered_state_covariances) = kf.filter(X)
(smoothed_state_means, smoothed_state_covariances) = kf.smooth(X)

# kf.em(X).smooth([X]) ## this gives the shape error
kf = kf.em(X).smooth(X) # This returns large tuple
# print len(kf_test)

# Plot lines for the observations without noise, the estimated position of the
# target before fitting, and the estimated position after fitting.

states_pred = kf

n_timesteps = X.shape[0]
x = np.linspace(0, 3 * np.pi, n_timesteps)
xobservations = 20 * (np.sin(x) + 0.5 * rnd.randn(n_timesteps))

observations['noise'] = x

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

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import smooth [as 别名]
transition_covariance = 0.05 # Q
observation_covariance = 0.5  # R

initial_state_mean = 0.0
initial_state_covariance = 1.0

# sample from model
kf = KalmanFilter(
    transition_matrix, observation_matrix, transition_covariance,
    observation_covariance, transition_offset, observation_offset,
    initial_state_mean, initial_state_covariance,
)

#kf = kf.em(y, n_iter=5) # Expectation Maximization
y_filtered['pykalman-zero-order'] = kf.filter(y)[0].flatten()
y_filtered['pykalman_smoothed-zero-order'] = kf.smooth(y)[0][:,0]




#--------------------------------------------------------------------------------------------
# Kalman filter 2nd order (mass-spring-damper) (PyKalman)
#--------------------------------------------------------------------------------------------
m = 1000.0 # mass
k_s = 0.05 # spring (the larger, the harder)
k_d = 0.5 # damper (the larger, the harder)
d = k_d / k_s
print("d: " + str(d) + ",  Damped: " + str(d > 1))

q = 0.1  # Variance of process noise, i.e. state uncertainty
r = 0.01 # Variance of measurement error

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import smooth [as 别名]
    for cnum, cpos in posDF.groupby("id"):
        if len(cpos) == 1:
            continue
        ft = np.arange(cpos["frame"].values[0], cpos["frame"].values[-1] + 1)
        # obs = np.vstack((cpos['x'].values, cpos['y'].values)).T
        obs = np.zeros((len(ft), 2))
        obs = np.ma.array(obs, mask=np.zeros_like(obs))
        for f in range(len(ft)):
            if len(cpos[cpos["frame"] == ft[f]].x.values) > 0:
                obs[f][0] = cpos[cpos["frame"] == ft[f]].x.values[0] * px_to_m
                obs[f][1] = cpos[cpos["frame"] == ft[f]].y.values[0] * px_to_m
            else:
                obs[f] = np.ma.masked

        kf.initial_state_mean = [cpos["x"].values[0] * px_to_m, cpos["y"].values[0] * px_to_m, 0, 0, 0, 0]
        sse = kf.smooth(obs)[0]

        ani = cpos["animal"].values[0]

        xSmooth = sse[:, 0]
        ySmooth = sse[:, 1]
        xv = sse[:, 2] / 0.1
        yv = sse[:, 3] / 0.1
        xa = sse[:, 4] / 0.01
        ya = sse[:, 5] / 0.01
        headings = np.zeros_like(xSmooth)
        dx = np.zeros_like(xSmooth)
        dy = np.zeros_like(xSmooth)
        for i in range(len(headings)):
            start = max(0, i - 5)
            stop = min(i + 5, len(headings)) - 1

# 需要导入模块: from pykalman import KalmanFilter [as 别名]
# 或者: from pykalman.KalmanFilter import smooth [as 别名]
    def speakerTracker(self):
        

        # Clean up
        cv2.destroyAllWindows()

        if self.inputPath is not None:

            # If a path to a file was given, assume it is a single video file
            if os.path.isfile(self.inputPath):
                cap = cv2.VideoCapture(self.inputPath)
                clip  = VideoFileClip(self.inputPath,audio=False)
                fps = cap.get(cv2.cv.CV_CAP_PROP_FPS)
                self.numFrames = cap.get(cv2.cv.CV_CAP_PROP_FRAME_COUNT)
                print "[speakerTracker] Number of frames" , self.numFrames
                pathBase = os.path.basename(self.inputPath)
                pathDirectory = os.path.dirname(self.inputPath)
                baseName = pathDirectory + '/' + os.path.splitext(pathBase)[0] + '_' 

            # Otherwise assume it is a format string for reading images
            else:
                cap = cmtutil.FileVideoCapture(self.inputPath)

        else:
            # If no input path was specified, open camera device
            sys.exit("[speakerTracker] Error: no input path was specified")

        # Read first frame
        status, im0 = cap.read()
        imGray0 = cv2.cvtColor(im0, cv2.COLOR_BGR2GRAY)
        imDraw = np.copy(im0)

        (tl, br) = cmtutil.get_rect(imDraw)

        print '[speakerTrackering] Using', tl, br, 'as initial bounding box for the speaker'

        self.CMT.initialise(imGray0, tl, br)
        #self.inity = tl[1] - self.CMT.center_to_tl[1]
        measuredTrack=np.zeros((self.numFrames+10,2))-1
        
        
        count =0
        for frame in clip.iter_frames():
            im_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
            
            self.CMT.process_frame(im_gray)

            print 'frame: {2:4d}, Center: {0:.2f},{1:.2f}'.format(self.CMT.center[0], self.CMT.center[1] , count)
            #print self.inity
            if not (math.isnan(self.CMT.center[0]) or (self.CMT.center[0] <= 0)):
                measuredTrack[count,0] = self.CMT.center[0]
                measuredTrack[count,1] = self.CMT.center[1]               
            count += 1 

    
        numMeas=measuredTrack.shape[0]
        markedMeasure=np.ma.masked_less(measuredTrack,0)
        
        # Kalman Filter Parameters
        deltaT = 1.0/clip.fps
        transitionMatrix=[[1,0,deltaT,0],[0,1,0,deltaT],[0,0,1,0],[0,0,0,1]]   #A
        observationMatrix=[[1,0,0,0],[0,1,0,0]]   #C

        xinit = markedMeasure[0,0]
        yinit = markedMeasure[0,1]
        vxinit = markedMeasure[1,0]-markedMeasure[0,0]
        vyinit = markedMeasure[1,1]-markedMeasure[0,1]
        initState = [xinit,yinit,vxinit,vyinit]    #mu0
        initCovariance = 1.0e-3*np.eye(4)          #sigma0
        transistionCov = 1.0e-4*np.eye(4)          #Q
        observationCov = 1.0e-1*np.eye(2)          #R
        kf = KalmanFilter(transition_matrices = transitionMatrix,
            observation_matrices = observationMatrix,
            initial_state_mean = initState,
            initial_state_covariance = initCovariance,
            transition_covariance = transistionCov,
            observation_covariance = observationCov)
        
        self.measuredTrack = measuredTrack
        (self.filteredStateMeans, self.filteredStateCovariances) = kf.filter(markedMeasure)
        (self.filterStateMeanSmooth, self.filterStateCovariancesSmooth) = kf.smooth(markedMeasure)
        self.inity = np.mean(self.filterStateMeanSmooth[:][1], axis=0)
        newClip = clip.fl_image( self.crop ) 
        return newClip