Skip to content
Snippets Groups Projects
Commit 4d3fa2dd authored by Ryan's avatar Ryan
Browse files

added unscented_GT evaluate_from_sigma_points and test cases

parent 59ef396e
Branches master
No related tags found
No related merge requests found
File added
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
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
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()
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment