コード例 #1
0
ファイル: bgds_agent.py プロジェクト: AndreaCensi/boot_agents
class BGDSAgent(ExpSwitcher):
    '''
        Skip: only consider every $skip observations.
        
        scales: list of floats, represents the scales at which the 
                sensels are analyzed. 0=raw data, 1= convolved with sigma=1.
    '''
    @contract(scales='list[>=1](number,>=0)')
    def __init__(self, beta, skip=1, scales=[0], fixed_dt=0):
        ExpSwitcher.__init__(self, beta)
        self.skip = skip
        self.scales = scales
        self.fixed_dt = fixed_dt

    def init(self, boot_spec):
        # TODO: do the 1D version
        shape = boot_spec.get_observations().shape()
        if len(shape) > 2:
            msg = 'BGDSagent only work with 2D or 1D signals.'
            raise UnsupportedSpec(msg)

        min_width = np.min(shape)
        if min_width <= 5:
            msg = ('BGDSagent thinks this shape is too'
                   'small to compute gradients: %s' % str(shape))
            raise UnsupportedSpec(msg)

        self.is2D = len(boot_spec.get_observations().shape()) == 2
        self.is1D = len(boot_spec.get_observations().shape()) == 1

        ExpSwitcher.init(self, boot_spec)
        self.count = 0
        self.y_deriv = DerivativeBox()
        self.bgds_estimator = BGDSEstimator()

        self.model = None
        self.y_disag = Expectation()
        self.y_disag_s = Expectation()
        self.u_stats = []
        self.last_y0 = None
        self.last_y = None

        self.rd = RemoveDoubles(0.5)

    def process_observations(self, obs):
        dt = float(obs['dt'])
        u = obs['commands']
        y0 = obs['observations']
        episode_start = obs['episode_start']

        self.count += 1
        if self.count % self.skip != 0:
            return

        if self.fixed_dt:
            # dt is not reliable sometime
            # you don't want to give high weight to higher dt samples.
            dt = 1  # XXX: add in constants

        self.rd.update(y0)
        if not self.rd.ready():
            return

        if self.is2D:
            y = create_scales(y0, self.scales)
        else:
            y = y0

        if episode_start:
            self.y_deriv.reset()
            return

        self.y_deriv.update(y, dt)

        if not self.y_deriv.ready():
            return

        y_sync, y_dot_sync = self.y_deriv.get_value()

        self.bgds_estimator.update(u=u.astype('float32'),
                                   y=y_sync.astype('float32'),
                                   y_dot=y_dot_sync.astype('float32'),
                                   dt=dt)
        self.last_y0 = y0
        self.last_y = y

        # TODO: implement this separately
        if False and self.is2D and self.count > MINIMUM_FOR_PREDICTION:
            # TODO: do for 1D
            if self.count % 200 == 0 or self.model is None:
                self.info('Updating BGDS model.')
                self.model = self.bgds_estimator.get_model()

            gy = self.bgds_estimator.last_gy
            y_dot_est = self.model.estimate_y_dot(y, u, gy=gy)
            y_dot_corr = y_dot_est * y_dot_sync
            self.y_disag.update(np.maximum(-y_dot_corr, 0))
            self.y_disag_s.update(np.sign(y_dot_corr))

            u_est = self.model.estimate_u(y, y_dot_sync, gy=gy)

            data = {'u': u,
                    'u_est': u_est,
                    'timestamp': obs.time,
                    'id_episode': obs.id_episode
            }
            self.u_stats.append(data)

