diff --git a/src/generalized_unsented_transform/__pycache__/evaluate_from_sigma_points.cpython-38.pyc b/src/generalized_unsented_transform/__pycache__/evaluate_from_sigma_points.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f46aad2a26990e15db0104cc86dcb03e11cf0bfe Binary files /dev/null and b/src/generalized_unsented_transform/__pycache__/evaluate_from_sigma_points.cpython-38.pyc differ diff --git a/src/generalized_unsented_transform/__pycache__/generalized_unscented_transform.cpython-38.pyc b/src/generalized_unsented_transform/__pycache__/generalized_unscented_transform.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..3349b0f03ac31167b4a9925486777134f77206ea Binary files /dev/null and b/src/generalized_unsented_transform/__pycache__/generalized_unscented_transform.cpython-38.pyc differ diff --git a/src/generalized_unsented_transform/evaluate_from_sigma_points.py b/src/generalized_unsented_transform/evaluate_from_sigma_points.py new file mode 100644 index 0000000000000000000000000000000000000000..1a82ac3a4b28419884e10f2d33c5aa81aaaa4cef --- /dev/null +++ b/src/generalized_unsented_transform/evaluate_from_sigma_points.py @@ -0,0 +1,38 @@ +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 diff --git a/src/generalized_unsented_transform/generalized_unscented_transform.py b/src/generalized_unsented_transform/generalized_unscented_transform.py new file mode 100644 index 0000000000000000000000000000000000000000..3800af8919943d7e97b57bd2e4ae758e7938494f --- /dev/null +++ b/src/generalized_unsented_transform/generalized_unscented_transform.py @@ -0,0 +1,134 @@ +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 diff --git a/src/generalized_unsented_transform/random_variable_test.py b/src/generalized_unsented_transform/random_variable_test.py new file mode 100644 index 0000000000000000000000000000000000000000..eda6df0a3ee6845a7970943c985995059a084e35 --- /dev/null +++ b/src/generalized_unsented_transform/random_variable_test.py @@ -0,0 +1,42 @@ +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()