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
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