#          u_est = self.model.estimate_u(y, y_dot_sync, gy=self.bgds_estimator)
#          self.u_stats.append()
#            
    def publish(self, pub):
        if self.count < 10:
            self.info('Skipping publishing as count=%d' % self.count)
            return

        self.bgds_estimator.publish(pub.section('model'))

        if False and self.is2D:  # TODO: implement separately
            sec = pub.section('preprocessing')
            sec.array_as_image('last_y0', self.last_y0, filter='scale')
            sec.array_as_image('last_y', self.last_y, filter='scale')
            example = np.zeros(self.last_y.shape)
            example.flat[150] = 1
            example_smooth = create_scales(example, self.scales)
            sec.array_as_image('example_smooth', example_smooth)

            if self.count > MINIMUM_FOR_PREDICTION:
                sec = pub.section('reliability')
                sec.array_as_image('y_disag',
                                   self.y_disag.get_value(), filter='posneg')
                sec.array_as_image('y_disag_s',
                                   self.y_disag_s.get_value(), filter='posneg')

        if False:  # XXX
            self.publish_u_stats(pub.section('u_stats'))

    def publish_u_stats(self, pub):
        T = len(self.u_stats)
        print('Obtained %d obs' % T)
        K = 2  # FIXME: change this
        u_act = np.zeros((T, K))
        u_est = np.zeros((T, K))
        u_mis = np.zeros((T, K))
        u_suc = np.zeros((T, K))
        time = np.zeros(T)
        num_episode = np.zeros(T, 'int')
        id_episode2num = {}
        num2id_episode = {}
        id_episode2start = {}
        # cmd2faults = {}
        for t, stats in enumerate(self.u_stats):
            u_act[t, :] = stats['u']
            u_est[t, :] = stats['u_est']
            time[t] = stats['timestamp']
            id_ep = stats['id_episode']
            if not id_ep in id_episode2num:
                id_episode2num[id_ep] = len(id_episode2num)
                id_episode2start[id_ep] = time[t]
                num2id_episode[id_episode2num[id_ep]] = id_ep
            num_episode[t] = id_episode2num[id_ep]

        s = ""
        for k, v in id_episode2num.items():
            s += '%s: %s\n' % (k, v)
        pub.text('episodes', s)
        with pub.plot('num_episode') as pylab:
            pylab.plot(num_episode, '-')
            pylab.xlabel('index')
            pylab.ylabel('num\_episode')

        for id_episode, num in id_episode2num.items():
            print id_episode
            S = pub.section('Episode:%s' % id_episode)
            # times for this episode
            et = num_episode == num
            # normalize from 0
            e_timestamps = time[et]
            log_start = e_timestamps[0]
            e_timestamps -= log_start
            cmd2color = {0: 'g', 1: 'b'}

            episode_bounds = (18, 60)
            markersize = 2
            with S.plot('mis', figsize=(8, 2), mime=MIME_PDF) as pylab:
                for k in range(K):
#                    scale = 7
#                    u_mis_smooth = scipy.signal.convolve(u_mis[et, k], 
#                     np.ones(scale) / scale,
#                                                         mode='same')
                    pylab.plot(e_timestamps, u_mis[et, k],  # u_mis_smooth,
                               '%s-' % cmd2color[k], label='u[%d]' % k,
                               markersize=markersize)
                x_axis_set(pylab, episode_bounds[0], episode_bounds[1])

            with S.plot('success', figsize=(8, 2), mime=MIME_PDF) as pylab:
                pylab.plot(e_timestamps, e_timestamps * 0, 'k--')
                pylab.plot(e_timestamps, np.ones(len(e_timestamps)), 'k--')
                for k in range(K):
                    pylab.plot(e_timestamps, u_suc[et, k],
                               '%s-' % cmd2color[k], label='cmd #%d' % k)
                y_axis_set(pylab, -0.05, 1.05)
                x_axis_set(pylab, episode_bounds[0], episode_bounds[1])
                pylab.legend(loc='lower right')

            for k in range(K):
                with S.plot('commands_%d' % k, figsize=(8, 2),
                            mime=MIME_PDF) as pylab:
                    pylab.plot(e_timestamps, u_act[et, k], 'y.',
                               label='actual', markersize=3)
                    plot_with_colors(pylab, e_timestamps,
                                     u_est[et, k], u_act[et, k],
                                     markersize=markersize)
                    y_axis_set(pylab, -2, 2)
                    x_axis_set(pylab, episode_bounds[0], episode_bounds[1])

    def get_predictor(self):
        model = self.bgds_estimator.get_model()
        return BGDSPredictor(model)
