def setForward_1(self, probaR, Cov, y, Mean_Y):
rnp1, indrnp1 = 0., 0
rn, indrn = 0., 0
if self._interpolation==True:
Mean_Y_rn = InterLineaire_Vector(Mean_Y, rn)
Cov_rn_0 = InterBiLineaire_Matrix(Cov, rn, rnp1)
else:
Mean_Y_rn = Mean_Y[indrn]
Cov_rn_0 = Cov[indrn*self._STEPS+indrnp1]
self._p0 = probaR(rn) * norm.pdf(y, loc=Mean_Y_rn, scale=np.sqrt(Cov_rn_0[1, 1])).item()
rn, indrn = 1., self._STEPSp1
if self._interpolation==True:
Mean_Y_rn = InterLineaire_Vector(Mean_Y, rn)
Cov_rn_0 = InterBiLineaire_Matrix(Cov, rn, rnp1)
else:
Mean_Y_rn = Mean_Y[indrn]
Cov_rn_0 = Cov[indrn*self._STEPS+indrnp1]
self._p1 = probaR(rn) * norm.pdf(y, loc=Mean_Y_rn, scale=np.sqrt(Cov_rn_0[1, 1])).item()
for indrn, rn in enumerate(self._Rcentres):
if self._interpolation==True:
Mean_Y_rn = InterLineaire_Vector(Mean_Y, rn)
Cov_rn_0 = InterBiLineaire_Matrix(Cov, rn, rnp1)
else:
Mean_Y_rn = Mean_Y[indrn+1]
Cov_rn_0 = Cov[(indrn+1)*self._STEPS+(indrnp1+1)]
self._p01[indrn] = probaR(rn) * norm.pdf(y, loc=Mean_Y_rn, scale=np.sqrt(Cov_rn_0[1, 1])).item()
self.normalisation(self.Integ())
def loijointeAP2(rnp1, rn, indrn, proba, probaR2CondR1, Cov, Mean_Y, yn, ynp1,
np1, interpolation, STEPS):
n_z = int(np.shape(Cov)[1] / 2)
if interpolation == True:
A_rn_rnp1, Q_rn_rnp1 = From_Cov_to_FQ_bis(
InterBiLineaire_Matrix(Cov, rn, rnp1), n_z)
moyrn = float(InterLineaire_Vector(Mean_Y, rn))
moyrnp1 = float(InterLineaire_Vector(Mean_Y, rnp1))
else:
indrnp1 = getindrnFromrn(STEPS, rnp1)
A_rn_rnp1, Q_rn_rnp1 = From_Cov_to_FQ_bis(
Cov[indrn * (STEPS + 2) + indrnp1], n_z)
moyrn = Mean_Y[indrn]
moyrnp1 = Mean_Y[indrnp1]
# pdf de la gaussienne
Gauss = norm.pdf(ynp1,
loc=(yn - moyrn) * float(A_rn_rnp1[1, 1]) + moyrnp1,
scale=np.sqrt(Q_rn_rnp1[1, 1])).item()
result = proba.getr(rnp1) * probaR2CondR1(rn, rnp1) * Gauss
if not np.isfinite(result):
print('proba.getr(rnp1)=', proba.getr(rnp1))
print('probaR2CondR1(rn, rnp1)=', probaR2CondR1(rn, rnp1))
print('Gauss=', Gauss)
input('Attente loijointeAP2')
return result
def set3b_1D(self, Mean_X, Mean_Y, Cov, ynp1, tab_E_Xnp1_dp1):
rnp1, indrnp1 = 0., 0
rn, indrn = 0., 0
if self._interpolation == True:
Mean_X_rn = InterLineaire_Vector(Mean_X, rn)
Mean_Y_rn = InterLineaire_Vector(Mean_Y, rn)
Cov_rn_0 = InterBiLineaire_Matrix(Cov, rn, rnp1)
else:
Mean_Y_rn = Mean_Y[indrn]
Mean_X_rn = Mean_X[indrn]
Cov_rn_0 = Cov[indrn * (self._STEPS + 2) + indrnp1]
Var_n_n_rn = Cov_rn_0[
0, 0] - Cov_rn_0[0, 1] * Cov_rn_0[0, 1] / Cov_rn_0[1, 1]
self._p0 = Var_n_n_rn + tab_E_Xnp1_dp1.getindr(
indrn) * tab_E_Xnp1_dp1.getindr(indrn)
rn, indrn = 1., self._STEPSp1
if self._interpolation == True:
Mean_X_rn = InterLineaire_Vector(Mean_X, rn)
Mean_Y_rn = InterLineaire_Vector(Mean_Y, rn)
Cov_rn_0 = InterBiLineaire_Matrix(Cov, rn, rnp1)
else:
Mean_Y_rn = Mean_Y[indrn]
Mean_X_rn = Mean_X[indrn]
Cov_rn_0 = Cov[indrn * (self._STEPS + 2) + indrnp1]
Var_n_n_rn = Cov_rn_0[
0, 0] - Cov_rn_0[0, 1] * Cov_rn_0[0, 1] / Cov_rn_0[1, 1]
self._p1 = Var_n_n_rn + tab_E_Xnp1_dp1.getindr(
indrn) * tab_E_Xnp1_dp1.getindr(indrn)
for indrn, rn in enumerate(self._Rcentres):
if self._interpolation == True:
Mean_X_rn = InterLineaire_Vector(Mean_X, rn)
Mean_Y_rn = InterLineaire_Vector(Mean_Y, rn)
Cov_rn_0 = InterBiLineaire_Matrix(Cov, rn, rnp1)
else:
Mean_Y_rn = Mean_Y[indrn + 1]
Mean_X_rn = Mean_X[indrn + 1]
Cov_rn_0 = Cov[(indrn + 1) * (self._STEPS + 2) + indrnp1]
Var_n_n_rn = Cov_rn_0[
0, 0] - Cov_rn_0[0, 1] * Cov_rn_0[0, 1] / Cov_rn_0[1, 1]
self._p01[indrn] = Var_n_n_rn + tab_E_Xnp1_dp1.getindr(
indrn) * tab_E_Xnp1_dp1.getindr(indrn)
def CovAQ(self, Cov, rn, rnp1):
n_z = np.shape(Cov)[2] // 2
if self._interpolation == True:
Cov_rn_rnp1 = InterBiLineaire_Matrix(Cov, rn, rnp1)
else:
indrn = getindrnFromrn(self._STEPS, rn)
indrnp1 = getindrnFromrn(self._STEPS, rnp1)
Cov_rn_rnp1 = Cov[indrn * (self._STEPS + 2) + indrnp1]
return From_Cov_to_FQ_bis(Cov_rn_rnp1, n_z)
def set3a_1D(self, Mean_X, Mean_Y, Cov, ynp1):
rnp1, indrnp1 = 0., 0
rn, indrn = 0., 0
if self._interpolation == True:
Mean_X_rn = InterLineaire_Vector(Mean_X, rn)
Mean_Y_rn = InterLineaire_Vector(Mean_Y, rn)
Cov_rn_0 = InterBiLineaire_Matrix(Cov, rn, rnp1)
else:
Mean_Y_rn = Mean_Y[indrn]
Mean_X_rn = Mean_X[indrn]
Cov_rn_0 = Cov[indrn * (self._STEPS + 2) + indrnp1]
self._p0 = Mean_X_rn + Cov_rn_0[0, 1] / Cov_rn_0[1, 1] * (ynp1 -
Mean_Y_rn)
rn, indrn = 1., self._STEPSp1
if self._interpolation == True:
Mean_X_rn = InterLineaire_Vector(Mean_X, rn)
Mean_Y_rn = InterLineaire_Vector(Mean_Y, rn)
Cov_rn_0 = InterBiLineaire_Matrix(Cov, rn, rnp1)
else:
Mean_Y_rn = Mean_Y[indrn]
Mean_X_rn = Mean_X[indrn]
Cov_rn_0 = Cov[indrn * (self._STEPS + 2) + indrnp1]
self._p1 = Mean_X_rn + Cov_rn_0[0, 1] / Cov_rn_0[1, 1] * (ynp1 -
Mean_Y_rn)
for indrn, rn in enumerate(self._Rcentres):
if self._interpolation == True:
Mean_X_rn = InterLineaire_Vector(Mean_X, rn)
Mean_Y_rn = InterLineaire_Vector(Mean_Y, rn)
Cov_rn_0 = InterBiLineaire_Matrix(Cov, rn, rnp1)
else:
Mean_Y_rn = Mean_Y[indrn + 1]
Mean_X_rn = Mean_X[indrn + 1]
Cov_rn_0 = Cov[(indrn + 1) * (self._STEPS + 2) + indrnp1]
self._p01[indrn] = Mean_X_rn + Cov_rn_0[0, 1] / Cov_rn_0[1, 1] * (
ynp1 - Mean_Y_rn)
def restore_withfuzzyjump(self,
Y,
R,
Cov,
Mean_X,
Mean_Y,
Likelihood=False,
smooth=True):
N, n_y = np.shape(Y)
n_r, n_x = np.shape(Mean_X)
n_z = n_x + n_y
s_xz = slice(n_x, n_z)
s_0x = slice(0, n_x)
# s_0z = slice(0, n_z)
# State estimation arrays
E_Xn_n = np.zeros((N, n_x))
Cov_Xn_n = np.zeros((N, n_x, n_x))
E_Xn_np1 = np.zeros((N, n_x))
Cov_Xn_np1 = np.zeros((N, n_x, n_x))
E_Xn_N = np.zeros((N, n_x))
Cov_Xn_N = np.zeros((N, n_x, n_x))
C_n_np1_N = np.zeros((N, n_x, n_x))
E_Xnp1_n = np.zeros((N, n_z)) # predictor
Cov_Xnp1_n = np.zeros((N, n_z, n_z)) # predictor
E_Yn_np1 = np.zeros((N, n_y))
M_znp1 = np.zeros((n_z, 1))
M_zn = np.zeros((n_z, 1))
A_n = np.zeros((N, n_x, n_x))
likelihood = 0
# ====================== Le premier ====================== #
i = 0
alpha = R[i]
MeanX_alpha = InterLineaire_Vector(Mean_X, alpha)
MeanY_alpha = InterLineaire_Vector(Mean_Y, alpha)
Cov_alpha_0 = InterBiLineaire_Matrix(Cov, alpha, 0.)
Temp = np.dot(Cov_alpha_0[s_0x, s_xz],
np.linalg.inv(Cov_alpha_0[s_xz, s_xz]))
E_Xn_n[i, :, ] = MeanX_alpha + np.dot(Temp, Y[i, :] - MeanY_alpha)
Cov_Xn_n[i, :, :] = Cov_alpha_0[s_0x, s_0x] - np.dot(
Temp, Cov_alpha_0[s_xz, s_0x])
# ====================== Les suivants ====================== #
for i in range(N - 1):
alpha = R[i]
beta = R[i + 1]
# interpolation of the covariance, then conversion to F, Q
F_temp, Q_temp = From_Cov_to_FQ_bis(
InterBiLineaire_Matrix(Cov, alpha, beta), n_z)
# interpolation of the two means
M_zn[s_0x, 0] = InterLineaire_Vector(Mean_X, alpha)
M_zn[s_xz, 0] = InterLineaire_Vector(Mean_Y, alpha)
M_znp1[s_0x, 0] = InterLineaire_Vector(Mean_X, beta)
M_znp1[s_xz, 0] = InterLineaire_Vector(Mean_Y, beta)
F_xx = F_temp[s_0x, s_0x]
F_xy = F_temp[s_0x, s_xz]
F_yx = F_temp[s_xz, s_0x]
F_yxT = F_yx.T
F_yy = F_temp[s_xz, s_xz]
Q_xx = Q_temp[s_0x, s_0x]
Q_xy = Q_temp[s_0x, s_xz]
Q_yx = Q_temp[s_xz, s_0x]
Q_yy = Q_temp[s_xz, s_xz]
dot_Q_xy_Q_yy_inv = np.dot(Q_xy, np.linalg.inv(Q_yy))
N_z = M_znp1 - np.dot(F_temp, M_zn)
N_x = N_z[s_0x, 0]
N_y = N_z[s_xz, 0]
# ----------------- Filtering (Forward) ---------------- #
A_n[i, :, :] = F_xx - np.dot(dot_Q_xy_Q_yy_inv, F_yx)
Q2 = Q_xx - np.dot(dot_Q_xy_Q_yy_inv, Q_yx)
S_n_np1 = Q_yy + np.dot(np.dot(F_yx, Cov_Xn_n[i, :, :]), F_yxT)
S_n_np1_inv = np.linalg.inv(S_n_np1)
K_n_np1 = np.dot(np.dot(Cov_Xn_n[i, :, :], F_yxT), S_n_np1_inv)
E_Yn_np1 = np.dot(F_yx, E_Xn_n[i, :]) + np.dot(F_yy, Y[i, :]) + N_y
E_Xn_np1[i, :] = E_Xn_n[i, :] + np.dot(K_n_np1,
(Y[i + 1, :] - E_Yn_np1))
Cov_Xn_np1[i, :, :] = Cov_Xn_n[i, :, :] - np.dot(
np.dot(K_n_np1, S_n_np1), np.transpose(K_n_np1))
B_n = np.dot(dot_Q_xy_Q_yy_inv, Y[i + 1, :]) + np.dot(
(F_xy - np.dot(dot_Q_xy_Q_yy_inv, F_yy)),
Y[i, :]) + N_x - np.dot(dot_Q_xy_Q_yy_inv, N_y)
# Filter X
E_Xn_n[i + 1, :, ] = np.dot(A_n[i, :, :], E_Xn_np1[i, :]) + B_n
Cov_Xn_n[i + 1, :, :] = Q2 + np.dot(
np.dot(A_n[i, :, :], Cov_Xn_np1[i, :, :]),
np.transpose(A_n[i, :, :]))
if Likelihood is True:
likelihood -= math.log(
np.linalg.det(S_n_np1), math.e) - np.squeeze(
np.dot(
np.dot((Y[i + 1, :] - E_Yn_np1)[np.newaxis],
S_n_np1_inv),
(Y[i + 1, :] - E_Yn_np1)[np.newaxis].T))
E_Xn_np1[N - 1, :] = E_Xn_n[N - 1, :]
Cov_Xn_np1[N - 1, :, :] = Cov_Xn_n[N - 1, :, :]
# ----------------- Smoothing (Backward) ----------------- #
if smooth:
E_Xn_N[N - 1:] = E_Xn_np1[N - 1:]
Cov_Xn_N[N - 1:, :] = Cov_Xn_np1[N - 1:, :]
for i in range(N - 2, -1, -1):
K_n_N = np.dot(
np.dot(Cov_Xn_np1[i, :, :], np.transpose(A_n[i, :, :])),
np.linalg.inv(Cov_Xn_n[i + 1, :, :]))
E_Xn_N[i, :, ] = E_Xn_np1[i, :, ] + np.dot(
K_n_N, (E_Xn_N[i + 1, :] - E_Xn_n[i + 1, :]))
Cov_Xn_N[i, :, :] = Cov_Xn_np1[i, :, :] + np.dot(
np.dot(K_n_N,
(Cov_Xn_N[i + 1, :, :] - Cov_Xn_n[i + 1, :, :])),
np.transpose(K_n_N))
C_n_np1_N[i, :, :] = np.dot(K_n_N, Cov_Xn_N[i + 1, :, :])
if Likelihood is True:
return E_Xn_n, Cov_Xn_n, E_Xn_N, Cov_Xn_N, likelihood
return E_Xn_n, Cov_Xn_n, E_Xn_N, Cov_Xn_N, E_Xnp1_n, Cov_Xnp1_n