added unscented_GT evaluate_from_sigma_points and test cases

src/generalized_unsented_transform/evaluate_from_sigma_points.py

+from numpy.linalg import matrix_power
+import numpy as np
+
+
+
+'''
+ This function is a tool that is used to evaluate the sample statistics 
+ of sigma points of an unscented transform
+
+ INPUTS
+
+   sigma_points        -   Matrix of sigma points
+   weights             -   Vector of weights corresponding to each sigma point
+
+ OUTPUTS
+
+   sigma_mean          -   Sample mean of sigma points
+   sigma_cov           -   Sample covariance matrix of sigma points
+   sigma_skew          -   Sample diagonal component of skewness tensor
+   sigma_kurt          -   Sample diagonal component of kurtosis tensor
+'''
+
+def Evaluate_sample_statistics(sigma_points, weights):
+
+    n = sigma_points.shape[0]
+    # row_weights = []
+    # weights = weights.reshape(1, -1) # Convert to row vector (need testing)
+    # Mean
+    sigma_mean = np.sum(np.matmul(sigma_points, np.tile(weights,(n, 1))))
+    # Covariance
+    Z = (sigma_points - sigma_mean) 
+    # print(np.diag(np.transpose(weights)[:,:]))
+    sigma_cov = np.sum(Z * np.diag(np.array(np.transpose(weights))[0])* np.transpose(Z))
+    # Diagonal skewness
+    sigma_skew = np.sum(np.matmul(np.power(Z, 3), np.tile(weights,(n, 1))))
+    # Diagonal kurtosis
+    sigma_kurt = np.sum(np.matmul(np.power(Z, 4), np.tile(weights,(n, 1))))
+    return sigma_mean, sigma_cov, sigma_skew, sigma_kurt

src/generalized_unsented_transform/generalized_unscented_transform.py

+from concurrent.futures.process import _MAX_WINDOWS_WORKERS
+import numpy as np
+from scipy.linalg import sqrtm
+from numpy.linalg import matrix_power, solve
+import warnings
+
+
+'''
+mu          -   Mean of random vector
+P           -   Covariance matrix
+x_skew      -   Vector of diagonal components of the skewness tensor
+x_kurt      -   Vector of diagonal components of the kurtosis tensor
+lb          -   Vector of lower bound of the state
+ub          -   Vector of upper bound of the state
+
+OUTPUTS
+
+x           -   Matrix of sigma points
+weights     -   Vector of weights corresponding to each sigma point
+s           -   Vector of proportionality constants used to generate
+                the sigma points
+'''
+def generalized_ut(mu, P, x_skew=None, x_kurt=None, lb=None, ub=None):
+
+    # Get the number of states
+    n = len(mu)
+    # Evaluate the matrix square root via singular value decomposition
+    # U1, S, Vh = np.linalg.svd(P)
+    # C = U1 * np.diag(np.sqrt(np.diag(S))) * U1
+    C = sqrtm(P)
+    # Handle the arguments for skewness and kurtosis
+    if x_skew == None:    # If no diagonal component of skewness is specified
+        warnings.warn('No skewness specified: Gaussian skewness and kurtosis is assumed')
+        x_skew = 0*mu          # Assume gaussian skewness if not provided
+        x_kurt = 3*np.ones(n)   # Assume gaussian diagonal kurtosis in turn
+    
+    if x_kurt == None:    # If no diagonal component of kurtosis is specified
+        warnings.warn('No kurtosis specified: kurtosis is selected to satisfy skewness kurtosis relationship')
+        # Manually ensure kurtosis skew relationship is satisfied
+        x_kurt = matrix_power(C, 4) * matrix_power(solve((matrix_power(C, 3)), x_skew), 2)      
+        x_kurt = 1.1 * x_kurt
+    
+    # Handle when specified kurtosis violates skewness kurtosis relationship
+    minkurt = matrix_power(C, 4) * matrix_power(solve((matrix_power(C, 3)), x_skew), 2)
+
+    if (np.sum(x_kurt < minkurt)):
+        warnings.warn('Bad Human Error: Kurtosis does not correspond to a distribution')
+        for i in range(len(x_kurt)):
+            if x_kurt[i] < minkurt[i]:
+                x_kurt[i] = 1.001* minkurt[i]
+        
+    # Handle the arguments for lower bounds and upper bounds
+    if lb == None: # If lower bound is not specified manually set lower bound as -inf
+        lb = -np.inf * np.ones(n)
+
+    if ub == None: # If lower bound is not specified manually set lower bound as -inf
+        ub = np.inf * np.ones(n)
+
+    # Calculate parameters u and v
+    u = 0.5* ( -(solve(matrix_power(C, 3), x_skew) ) 
+        + np.sqrt(4 * solve(matrix_power(C, 4), x_kurt) 
+        - 3 * matrix_power(solve(matrix_power(C, 3) , x_skew), 2)))
+    v = u + solve(matrix_power(C, 3), x_skew)
+    # Generate the sigma points
+    x0 = mu
+    x1 = mu - C * np.diag(u)
+    x2 = mu + C * np.diag(v)
+
+    # # --------------- This section handles the constraints  --------------- 
+    # Flag_constrain = 0     # Default flag to enforce constraint
+    # # Check if mean violates constraints
+    # if np.subtract(mu, lb).min() < 0 or np.subtract(mu, lb).min() < 0:
+    #     Flag_constrain = 1  # Set flag to avoid enforcing state constraints
+    #     warnings.warn('Unable fo enforce constraints: one or more of the mean does not satisfy lb < mean < ub')
+
+    # if Flag_constrain == 0:
+    #     theta = 0.9;    # Default value of user defined slack parameter
+        
+    #     # Ensure lower bound 'lb' is not violated
+    #     Temp1 = np.subtract(np.hstack((x1, x2)), lb)
+    #     L1 = np.nonzero(Temp1.min() < 0)   # Find the location of sigma points that violate the lower bound
+    #     Flag_calc = 0;      # Flag that determines if skewness can be matched
+    #     print(u)
+    #     print(v)
+    #     print(n)
+    #     print(L1)
+    #     for i in range(len(L1)):
+    #         if L1[i] <= n:
+    #             # Recalculate 'u' to satisfy lower bound 'lb'
+    #             u[L1[i]] = theta * np.min(np.abs(np.subtract(mu, lb) / C[:, L1[i]]))
+    #         else:
+    #             # Recalculate 'v' to satisfy lower bound 'lb'
+    #             print(L1[i])
+    #             v[L1[i] - n] = theta * np.min(np.abs(np.subtract(mu, lb) / C[:, L1[i]-n]))
+    #             Flag_calc = 1   # Set flag
+
+    #     # Regenerate the sigma points
+    #     x1 = mu - C * np.diag(u)
+    #     x2 = mu + C * np.diag(v)
+        
+    #     #     Ensure upper bound 'ub' is not violated
+    #     Temp2 = ub  - np.hstack((x1, x2))
+    #     L2 = np.nonzero(min(Temp2) < 0)    # Find the location of sigma points that
+    #                                        # violate the upper bound
+    #     for i in range(len(L2)): 
+    #         if L2(i) <= n:
+    #             # Recalculate 'u' to satisfy upper bound 'ub'
+    #             u[L2[i]] = theta * np.min(np.abs(np.subtract(mu, lb) / C[:, L1[i]]))
+    #         else:
+    #             # Recalculate 'v' to satisfy upper bound 'ub'
+    #             v[L2[i] - v] = theta * np.min(np.abs(np.subtract(ub, mu) / C[:, L2[i]-n]))
+    #             Flag_calc = 1   # Set flag
+
+    #     if Flag_calc == 0:
+    #         # Now recalculate parameter 'v' to match diagonal componen of 
+    #         # skewness tensor because it was 'v' was not previously redefined
+    #         v = u + np.solve(matrix_power(C, 3), x_skew)  # only done of v was not redefined
+        
+    #     # Regenerate the sigma points to reflect any change in 'u' or 'v'
+    #     x1 = mu - C * np.diag(u)
+    #     x2 = mu + C * np.diag(v)
+
+    # Recalculate weights to reflect any change in 'u' or 'v'
+    w2 = np.ones(n) / v  / np.add(u, v)
+    w1 = w2 * v / u
+    # Output sigma point values
+    x = np.hstack((x0, x1, x2))
+    w0 = 1 - np.sum(np.vstack((w1, w2)), axis = 0)
+    weights = np.transpose(np.hstack((w0, np.transpose(w1), np.transpose(w2))))
+    # s = np.vstack((u, v))
+    # print(x)
+    # print(weights)
+    # print(s)
+    return x, weights
\ No newline at end of file

src/generalized_unsented_transform/random_variable_test.py

+import numpy as np
+from generalized_unscented_transform import generalized_ut
+from evaluate_from_sigma_points import Evaluate_sample_statistics
+
+
+def quadratic_transform(sigma_points):
+    sigma_points = np.array(sigma_points)[0]
+    y = []
+    for x in sigma_points:
+        y.append(3*x + 2*(x**2))
+    y = np.matrix(y)
+    return y
+
+
+# Gaussian case
+def test_Gaussian():
+    m1 = 1
+    m2 = 4
+    m3 = 0
+    m4 = 48
+    sigma_points, weights = generalized_ut(np.matrix([m1]), np.matrix([m2]), np.matrix([m3]), np.matrix([m4]))
+    sigma_mean, sigma_cov, sigma_skew, sigma_kurt = Evaluate_sample_statistics(quadratic_transform(sigma_points), weights)
+    print(sigma_mean)
+    print(sigma_cov)
+    print(sigma_skew)
+    print(sigma_kurt)
+
+def test_Exp():
+    m1 = 0.5
+    m2 = 0.25
+    m3 = 0.25
+    m4 = 0.5625
+    sigma_points, weights = generalized_ut(np.matrix([m1]), np.matrix([m2]), np.matrix([m3]), np.matrix([m4]))
+    sigma_mean, sigma_cov, sigma_skew, sigma_kurt = Evaluate_sample_statistics(quadratic_transform(sigma_points), weights)
+    print(sigma_mean)
+    print(sigma_cov)
+    print(sigma_skew)
+    print(sigma_kurt)
+
+
+if __name__ == "__main__":
+    test_Exp()