コード例 #2
0
class BDSEEstimator(BDSEEstimatorInterface):
    """
        Estimates a BDSE model.
        
        Tensors used: ::
        
            M^s_vi   (N) x (N x K)
            N^s_i    (N) x (K)
            T^svi    (NxNxK)
            U^si     (NxK)
    
    """

    @contract(rcond='float,>0')
    def __init__(self, rcond=1e-10, antisym_T=False, antisym_M=False, use_P_scaling=False):
        """
            :param rcond: Threshold for computing pseudo-inverse of P.
            :param antisym_T: If True, the estimate of T is antisymmetrized.
            :param antisym_M: If True, the estimate of M is antisymmetrized.
        """
        self.rcond = rcond
        self.antisym_M = antisym_M
        self.antisym_T = antisym_T
        self.use_P_scaling = use_P_scaling
        self.info('rcond: %f' % rcond)
        self.info('antisym_T: %s' % antisym_T)
        self.info('antisym_M: %s' % antisym_M)
        self.info('use_P_scaling: %s' % use_P_scaling)

        self.T = Expectation()
        self.U = Expectation()
        self.y_stats = MeanCovariance()
        self.u_stats = MeanCovariance()
        self.nsamples = 0
        self.once = False
        
    def merge(self, other):
        assert isinstance(other, BDSEEstimator)
        self.T.merge(other.T)
        self.U.merge(other.U)
        self.y_stats.merge(other.y_stats)
        self.u_stats.merge(other.u_stats)
        self.nsamples += other.nsamples
    
    @contract(u='array[K],K>0,finite',
              y='array[N],N>0,finite',
              y_dot='array[N],finite', w='>0')
    def update(self, y, u, y_dot, w=1.0):
        self.once = True
        self.nsamples += 1
        
        self.n = y.size
        self.k = u.size  # XXX: check

        self.y_stats.update(y, w)
        self.u_stats.update(u, w)
        
        # remove mean
        u_n = u - self.u_stats.get_mean()
        y_n = y - self.y_stats.get_mean()
        
        # make products
        T_k = outer(outer(y_n, y_dot), u_n)
        assert T_k.shape == (self.n, self.n, self.k)
        
        U_k = outer(y_dot, u_n)
        assert U_k.shape == (self.n, self.k)
        
        # update tensor
        self.T.update(T_k, w)
        self.U.update(U_k, w)

    def get_P_inv_cond(self):
        P = self.y_stats.get_covariance()
        if False:
            P_inv = np.linalg.pinv(P, rcond=self.rcond)
        if True:
            P2 = P + np.eye(P.shape[0]) * self.rcond
            P_inv = np.linalg.inv(P2)
        return P_inv

    def get_T(self):
        T = self.T.get_value()
        if self.antisym_T:
            self.info('antisymmetrizing T')
            T = antisym(T)
        return T
    
    def get_model(self):
        T = self.get_T()
            
        U = self.U.get_value()
        P = self.y_stats.get_covariance()
        Q = self.u_stats.get_covariance()

        P_inv = self.get_P_inv_cond()
        Q_inv = np.linalg.pinv(Q)

        if False:
            M = get_M_from_P_T_Q(P, T, Q)
        else:
            if hasattr(self, 'use_P_scaling') and self.use_P_scaling:
                M = get_M_from_P_T_Q_alt_scaling(P, T, Q)
            else:
                warnings.warn('untested')
                try:
                    M = get_M_from_Pinv_T_Q(P_inv, T, Q)
                except LinAlgError as e:
                    msg = 'Could not get_M_from_Pinv_T_Q.\n'
                    msg += indent(traceback.format_exc(e), '> ')
                    raise BDSEEstimatorInterface.NotReady(msg)
        
        UQ_inv = np.tensordot(U, Q_inv, axes=(1, 0))
        # This works but badly conditioned
        Myav = np.tensordot(M, self.y_stats.get_mean(), axes=(1, 0))
        N = UQ_inv - Myav

        if self.antisym_M:
            self.info('antisymmetrizing M')
            M = antisym(M)
        
