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