Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
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