#         # Note: this does not work, don't know why
#         if False:
#             printm('MYav1', Myav)
#             y2 = np.linalg.solve(P, self.y_stats.get_mean())
#             Myav2 = np.tensordot(T, y2, axes=(0, 0))
#             # Myav = np.tensordot(T, y2, axes=(1, 0))
#             printm('MYav2', Myav2)
#         if False:
#             printm('U', U, 'Q_inv', Q_inv)
#             printm('UQ_inv', UQ_inv, 'Myav', Myav, 'N', N)
#             printm('u_mean', self.u_stats.get_mean())
#             printm('u_std', np.sqrt(Q.diagonal()))
#             printm('y_mean', self.y_stats.get_mean())
            
        self.Myav = Myav
        self.UQ_inv = UQ_inv
            
        return BDSEmodel(M, N)

    def publish(self, pub):
        if not self.once:
            pub.text('warning', 'not updated yet')
            return
        
        pub.text('nsamples', '%s' % self.nsamples)
        
        pub.text('rcond', '%g' % self.rcond)
        with pub.subsection('model') as sub:
            try:
                model = self.get_model()
                model.publish(sub)
            except BDSEEstimatorInterface.NotReady as e:
                pub.text('not-ready', str(e))

        with pub.subsection('tensors') as sub:
            T = self.get_T()
            U = self.U.get_value()
            P = self.y_stats.get_covariance()
            Q = self.u_stats.get_covariance()
            P_inv = np.linalg.pinv(P)
            P_inv_cond = self.get_P_inv_cond()
            Q_inv = np.linalg.pinv(Q)
    #
    #        TP_inv2 = obtain_TP_inv_from_TP_2(T, P)  
    #        M2 = np.tensordot(TP_inv2, Q_inv, axes=(2, 0))
        
            pub_tensor3_slice2(sub, 'T', T)
            pub_tensor2_comp1(sub, 'U', U)
            pub_tensor2_cov(sub, 'P', P, rcond=self.rcond)
            pub_tensor2_cov(sub, 'P_inv', P_inv)
            pub_tensor2_cov(sub, 'P_inv_cond', P_inv_cond)
            pub_tensor2_cov(sub, 'Q', Q)
            pub_tensor2_cov(sub, 'Q_inv', Q_inv)
            # Might not have been computed
            # pub_tensor2_comp1(sub, 'Myav', self.Myav)
            # pub_tensor2_comp1(sub, 'UQ_inv', self.UQ_inv)

        with pub.subsection('y_stats') as sub:
            self.y_stats.publish(sub)

        with pub.subsection('u_stats') as sub:
            self.u_stats.publish(sub)
          
        with pub.subsection('alternative', robust=True) as sub:
            sub.text('info', 'This is estimating without conditioning P')
            T = self.get_T() 
            P = self.y_stats.get_covariance()
            Q = self.u_stats.get_covariance()
            
            M1 = get_M_from_P_T_Q(P, T, Q)
            pub_tensor3_slice2(sub, 'get_M_from_P_T_Q', M1)
            
            M2 = get_M_from_P_T_Q_alt(P, T, Q)
            pub_tensor3_slice2(sub, 'get_M_from_P_T_Q_alt', M2)

            M3 = get_M_from_P_T_Q_alt_scaling(P, T, Q)
            pub_tensor3_slice2(sub, 'get_M_from_P_T_Q_alt2', M3)