def sigma_points(self, x, P):
        """
        Computes the implex sigma points for an unscented Kalman filter
        given the mean (x) and covariance(P) of the filter.
        Returns tuple of the sigma points and weights.

        Works with both scalar and array inputs:
        sigma_points (5, 9, 2) # mean 5, covariance 9
        sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I

        Parameters
        ----------

        x : An array-like object of the means of length n
            Can be a scalar if 1D.
            examples: 1, [1,2], np.array([1,2])

        P : scalar, or np.array
           Covariance of the filter. If scalar, is treated as eye(n)*P.

        Returns
        -------

        sigmas : np.array, of size (n, n+1)
            Two dimensional array of sigma points. Each column contains all of
            the sigmas for one dimension in the problem space.

            Ordered by Xi_0, Xi_{1..n}
        """

        if self.n != np.size(x):
            raise ValueError("expected size(x) {}, but size is {}".format(
                self.n, np.size(x)))

        n = self.n

        if np.isscalar(x):
            x = np.asarray([x])
        x = x.reshape(-1, 1)

        if np.isscalar(P):
            P = np.eye(n) * P
        else:
            P = np.atleast_2d(P)

        U = self.sqrt(P)

        lambda_ = n / (n + 1)
        Istar = np.array([[-1/np.sqrt(2*lambda_), 1/np.sqrt(2*lambda_)]])
        for d in range(2, n+1):
            row = np.ones((1, Istar.shape[1] + 1)) * 1. / np.sqrt(lambda_*d*(d + 1))
            row[0, -1] = -d / np.sqrt(lambda_ * d * (d + 1))
            Istar = np.r_[np.c_[Istar, np.zeros((Istar.shape[0]))], row]

        I = np.sqrt(n)*Istar
        scaled_unitary = U.dot(I)

        sigmas = self.subtract(x, -scaled_unitary)
        return sigmas.T
def sigma_points ( self , x , P ) : if self . n != np . size ( x ) : raise ValueError ( "expected size(x) {}, but size is {}" . format ( self . n , np . size ( x ) ) ) n = self . n if np . isscalar ( x ) : x = np . asarray ( [ x ] ) x = x . reshape ( - 1 , 1 ) if np . isscalar ( P ) : P = np . eye ( n ) * P else : P = np . atleast_2d ( P ) U = self . sqrt ( P ) lambda_ = n / ( n + 1 ) Istar = np . array ( [ [ - 1 / np . sqrt ( 2 * lambda_ ) , 1 / np . sqrt ( 2 * lambda_ ) ] ] ) for d in range ( 2 , n + 1 ) : row = np . ones ( ( 1 , Istar . shape [ 1 ] + 1 ) ) * 1. / np . sqrt ( lambda_ * d * ( d + 1 ) ) row [ 0 , - 1 ] = - d / np . sqrt ( lambda_ * d * ( d + 1 ) ) Istar = np . r_ [ np . c_ [ Istar , np . zeros ( ( Istar . shape [ 0 ] ) ) ] , row ] I = np . sqrt ( n ) * Istar scaled_unitary = U . dot ( I ) sigmas = self . subtract ( x , - scaled_unitary ) return sigmas . T
Computes the implex sigma points for an unscented Kalman filter
        given the mean (x) and covariance(P) of the filter.
        Returns tuple of the sigma points and weights.

        Works with both scalar and array inputs:
        sigma_points (5, 9, 2) # mean 5, covariance 9
        sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I

        Parameters
        ----------

        x : An array-like object of the means of length n
            Can be a scalar if 1D.
            examples: 1, [1,2], np.array([1,2])

        P : scalar, or np.array
           Covariance of the filter. If scalar, is treated as eye(n)*P.

        Returns
        -------

        sigmas : np.array, of size (n, n+1)
            Two dimensional array of sigma points. Each column contains all of
            the sigmas for one dimension in the problem space.

            Ordered by Xi_0, Xi_{1..n}