def _maximize(Xs, Ps, Ps_lag, Yo, h, jacH, f, jacF, structQ, baseQ=None):
No = Yo.shape[0]
T = Yo.shape[1]
Nx = Xs.shape[0]
xb = Xs[:, 0]
B = Ps[:, :, 0]
R = 0
nobs = 0
sumSig = 0
# Dreano et al. 2017, Eq. (34)
for t in range(T):
if not np.isnan(Yo[0, t]):
nobs += 1
H = jacH(Xs[:, t + 1])
R += np.outer(Yo[:, t] - h(Xs[:, t + 1]),
Yo[:, t] - h(Xs[:, t + 1]))
R += H.dot(Ps[:, :, t + 1]).dot(H.T)
R = .5 * (R + R.T)
R /= nobs
# for Shumway 1982
mat_A = 0
mat_B = 0
mat_C = 0
for t in range(T):
# Dreano et al. 2017, Eq. (33)
F = jacF(Xs[:, t + 1])
sumSig += Ps[:, :, t + 1]
sumSig += np.outer(Xs[:, t + 1] - f(Xs[:, t]), Xs[:, t + 1] -
f(Xs[:, t])) # CAUTION: error in Dreano equations
sumSig += F.dot(Ps[:, :, t]).dot(F.T)
sumSig -= Ps_lag[:, :, t].dot(F.T) + F.dot(
Ps_lag[:, :, t].T
) # CAUTION: transpose at the end (error in Dreano equations)
sumSig = .5 * (sumSig + sumSig.T)
# Shumway 1982, Eq. (13), not working
#mat_A += Ps[:,:,t] + np.outer(Xs[:,t], Xs[:,t])
#mat_B += Ps_lag[:,:,t] + np.outer(Xs[:,t+1], Xs[:,t])
#mat_C += Ps[:,:,t+1] + np.outer(Xs[:,t+1], Xs[:,t+1])
#sumSig = mat_C - mat_B.dot(inv(mat_A)).dot(mat_B.T) # CAUTION: only for Shumway solution
if structQ == 'full':
Q = sumSig / T
elif structQ == 'diag':
Q = np.diag(np.diag(sumSig)) / T
elif structQ == 'const':
alpha = np.trace(inv_svd(baseQ).dot(sumSig)) / (T * Nx)
Q = alpha * baseQ
beta = np.trace(inv_svd(baseQ).dot(R)) / (No) ### MODIF PIERRE ###
R = beta * baseQ ### MODIF PIERRE ###
return xb, B, Q, R
def _maximize(X, obs, H, f, structQ='full', baseQ=None):
Nx, Ne, T = X.shape
No = obs.shape[0]
xb = np.mean(X[:, :, 0], 1)
B = np.cov(X[:, :, 0])
sumSig = np.zeros((Nx, Ne, T - 1))
for t in range(T - 1):
sumSig[..., t] = X[..., t + 1] - f(X[..., t])
sumSig = np.reshape(sumSig, (Nx, (T - 1) * Ne))
sumSig = sumSig.dot(sumSig.T) / Ne
if structQ == 'full':
Q = sumSig / (T - 1)
elif structQ == 'diag':
Q = np.diag(np.diag(sumSig)) / T
elif structQ == 'const':
alpha = np.trace(inv_svd(baseQ).dot(sumSig)) / ((T - 1) * Nx)
Q = alpha * baseQ
W = np.zeros([No, Ne, T - 1])
nobs = 0
for t in range(T - 1):
if not np.isnan(obs[0, t]):
nobs += 1
W[:, :, t] = np.tile(obs[:, t], (Ne, 1)).T - H.dot(X[:, :, t + 1])
W = np.reshape(W, (No, (T - 1) * Ne))
R = W.dot(W.T) / (nobs * Ne)
return xb, B, Q, R
def EnKF(dx, T, dy, xb, B, Q, R, Ne, f, H, obs, prng):
sqQ = sqrt_svd(Q)
sqR = sqrt_svd(R)
sqB = sqrt_svd(B)
Xa = np.zeros([dx, Ne, T + 1])
Xf = np.zeros([dx, Ne, T])
# Initialize ensemble
for i in range(Ne):
Xa[:, i, 0] = xb + sqB.dot(prng.normal(size=dx))
for t in range(T):
#print(Xf[:,:,t].ndim)
Xf[:, :, t] = f(Xa[:, :, t]) + sqQ.dot(prng.normal(size=(dx, Ne)))
Y = H.dot(Xf[:, :, t])
# Update
if np.isnan(obs[0, t]):
Xa[:, :, t + 1] = Xf[:, :, t]
else:
Pfxx = np.cov(Xf[:, :, t])
K = Pfxx.dot(H.T).dot(inv_svd(H.dot(Pfxx).dot(H.T) + R))
innov = (np.tile(obs[:, t], (Ne, 1))).T + sqR.dot(
prng.normal(size=(dy, Ne))) - Y
Xa[:, :, t + 1] = Xf[:, :, t] + K.dot(innov)
return Xa, Xf
def _EnKF(Nx, T, No, xb, B, Q, R, Ne, alpha, f, H, obs, prng):
sqQ = sqrt_svd(Q)
sqR = sqrt_svd(R)
sqB = sqrt_svd(B)
Xa = np.zeros([Nx, Ne, T + 1])
Xf = np.zeros([Nx, Ne, T])
# Initialize ensemble
for i in range(Ne):
Xa[:, i, 0] = xb + sqB.dot(prng.normal(size=Nx))
for t in range(T):
# Forecast
# for i in range(Ne):
# Xf[:,i,t] = f(Xa[:,i,t]) + sqQ.dot(prng.normal(size=Nx))
Xf[:, :, t] = f(Xa[:, :, t]) + sqQ.dot(prng.normal(size=(Nx, Ne)))
Y = H.dot(Xf[:, :, t]) + sqR.dot(prng.normal(size=(No, Ne)))
# Update
if np.isnan(obs[0, t]):
Xa[:, :, t + 1] = Xf[:, :, t]
else:
Pfxx = np.cov(Xf[:, :, t])
K = Pfxx.dot(H.T).dot(inv_svd(H.dot(Pfxx).dot(H.T) + R / alpha))
innov = np.tile(obs[:, t], (Ne, 1)).T - Y
Xa[:, :, t + 1] = Xf[:, :, t] + K.dot(innov)
# for i in range(Ne):
# innov = obs[:,t] - Y[:,i]
# Xa[:,i,t+1] = Xf[:,i,t] + K.dot(innov)
return Xa, Xf
def _maximize(Xs, Ps, Ps_lag, Yo, h, jacH, f, jacF, structQ, baseQ=None):
T = Yo.shape[1]
Nx = Xs.shape[0]
xb = Xs[:, 0]
B = Ps[:, :, 0]
R = 0
nobs = 0
for t in range(T):
if not np.isnan(Yo[0, t]):
nobs += 1
H = jacH(Xs[:, t + 1])
R += np.outer(Yo[:, t] - h(Xs[:, t + 1]),
Yo[:, t] - h(Xs[:, t + 1]))
R += H.dot(Ps[:, :, t + 1]).dot(H.T)
R = .5 * (R + R.T)
R /= nobs
sumSig = 0
for t in range(T):
F = jacF(Xs[:, t + 1])
sumSig += Ps[:, :, t + 1]
sumSig += np.outer(Xs[:, t + 1] - f(Xs[:, t]),
Xs[:, t + 1] - f(Xs[:, t]))
sumSig += F.dot(Ps[:, :, t]).dot(F.T)
sumSig -= Ps_lag[:, :, t].dot(F.T) + F.dot(Ps_lag[:, :, t].T)
if structQ == 'full':
Q = sumSig / T
elif structQ == 'diag':
Q = np.diag(np.diag(sumSig)) / T
elif structQ == 'const':
alpha = np.trace(inv_svd(baseQ).dot(sumSig)) / (T * Nx)
Q = alpha * baseQ
return xb, B, Q, R