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 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 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 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 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 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 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 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 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 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 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]

 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

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 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))

    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)