def _calculate_gamma(self, posterior_mean_estimation_risk_level, posterior_covariance_estimation_risk_level, max_volatility, num_assets):
# pylint: disable=invalid-name
"""
Calculate the gamma values appearing in the robust bayesian allocation objective and risk constraint.
:param posterior_mean_estimation_risk_level: (float) Denotes the confidence of investor to estimation risk in posterior mean. Lower value corresponds
to less confidence and a more conservative investor while a higher value will result in a more
risky portfolio.
:param posterior_covariance_estimation_risk_level: (float) Denotes the confidence of investor to estimation risk in posterior covariance. Lower value
corresponds to less confidence and a more conservative investor while a higher value will
result in a more risky portfolio.
:param max_volatility: (float) The maximum preferred volatility of the final robust portfolio.
:param num_assets: (int) Number of assets in the portfolio.
:return: (float, float) gamma mean, gamma covariance
"""
mean_risk_aversion = chi2.ppf(posterior_mean_estimation_risk_level, num_assets)
gamma_mean = np.sqrt((mean_risk_aversion / self.posterior_mean_confidence) *
(self.posterior_covariance_confidence / (self.posterior_covariance_confidence - 2)))
covariance_risk_aversion = chi2.ppf(posterior_covariance_estimation_risk_level,
num_assets * (num_assets + 1) / 2)
gamma_covariance = max_volatility / \
(self.posterior_covariance_confidence / (self.posterior_covariance_confidence + num_assets + 1) +
np.sqrt(2 * self.posterior_covariance_confidence * self.posterior_covariance_confidence * covariance_risk_aversion /
((self.posterior_covariance_confidence + num_assets + 1) ** 3)))
return gamma_mean, gamma_covariance
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
#0-----associate/match-----[alpha]-----ambiguous-----[beta]-----new_landmark-----
#pdb.set_trace()
alpha = chi2.ppf(0.95, df=2)
beta = chi2.ppf(0.999, df=2)
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
measurements = np.array(measurements)[:, 0:2]
zhat = np.zeros([ekf_state["num_landmarks"], 2])
S = np.zeros([ekf_state["num_landmarks"], 2, 2])
Q = np.diag(
np.array([
sigmas['range'] * sigmas['range'],
sigmas['bearing'] * sigmas['bearing']
]))
for j in range(ekf_state["num_landmarks"]):
zhat[j], H = laser_measurement_model(ekf_state, j)
S[j] = np.matmul(np.matmul(H, ekf_state['P']), H.T) + Q.T
M = alpha * np.ones((measurements.shape[0],
ekf_state["num_landmarks"] + measurements.shape[0]))
for i in range(measurements.shape[0]):
residuals = measurements[i] - zhat
for j in range(ekf_state["num_landmarks"]):
mahalanobis_dist = np.matmul(
residuals[j], np.matmul(np.linalg.inv(S[j]), residuals[j].T))
M[i, j] = mahalanobis_dist
matches = slam_utils.solve_cost_matrix_heuristic(np.copy(M))
matches.sort()
assoc = list(range(measurements.shape[0]))
for k in range(measurements.shape[0]):
if (matches[k][1] >= ekf_state['num_landmarks']):
if (np.amin(M[k, 0:ekf_state['num_landmarks']]) >
beta): #new landmark
assoc[matches[k][0]] = -1
else: #ambiguous
assoc[matches[k][0]] = -2
else: #matched
assoc[matches[k][0]] = matches[k][1]
return assoc
def wave_signif(t, siglevel=0.95, lag1=0.0, test='local', dof=None, scale_range=None):
fft_theor = red_spectrum(lag1, t.dt / t.period)
fft_theor *= t.series.var() # Include time-series variance
# No smoothing, DOF = dofmin (Sec. 4)
if test == 'local':
return fft_theor * chi2.ppf(siglevel, t.wavelet.dofmin) / t.wavelet.dofmin
# Time-averaged significance
elif test == 'global':
# Eqn. 23
dof = t.wavelet.dofmin * sp.sqrt(1 + ((len(t.series) * t.dt) /
(t.wavelet.gamma * t.scales))**2)
dof[dof < t.wavelet.dofmin] = t.wavelet.dofmin # minimum DOF is dofmin
return fft_theor * chi2.ppf(siglevel, dof) / dof
elif test == 'scale':
if not scale_range:
raise ValueError("Must supply a scale_range for time-averaged \
significance testing.")
if period:
scale_indices = (transform.period >= min(scale_range)) \
& (transform.period <= max(scale_range))
else:
scale_indices = (transform.scales >= min(scale_range)) \
& (transform.scales <= max(scale_range))
scale_indices = sp.arange(len(scale_indices))[scale_indices]
na = len(t.series)
savg = scale_avg(t, min(scale_range), max(scale_range))
smid = t.minscale * 2 ** (0.5 * (min(scale_range) + max(scale_range)) * t.dscale)
dof = 2 * len(na) * savg[1] / smid \
* sp.sqrt(1 + (na * t.dscale / t.wavelet.dj0)**2)
P = savg[1] * (fft_theor[scale_indices] / t.scales[scale_indices]).sum()
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
###
# Implement this function.
###
Q = np.eye(2, 2)
Q[0][0] = sigmas['range']**2
Q[1][1] = sigmas['bearing']**2
xv, yv, phi = ekf_state['x'][:3]
landmark_state = ekf_state['x'][3:]
landmarks = np.zeros((2, ekf_state['num_landmarks']))
landmarks[0:] = landmark_state[::2]
landmarks[1:] = landmark_state[1::2]
landmark_rb = np.zeros((2, ekf_state['num_landmarks']))
landmark_rb[0, :] = np.sqrt((landmarks[0, :] - xv)**2 +
(landmarks[1, :] - yv)**2)
landmark_rb[1, :] = np.arctan2(landmarks[1, :] - yv,
landmarks[0, :] - xv) - phi + np.pi / 2
M = np.zeros((len(measurements), ekf_state['num_landmarks']))
# r = measurements - landmark_rb
# H = np.zeros((2, 3+2*ekf_state['num_landmarks']))
r = np.zeros((2, ))
for i in range(len(measurements)):
for j in range(ekf_state['num_landmarks']):
zhat, H = laser_measurement_model(ekf_state, j)
Sinv = np.linalg.inv(
np.matmul(np.matmul(H, ekf_state['P']), H.T) + Q)
r[0] = measurements[i][0] - zhat[0]
r[1] = measurements[i][1] - zhat[1]
dist = np.matmul(np.matmul(r.T, Sinv), r)
M[i, j] = dist
assoc = [-2] * len(measurements)
padding = np.zeros(
(len(measurements), len(measurements))) + chi2.ppf(0.95, df=2)
cost = np.hstack((M, padding))
result = slam_utils.solve_cost_matrix_heuristic(cost.copy())
for index, (i, j) in enumerate(result):
if j < ekf_state['num_landmarks']:
assoc[i] = j
elif np.min(M[i, :ekf_state['num_landmarks']]) > chi2.ppf(0.99, df=2):
assoc[i] = -1
return assoc
def interval_border(variable, alpha):
length = len(variable)
high = chi2.ppf(alpha / 2, df=(length - 1))
low = chi2.ppf(1 - alpha / 2, df=(length - 1))
variable_displaced_variance = displaced_variance(variable)
low_border = variable_displaced_variance * (length - 1) / low
high_border = variable_displaced_variance * (length - 1) / high
return low_border, high_border
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
###
# Implement this function.
###
trees = measurements
append_M = np.ones(
(len(measurements), len(measurements))) * chi2.ppf(0.95, df=2)
measurements = [(tree[0], tree[1]) for tree in trees]
assoc = -1 * np.ones((len(measurements), ))
Q = np.eye(2, 2)
Q[0][0] = sigmas['range'] * sigmas['range']
Q[1][1] = sigmas['bearing'] * sigmas['bearing']
M = np.zeros((len(measurements), ekf_state['num_landmarks']))
for i in range(len(measurements)):
m = np.asarray(measurements[i])
for j in range(ekf_state['num_landmarks']):
z_hat, H = laser_measurement_model(ekf_state, j)
each_r = (m - z_hat).T
S = np.matmul(np.matmul(H, ekf_state['P']), H.T) + Q.T
mahal = np.matmul(np.matmul(each_r.T, np.linalg.inv(S)), each_r)
M[i][j] = mahal
M = np.hstack((M, append_M))
result = slam_utils.solve_cost_matrix_heuristic(M.copy())
assoc = [-2] * len(measurements)
for (i, j) in result:
if j < ekf_state['num_landmarks']:
assoc[i] = j
else:
if np.min(M[i, :ekf_state['num_landmarks']]) > chi2.ppf(0.99,
df=2):
assoc[i] = -1
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
###
# Implement this function.
###
# print(ekf_state["num_landmarks"])
P = ekf_state['P']
R = np.diag([sigmas['range']**2, sigmas['bearing']**2])
A = np.full((len(measurements), len(measurements)), chi2.ppf(0.96, df=2))
cost_mat = np.full((len(measurements), ekf_state['num_landmarks']),
chi2.ppf(0.96, df=2))
for k in range(0, len(measurements)):
for j in range(0, ekf_state['num_landmarks']):
z_hat, H = laser_measurement_model(ekf_state, j)
# print(measurements[k][0:2])
r = np.array(np.array(measurements[k][0:2]) - np.array(z_hat))
S_inv = np.linalg.inv(
np.matmul(np.matmul(H, P), np.transpose(H)) + R)
MD = np.matmul(np.matmul(np.transpose(r), S_inv), r)
cost_mat[k, j] = MD
cost_mat_conc = np.concatenate((cost_mat, A), axis=1)
temp1 = np.copy(cost_mat)
results = slam_utils.solve_cost_matrix_heuristic(temp1)
assoc = np.zeros(len(measurements), dtype=np.int32)
for k in range(0, len(results)):
# print(cost_mat[results[k][0],results[k][1]])
if cost_mat_conc[results[k][0], results[k][1]] > chi2.ppf(0.99, df=2):
assoc[results[k][0]] = -1
elif cost_mat_conc[results[k][0], results[k][1]] >= chi2.ppf(0.95,
df=2):
assoc[results[k][0]] = -2
else:
assoc[results[k][0]] = results[k][1]
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
# assoc = np.zeros((len(measurements),1))
# assoc = np.asarray([-1])
return [-1 for m in measurements]
###
# Implement this function.
###
Qt = np.diag([sigmas['range']**2, sigmas['bearing']**2])
zk = np.asarray(measurements)
n_landmarks = ekf_state['num_landmarks']
n_measurements = zk.shape[0]
M = np.zeros((n_measurements, n_landmarks))
# Thresholds for classifying as New or Ambiguous Landmarks
alpha = chi2.ppf(0.95, 2)
beta = chi2.ppf(0.99, 2)
for k in range(n_landmarks):
zhat, H = laser_measurement_model(ekf_state, k)
S = np.matmul(H, np.matmul(ekf_state['P'], H.T)) + Qt
Sinv = slam_utils.invert_2x2_matrix(S)
innovation = zk[:, :2] - zhat.T
M[:, k] = np.sum(innovation.T * np.matmul(Sinv, innovation.T), axis=0)
# Augmented Matrix with Cost Matrix
pairs = slam_utils.solve_cost_matrix_heuristic(
np.hstack((M, alpha * np.ones((n_measurements, n_measurements)))))
pairs.sort()
pairs = np.asarray(pairs)
assoc = pairs[:, 1]
assoc = np.where(assoc >= n_landmarks, -1, assoc)
for i in range(assoc.shape[0]):
if assoc[i] == -1 and np.any(M[i, :] < beta):
assoc[i] = -2
return assoc
def main():
gen_data = gen2D(1, 1, 1e-2,
1e-2) # init real values, measured values etc.
index = 5 # For saving file purposes
extKF_T = ExtendedKF(0.5, gen_data.z, gen_data.Q1, gen_data.Q2,
gen_data.R1, gen_data.R2) # T value for initialising
extKF_T.get_x_hat_0(gen_data.z)
extKF_T.KalmanFiltering()
x_hat_c = np.delete(np.asarray(extKF_T.x_hat), (0), axis=0)
x_hat_minus_c = np.delete(np.asarray(extKF_T.x_hat_minus), (0), axis=0)
C_c = np.delete(np.asarray(extKF_T.matC), (0), axis=0)
nees = NEES(gen_data.x.reshape(1507, 4, 1), x_hat_c, extKF_T.P_corr_list)
nis = NIS(gen_data.z.reshape(1507, 2, 1), x_hat_minus_c,
np.asarray(extKF_T.s_k), C_c)
max_nees = chi2.ppf(0.95, df=4)
max_nis = chi2.ppf(0.95, df=2)
test = np.asarray(extKF_T.x_hat[:-1])
plt.plot()
plt.plot(test[:, 0], test[:, 2], label="Filter est.")
plt.plot(gen_data.x[:, 0], gen_data.x[:, 2], label="True pos.")
plt.title("Position Map for Q:[%0.3f %0.3f] R:[%0.3f %0.3f]" %
(gen_data.Q1, gen_data.Q2, gen_data.R1, gen_data.R2))
plt.legend(loc='upper right')
plt.xlabel('x-axis [m]')
plt.ylabel('y-axis [m]')
plt.savefig('%i Position.png' % (index), dpi=300)
plt.show()
plt.subplot(211)
plt.plot(nees, linestyle='-', marker='x')
plt.axhline(max_nees, linestyle='--', color='r', label='5% tail point')
plt.xlabel('Iteration')
plt.ylabel('NEES')
plt.title("NEES Values for Q:[%0.3f %0.3f] R:[%0.3f %0.3f]" %
(gen_data.Q1, gen_data.Q2, gen_data.R1, gen_data.R2))
plt.legend(loc='upper right')
plt.subplot(212)
plt.plot(nis, linestyle='-', marker='x')
plt.axhline(max_nis, linestyle='--', color='r', label='5% tail point')
plt.xlabel('Iteration')
plt.ylabel('NIS')
plt.title("NIS Values for Q:[%0.3f %0.3f] R:[%0.3f %0.3f]" %
(gen_data.Q1, gen_data.Q2, gen_data.R1, gen_data.R2))
plt.legend(loc='upper right')
plt.tight_layout()
plt.savefig('%i NEES-NIS.png' % (index), dpi=300)
plt.show()
def _extremePoints(self):
WRMS = []
WNA = []
FNA = []
pyGCTSpath = "{}".format(os.environ['pyGCTS'])
for file in glob.iglob(pyGCTSpath + '/metaData/results*',
recursive=True):
with open(file, 'r') as ff:
for line in ff.readlines():
WRMS.append(float(line.split()[1]))
WNA.append(float(line.split()[2]))
FNA.append(float(line.split()[3]))
ff.close()
WRMSnew = (np.asarray(WRMS)).reshape(len(WRMS), 1)
WNAnew = (np.asarray(WNA)).reshape(len(WNA), 1)
FNAnew = (np.asarray(FNA)).reshape(len(FNA), 1)
alphaNew = 1 - np.sqrt(1 - self.alpha)
pLower_wna = qr(WRMSnew, WNAnew, (1 - alphaNew / 2))
pUpper_wna = qr(WRMSnew, WNAnew, (alphaNew / 2))
pLower_fna = qr(WRMSnew, FNAnew, (1 - alphaNew / 2))
pUpper_fna = qr(WRMSnew, FNAnew, (alphaNew / 2))
funWRMS = lambda wrms, alp, dof: np.array([
np.sqrt((dof * wrms**2) / chi2.ppf(1 - alp / 2, dof)),
np.sqrt((dof * wrms**2) / chi2.ppf(alp / 2, dof))
])
WRMSlims = funWRMS(self.WRMS, alphaNew, self.dof)
funWNA = lambda horlims, pUpwna, pLowna: np.array([
pUpwna[0] * horlims[0] + pUpwna[1], pUpwna[0] * horlims[1] +
pUpwna[1], pLowna[0] * horlims[1] + pLowna[1], pLowna[0] * horlims[
0] + pLowna[1]
])
extWNAs = funWNA(WRMSlims, pUpper_wna, pLower_wna)
funFNA = lambda horlims, pUpfna, pLofna: np.array([
pUpfna[0] * horlims[0] + pUpfna[1], pUpfna[0] * horlims[1] +
pUpfna[1], pLofna[0] * horlims[1] + pLofna[1], pLofna[0] * horlims[
0] + pLofna[1]
])
extFNAs = funFNA(WRMSlims, pUpper_fna, pLower_fna)
#np.savetxt('6-extWNAs.dat', extWNAs, fmt="%10.4f")
#np.savetxt('7-extFNAs.dat', extFNAs, fmt="%10.4f")
#np.savetxt('8-WRMSlims.dat', WRMSlims, fmt="%10.4f")
#np.savetxt('11-wna_lo.dat', pLower_wna, fmt="%10.4f")
#np.savetxt('12-wna_up.dat', pUpper_wna, fmt="%10.4f")
#np.savetxt('13-fna_lo.dat', pLower_fna, fmt="%10.4f")
#np.savetxt('14-fna_up.dat', pUpper_fna, fmt="%10.4f")
return extWNAs, extFNAs, WRMSlims
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
###
P = ekf_state["P"].copy()
n = ekf_state['num_landmarks']
m = len(measurements)
cost_matrix = np.zeros([m, n + 1])
cost_matrix[:, n] = chi2.ppf(0.99, 2)
R = np.diag([sigmas['range']**2, sigmas['bearing']**2])
for i in range(n):
zhat, H = laser_measurement_model(ekf_state, i)
S = np.dot(np.dot(H, P), H.transpose()) + R
inv_S = slam_utils.invert_2x2_matrix(S)
for j in range(m):
update = np.asarray(measurements[j][0:2]) - zhat.flatten()
cost_matrix[j, i] = np.dot(np.dot(update.transpose(), inv_S),
update)
result = slam_utils.solve_cost_matrix_heuristic(cost_matrix)
assoc = [0] * m
for i, j in result:
if j < ekf_state["num_landmarks"]:
assoc[i] = j
else:
if min(cost_matrix[i, 0:]) < chi2.ppf(0.99, 2):
assoc[i] = -2
else:
assoc[i] = -1
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
return [-1 for m in measurements]
n_lmark = ekf_state['num_landmarks']
n_scans = len(measurements)
M = np.zeros((n_scans, n_lmark))
Q_t = np.array([[sigmas['range']**2, 0], [0, sigmas['bearing']**2]])
alpha = chi2.ppf(0.95, 2)
beta = chi2.ppf(0.99, 2)
A = alpha*np.ones((n_scans, n_scans))
for i in range(n_lmark):
zhat, H = laser_measurement_model(ekf_state, i)
S = np.matmul(H, np.matmul(ekf_state['P'],H.T)) + Q_t
Sinv = slam_utils.invert_2x2_matrix(S)
for j in range(n_scans):
temp_z = measurements[j][:2]
res = temp_z - np.squeeze(zhat)
M[j, i] = np.matmul(res.T, np.matmul(Sinv, res))
M_new = np.hstack((M, A))
pairs = slam_utils.solve_cost_matrix_heuristic(M_new)
pairs.sort()
pairs = list(map(lambda x:(x[0],-1) if x[1]>=n_lmark else (x[0],x[1]),pairs))
assoc = list(map(lambda x:x[1],pairs))
for i in range(len(assoc)):
if assoc[i] == -1:
for j in range(M.shape[1]):
if M[i, j] < beta:
assoc[i] = -2
break
return assoc
def gps_update(gps, ekf_state, sigmas):
'''
Perform a measurement update of the EKF state given a GPS measurement (x,y).
Returns the updated ekf_state.
'''
###
# Implement the GPS update.
###
r = np.array(gps - ekf_state['x'][0:2])
P = ekf_state['P']
R = np.diag([sigmas['gps']**2, sigmas['gps']**2])
H = np.zeros((2, ekf_state['x'].size))
H[0, 0] = 1
H[1, 1] = 1
S_inv = np.linalg.inv(P[0:2, 0:2] + R)
MD = np.matmul(np.matmul(np.transpose(r), S_inv), r)
if MD < chi2.ppf(0.999, df=2):
K = np.matmul(np.matmul(P, np.transpose(H)), S_inv)
ekf_state['x'] = ekf_state['x'] + np.matmul(K, r)
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
temp1 = np.identity(ekf_state['x'].size) - np.matmul(K, H)
ekf_state['P'] = slam_utils.make_symmetric(np.matmul(temp1, P))
# else:
# print("no gps")
return ekf_state
def gps_update(gps, ekf_state, sigmas):
'''
Perform a measurement update of the EKF state given a GPS measurement (x,y).
Returns the updated ekf_state.
'''
##
#Implement the GPS update.
##
H = np.zeros((2, 3 + 2 * ekf_state['num_landmarks']))
H[0, 0] = 1
H[1, 1] = 1
Q = np.eye(2, 2) * (sigmas['gps']**2)
Sigma = ekf_state['P']
r = gps - ekf_state['x'][:2]
Sinv = np.linalg.inv(np.matmul(np.matmul(H, Sigma), H.T) + Q.T)
d = np.matmul(np.matmul(r.T, Sinv), r)
if d > chi2.ppf(0.999, df=2):
return ekf_state
K = np.matmul(np.matmul(Sigma, H.T), Sinv)
ekf_state['x'] = ekf_state['x'] + np.matmul(K, r)
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
Sigma = np.matmul(
(np.eye(3 + 2 * ekf_state['num_landmarks'],
3 + 2 * ekf_state['num_landmarks']) - np.matmul(K, H)), Sigma)
ekf_state['P'] = slam_utils.make_symmetric(Sigma)
# print(ekf_state)
return ekf_state
def test_association_dependent_tracks(track1_mean,
track1_cov,
track2_mean,
track2_cov,
cross_cov_ij,
cross_cov_ji,
alpha=0.05):
"""
checks whether the tracks are from the same target, when the dependence is accounted for.
:param track1: track to check for association
:param track2: track to check for association
:param cross_cov_ij: cross-covariance of the estimation errors. See article
:param cross_cov_ji:
:param alpha: desired test power
:return: true if the tracks are from the same target, false else
"""
delta_estimates = track1_mean - track2_mean
error_delta_estimates = delta_estimates # as the difference of the true states is 0 if it is the same target
error_delta_estimates_covar = track1_cov + track2_cov - cross_cov_ij - cross_cov_ji
d = (error_delta_estimates.transpose()
@ np.linalg.inv(error_delta_estimates_covar)
@ error_delta_estimates)[0]
# 4 degrees of freedom as we have 4 dimensions in the state vector
d_alpha = chi2.ppf((1 - alpha), df=4)
# Accept H0 if d <= d_alpha
return d <= d_alpha
def demonstrated_reliability_sr(val, start, end, beta=1.21, CL=0.9, T=30000, ft=pd.DataFrame, size=10):
# failures array
f_arr = np.zeros(size)
# time points array
t_arr = np.linspace(start, end, size)
# populate the number of failures vs time array
# based on failures Dataframe information
if not(ft.empty):
for row in ft.values:
fl = np.where(t_arr > row[0].timestamp(), row[1], 0)
f_arr += fl
# initialize demonstrated Reliability vs. time array
dr_arr = []
# exeecute the algorithm
m = np.array([e.Cylinders for e in val.engines])
for i, t in enumerate(t_arr):
tt = np.array([e.oph(t) for e in val.engines])
tt_max = max(tt)
if tt_max > 0.0: # avoid division by zero
# sum all part's per lipson equality to max hours at time t
n_lip = sum(m*(tt/tt_max) ** beta)
# use Lipson equality again to calc n@T hours
n_lip_T = n_lip * ((tt_max/T) ** beta)
# calc demonstrated Reliability per Chi.square dist (see A.Kleyner Paper)
dr = np.exp(-chi2.ppf(CL, 2*(f_arr[i]+1))/(2*n_lip_T)) * 100.0
# store in list for numpy vector
dr_arr.append(dr)
else:
dr_arr.append(0.0)
return (t_arr, np.array(dr_arr), f_arr)
def __init__(self,
dt,
sensor_para,
P_G=0.9,
P_D=0.9,
ConfirmationThreshold=[6, 8],
DeletionThreshold=[7, 10],
isSimulation=True):
self.gating_size = chi2.ppf(P_G, df=2)
self.P_D = P_D # probability of detection
# Define gating size, for elliposidal gating, we need parameter P_G
self.P_G = P_G
self.dt = dt
self.F = np.matrix([[1, 0, self.dt, 0], [0, 1, 0, self.dt],
[0, 0, 1, 0], [0, 0, 0, 1]])
self.H = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0]])
self.Q = 1 * np.diag([0.1, 0.1, 0.5, 0.5])
self.P = self.Q
self.R = 1 * np.diag([0.1, 0.1])
self.common_P = 5 * np.linalg.det(self.R)
if DeletionThreshold[1] < ConfirmationThreshold[1]:
print(
"Deletion threshold period should be larger than confirmation threshold period"
)
self.ConfirmationThreshold = ConfirmationThreshold
self.DeletionThreshold = DeletionThreshold
self.isSimulation = isSimulation
self.track_list = []
self.track_list_next_index = []
# self.t = time.time() ## time synchronize will be important
self.sensor_para = sensor_para
def ellipsoid(E,
centre=(0, 0, 0),
scale=1,
confidence=None,
npoints=40,
inverted=False):
if E.shape != (3, 3):
raise ValueError('ellipsoid is defined by a 3x3 matrix')
if confidence:
# process the probability
from scipy.stats.distributions import chi2
s = math.sqrt(chi2.ppf(s, df=2)) * scale
else:
s = scale
if not inverted:
E = np.linalg.inv(E)
x, y, z = sphere() # unit sphere
e = s * sp.linalg.sqrtm(E) @ np.array(
[x.flatten(), y.flatten(), z.flatten()]) + np.c_[centre].T
return e[0, :].reshape(x.shape), e[1, :].reshape(x.shape), e[2, :].reshape(
x.shape)
def gating(self, MHD):
# check if measurement lies inside gate
limit = chi2.ppf(0.95, df=2)
if MHD < limit:
return True
else:
return False
def gps_update(gps, ekf_state, sigmas):
'''
Perform a measurement update of the EKF state given a GPS measurement (x,y).
Returns the updated ekf_state.
'''
###
# Implement the GPS update.
###
P = ekf_state['P']
dim = P.shape[0]-2
H = np.hstack((np.eye(2),np.zeros((2,dim))))
r = np.transpose([gps - ekf_state['x'][:2]])
Q = (sigmas['gps']**2)*(np.eye(2))
S = np.matmul(np.matmul(H,P),H.T) + Q
S_inv = slam_utils.invert_2x2_matrix(S)
d = np.matmul(np.matmul(r.T,S_inv),r)
if d <= chi2.ppf(0.999, 2):
K = np.matmul(np.matmul(P,H.T),S_inv)
ekf_state['x'] = ekf_state['x'] + np.squeeze(np.matmul(K,r))
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
P_temp = np.matmul((np.eye(P.shape[0])- np.matmul(K,H)),P)
ekf_state['P'] = slam_utils.make_symmetric(P_temp)
return ekf_state
def gps_update(gps, ekf_state, sigmas):
'''
Perform a measurement update of the EKF state given a GPS measurement (x,y).
Returns the updated ekf_state.
'''
###
# Implement the GPS update.
###
P = ekf_state['P']
residual = np.transpose([gps - ekf_state['x'][:2]])
H_mat = np.matrix(np.zeros([2, P.shape[0]]))
H_mat[0, 0], H_mat[1, 1] = 1, 1
R_mat = np.diag([sigmas['gps']**2, sigmas['gps']**2])
S_mat = H_mat * P * H_mat.T + R_mat
d = (np.matrix(residual)).T * np.matrix(
slam_utils.invert_2x2_matrix(np.array(S_mat))) * np.matrix(residual)
if d <= chi2.ppf(0.999, 2):
Kt = P * H_mat.T * np.matrix(
slam_utils.invert_2x2_matrix(np.array(S_mat)))
ekf_state['x'] = ekf_state['x'] + np.squeeze(
np.array(Kt * np.matrix(residual)))
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(
np.array((np.matrix(np.eye(P.shape[0])) - Kt * H_mat) * P))
return ekf_state
def gating(self, MHD, sensor):
############
# TODO Step 3: return True if measurement lies inside gate, otherwise False
############
p = 0.9995 #0.999 #params.gating_threshold #0.999 #0.95
limit = chi2.ppf(p, df = 2)
return MHD < limit
def find_connection(bucket, test_statistic):
test_statistic = calculate_test_statistic(bucket)
pValThreshold = chi2.ppf(.01, subSize ** 2)
too_similar = test_statistic < pValThreshold
# blocks are "connected" if they occur by chance < 1% of the time and
# do not overlap.
connection = np.any(np.logical_and(overlap, np.logical_not(too_similar)))
return connection
def _threshold(self, data: np.ndarray, deltas: np.ndarray,
precision: Optional[float]) -> Optional[float]:
if self.confidence is None:
return precision
if callable(self.confidence):
return self.confidence(data, deltas, precision) # pylint: disable=not-callable
return (precision *
chi2.ppf(1. - cast(float, self.confidence), self.window - 1) /
self.window)
def compute_var_interval_border(random_variable, alpha):
length = len(random_variable)
quantile_high = compute_chi2(random_variable, alpha / 2)
quantile_low = chi2.ppf(1 - alpha / 2, df=(length - 1))
variance = unbiased_sample_variance(random_variable)
low_border = variance * (length - 1) / quantile_low
high_border = variance * (length - 1) / quantile_high
return low_border, high_border
def plot_ness(ness, number_of_samples):
chi_squared = np.zeros(number_of_samples)
chi_squared = chi_squared + chi2.ppf(0.95, df=4)
plt.figure()
plt.plot(chi_squared, color='b', linestyle='dashed', label='chi-squared')
plt.plot(ness, color='r', label='NEES')
plt.legend()
plt.title('NEES for Q=0', fontweight='bold')
plt.xlabel('Step')
plt.ylabel('NEES values')
plt.show()
def _find_connection(subBlocks) -> np.ndarray[bool]:
'''calculate whether blocks are too similar to have occured by chance
blocks are "connected" if they occur by chance < 1% of the time and
do not overlap.'''
testStatistic = _calculate_test_statistic(subBlocks)
overlap = _find_overlap()
pValThreshold = chi2.ppf(.01, (subSize**2))
tooSimilar = testStatistic < pValThreshold
connection = np.logical_and(tooSimilar, np.logical_not(overlap))
return connection
def pirson(g, intervals, h):
n = len(g)
k = int(round(1.72 * n**(1 / 3), 0))
mu = expected_value(g)
sigma = variance(g)**(1 / 2)
all_p = norm.cdf(intervals, mu, sigma)
p = get_interval_p(all_p)
xhi2_list = []
for i in range(k):
xhi2_list.append((h[i] - n * p[i])**2 / n / p[i])
return sum(xhi2_list), chi2.ppf(0.95, df=k - 3), all_p, p
def test_cpp_splits():
"test cpp splits"
np.random.seed(0)
data = np.random.normal(0, 3e-3, 70).astype('f4')
data[:10] += 20
data[10:20] += np.linspace(20, 19, 10)
data[20:40] += 19
data[40:45] += np.linspace(19, 18, 5)
data[45:48] += 18
data[48:53] += np.linspace(18, 17, 5)
data[53:] += 17
der = np.array([
data[0] - np.mean(data[0:3]), data[0] - np.mean(data[1:4]),
(data[0] * 2 + data[1]) / 3. - np.mean(data[2:5])
] + [
np.mean(data[i - 3:i]) - np.mean(data[i:i + 3])
for i in range(3, data.size - 2)
] + [
np.mean(data[-5:-2]) - (data[-1] * 2 + data[-2]) / 3.,
np.mean(data[-4:-1]) - data[-1]
],
dtype='f4')
der = np.abs(der)
der /= np.percentile(der, 75.) + 3e-3
out = DerivateSplitDetector().grade(data, 3e-3)
assert np.max(np.abs(out - der)) < 5e-2
assert np.max(np.abs(out - der / der.max() * out.max())) < 2e-5
gx2 = np.array(
[np.var(data[max(0, i - 2):i + 3]) for i in range(data.size)],
dtype='f4')
gx2 = np.sqrt(gx2)
gx2 /= 3e-3 * chi2.ppf(.9, 4) / 5
out2 = ChiSquareSplitDetector(gradewindow=5).grade(data, 3e-3)
assert_allclose(out2[2:-2], gx2[2:-2], rtol=1e-6, atol=1e-5)
gmu = np.copy(out)
gmu[13:18] = out2[13:18]
gmu[42:44] = out2[42:44]
gmu[50:52] = out2[50:52]
cnf = MultiGradeSplitDetector(
chisquare=ChiSquareSplitDetector(gradewindow=5), minpatchwindow=3)
out3 = cnf.grade(data, 3e-3)
assert_allclose(out3, gmu)
ints = MultiGradeSplitDetector()(data, 3e-3)
assert tuple(tuple(i)
for i in ints) == ((0, 12), (19, 41), (44, 49), (52, 70))
data[1] = data[15] = data[50] = np.NaN
ints = MultiGradeSplitDetector()(data, 3e-3)
assert tuple(tuple(i)
for i in ints) == ((0, 12), (19, 41), (44, 48), (52, 70))
def main():
bound = ''.ljust(40, '*')
start = ' [ STARTED ] '.center(40, '*')
end = ' [ FINISHED ] '.center(40, '*')
print(bound + '\n' + start + '\n' + bound)
fileName = 'lesson7_mahal_diamonds.csv'
if fileName == 'lesson7_mahal_dataset_0.csv':
# Read data
# Taken from https://jamesmccaffrey.wordpress.com/2017/11/09/example-of-calculating-the-mahalanobis-distance/
# We assumed that the three variables are independent
data = pd.read_csv(fileName)
data.head()
test = pd.DataFrame([[66, 640, 44], [69, 595, 38]],
columns=list(['height', 'score', 'age']))
elif fileName == 'lesson7_mahal_diamonds.csv':
# Read data
# Taken from https://www.machinelearningplus.com/statistics/mahalanobis-distance/
data = pd.read_csv(fileName).iloc[:, [0, 4, 6]]
data.head()
test = data[['carat', 'depth', 'price']].head(5)
else:
sys.exit('[ERROR]: File not found!')
# Mahalanobis distance
test['mahalanobis'] = mahalanobis(test, data, None)
test.head()
# Probability (1-p) with which we are certain that, when the
# squared Mahalanobis distance is greater than the critical
# value (cv) associated with this probability and the proper DOF,
# then the test vector (tv) is an outlier
p = 0.001
dof = 3
cv = chi2.ppf(1 - p, dof)
outlier = []
critical_value = []
for i in range(len(test['mahalanobis'])):
critical_value.append(cv)
if test['mahalanobis'][i] > cv:
outlier.append('true')
else:
outlier.append('false')
test['critical-value'] = np.asarray(critical_value)
test['p_value'] = 1 - chi2.cdf(test['mahalanobis'], dof)
test['outlier'] = np.asarray(outlier)
print(test)
print(bound + '\n' + end + '\n' + bound)
return 0
def testVarianza(randoms):
n = len(randoms)
grade_liberty = n - 1
aceptation = 0.95
a = 1 - aceptation
variance = np.var(randoms, ddof=1)
a_2 = a / 2
a_3 = 1 - a_2
xa_1 = chi2.ppf(a_3, df=grade_liberty)
xa_2 = chi2.ppf(a_2, df=grade_liberty)
li = xa_1 / (12 * grade_liberty)
ls = xa_2 / (12 * grade_liberty)
if ls > li:
return (variance <= ls) & (variance >= li)
else:
return (variance >= ls) & (variance <= li)
def chi2inv(p, nfft, nperseg, test=None):
#
# External Function: Statistical Analysis
# Function to estimate the inverse of cumulative distribution function (percentile)
#
if test == None:
nw2 = 2 * (2.5164 * (nfft / nperseg)) * 1.2
return chi2.ppf(p, df=nw2) / nw2
else:
nw2 = (nfft / nperseg)
return 2 * sc.gammaincinv(nw2,
p) / nw2 # Inverse incomplete gamma function
def filter_matfile(fname, outstem, p_reject=0.001, plot=1):
stack = read_mat(fname)
md5 = md5_file(fname)
print("Rejection probability: %0.3g" % p_reject)
N = np.sum(np.logical_not(np.isnan(stack[0,0,1,:])))
print("Number of valid channels: %d" % N)
threshold = chi2.ppf(1.0 - p_reject, N) / N
print("Chisq rejection threshold: %0.3g" % threshold)
for pos in range(stack.shape[0]):
reps = stack[pos,...]
incinds, cdm = filter_outliers(reps, threshold=threshold, plot=plot)
ms = mean_stack(reps[incinds,...])
disinds = range(reps.shape[0])
for i in incinds:
disinds.remove(i)
print("Pos %d, discarded: %s" % (pos, str(disinds)))
ad = { 'chi2cutoff' : float(threshold),
'rejection_prob' : float(p_reject),
'incinds' : map(int, list(incinds)),
'disinds' : map(int, list(disinds)),
'chi2matrix' : map(float, list(cdm)),
'method' : "filter_outliers",
'inputfile' : [ fname, md5 ],
'inputposition' : int(pos),
'q~unit' : '1/nm',
'I~unit' : 'arb.',
'Ierr~unit' : 'arb.',
'I_first~unit' : 'arb.',
'Ierr_first~unit' : 'arb.',
'I_all~unit' : 'arb.',
'Ierr_all~unit' : 'arb.',
}
outarr = np.zeros((7, ms.shape[1]))
outarr[0:3,:] = ms
outarr[3:5,:] = reps[0,1:3,:]
outarr[5:7,:] = mean_stack(reps)[1:3,:]
outname = "%s.p%02d.out.ydat" % (outstem, pos)
print(outname)
write_ydat(outarr, outname, addict=ad,
cols=['q','I','Ierr','I_first','Ierr_first','I_all','Ierr_all'],
attributes=['~unit'])
def findConnection(self,testStatistic,overLap,subSize):
pvalThreshold = chi2.ppf(.01,subSize**2)
tooSimilar = testStatistic < self.pvalThreshold
connection = np.ones_like(tooSimilar)
connection = np.logical_xor(connection, np.logical_or(overLap, np.logical_not(tooSimilar)))
return connection