Example #1
0
    def eval_logp_x0(self, particles, t):
        """
        Evaluate log p(x_0)

        Args:
         - particles  (array-like): Model specific representation
           of all particles, with first dimension = N (number of particles)
         - t (float): time stamp
        """

        N = len(particles)
        res = numpy.empty(N)
        # Assumes Px0 is either full rang or zero
        if ((self.Px0 == 0.0).all()):
            x0 = self.x0.ravel()
            for i in range(N):
                if (numpy.array_equiv(particles[i], x0)):
                    res[i] = 0.0
                else:
                    res[i] = -numpy.Inf
        else:
            Pchol = scipy.linalg.cho_factor(self.Px0, check_finite=False)
            for i in range(N):
                res[i] = kalman.lognormpdf_cho(
                    particles[i].ravel() - self.x0.ravel(), Pchol)

        return res
Example #2
0
    def logp_xnext(self, particles, next_part, u, t):
        """
        Return the log-pdf value for the possible future state 'next'
        given input u

        Args:

         - particles  (array-like): Model specific representation
           of all particles, with first dimension = N (number of particles)
         - next_part (array-like): particle estimate for t+1
         - u (array-like): input signal
         - t (float): time stamps

        Returns:
         (array-like) with first dimension = N, logp(x_{t+1}|x_t^i)
        """

        f = self.calc_f(particles, u, t)
        if (f is None):
            f = self.f
        diff = next_part - f
        Q = self.calc_Q(particles, u, t)
        if (Q is None):
            if (self.Qcholtri.shape[0] == 1):
                lpx = kalman.lognormpdf_scalar(diff, self.Qcholtri)
            else:
                lpx = kalman.lognormpdf_cho_vec(diff, self.Qchol)
        else:
            N = len(particles)
            lpx = numpy.empty(N)
            for i in range(N):
                Qchol = scipy.linalg.cho_factor(Q[i], check_finite=False)
                lpx[i] = kalman.lognormpdf_cho(diff[i], Qchol)

        return lpx
Example #3
0
    def eval_logp_x0(self, particles, t):
        """
        Evaluate log p(x_0)

        Args:
         - particles  (array-like): Model specific representation
           of all particles, with first dimension = N (number of particles)
         - t (float): time stamp
        """

        N = len(particles)
        res = numpy.empty(N)
        # Assumes Px0 is either full rang or zero
        if ((self.Px0 == 0.0).all()):
            x0 = self.x0.ravel()
            for i in xrange(N):
                if (numpy.array_equiv(particles[i], x0)):
                    res[i] = 0.0
                else:
                    res[i] = -numpy.Inf
        else:
            Pchol = scipy.linalg.cho_factor(self.Px0, check_finite=False)
            for i in xrange(N):
                res[i] = kalman.lognormpdf_cho(particles[i].ravel() - self.x0.ravel(), Pchol)

        return res
Example #4
0
    def logp_xnext(self, particles, next_part, u, t):
        """
        Return the log-pdf value for the possible future state 'next'
        given input u

        Args:

         - particles  (array-like): Model specific representation
           of all particles, with first dimension = N (number of particles)
         - next_part (array-like): particle estimate for t+1
         - u (array-like): input signal
         - t (float): time stamps

        Returns:
         (array-like) with first dimension = N, logp(x_{t+1}|x_t^i)
        """

        f = self.calc_f(particles, u, t)
        if (f is None):
            f = self.f
        diff = next_part - f
        Q = self.calc_Q(particles, u, t)
        if (Q is None):
            if (self.Qcholtri.shape[0] == 1):
                lpx = kalman.lognormpdf_scalar(diff, self.Qcholtri)
            else:
                lpx = kalman.lognormpdf_cho_vec(diff, self.Qchol)
        else:
            N = len(particles)
            lpx = numpy.empty(N)
            for i in xrange(N):
                Qchol = scipy.linalg.cho_factor(Q[i], check_finite=False)
                lpx[i] = kalman.lognormpdf_cho(diff[i], Qchol)

        return lpx
Example #5
0
    def measure(self, particles, y, t):
        """
        Return the log-pdf value of the measurement

        Args:

         - particles  (array-like): Model specific representation
           of all particles, with first dimension = N (number of particles)
         - y (array-like):  measurement
         - t (float): time-stamp

        Returns:
         (array-like) with first dimension = N, logp(y|x^i)
        """
        N = len(particles)
        lpy = numpy.empty(N)
        g = self.calc_g(particles=particles, t=t)
        R = self.calc_R(particles=particles, t=t)

        if (g is None):
            g = numpy.repeat(self.g.reshape((1, -1, 1)), N, 0)
        else:
            g = g.reshape((N, -1, 1))
        yrep = numpy.repeat(numpy.asarray(y).reshape((1, -1, 1)), N, 0)
        diff = yrep - g
        if (R is None):
            if (self.Rcholtri.shape[0] == 1):
                lpy = kalman.lognormpdf_scalar(diff, self.Rcholtri)
            else:
                lpy = kalman.lognormpdf_cho_vec(diff, self.Rchol)
        else:
            lpy = numpy.empty(N)
            for i in range(N):
                Rchol = scipy.linalg.cho_factor(R[i], check_finite=False)
                lpy[i] = kalman.lognormpdf_cho(diff[i], Rchol)

        return lpy
Example #6
0
    def measure(self, particles, y, t):
        """
        Return the log-pdf value of the measurement

        Args:

         - particles  (array-like): Model specific representation
           of all particles, with first dimension = N (number of particles)
         - y (array-like):  measurement
         - t (float): time-stamp

        Returns:
         (array-like) with first dimension = N, logp(y|x^i)
        """
        N = len(particles)
        lpy = numpy.empty(N)
        g = self.calc_g(particles=particles, t=t)
        R = self.calc_R(particles=particles, t=t)

        if (g is None):
            g = numpy.repeat(self.g.reshape((1, -1, 1)), N, 0)
        else:
            g = g.reshape((N, -1, 1))
        yrep = numpy.repeat(numpy.asarray(y).reshape((1, -1, 1)), N, 0)
        diff = yrep - g
        if (R is None):
            if (self.Rcholtri.shape[0] == 1):
                lpy = kalman.lognormpdf_scalar(diff, self.Rcholtri)
            else:
                lpy = kalman.lognormpdf_cho_vec(diff, self.Rchol)
        else:
            lpy = numpy.empty(N)
            for i in xrange(N):
                Rchol = scipy.linalg.cho_factor(R[i], check_finite=False)
                lpy[i] = kalman.lognormpdf_cho(diff[i], Rchol)

        return lpy