def IteratedExtendedKalman(topology, meas, meas_unc, meas_idx, pseudo_meas, model, V0, Vs,slack_idx=0, Y=None, accuracy=1e-9, maxiter=50): """ Iterated Extended Kalman filter for the nodal load observer Real-valued matrices of complex-valued quantities are assumed to be structured as [ [real part], [imag part] ] :param topology: dict containing information on bus, branch, ... in PyPower format :param meas: dict containing measurements "Pk", "Qk", "Vm" and "Va" :param meas_unc: dict containing uncertainties with the values in `meas' :param meas_idx: dict containing the corresponding indices :param pseudo_meas: dict containing the corresponding pseudo-measurements :param model: DynamicModel object as defined in dynamic_models.py :param V0: initial estimate of nodal voltages at all nodes except slack :param Vs: voltage amplitude and phase at slack node as (2,nT)-dimensional vector :param slack_idx: index of slack node :param Y: (optional) admittance matrix :param accuracy: threshold for inner iteration of the iterated EKF :param maxiter: maximum number of inner iterations of the iterated EKF :return: Shat, Vhat, uShat, DeltaS, uDeltaS """ if issparse(Y): Y = Y.toarray() n = model.dim n_K = len(V0)/2 pmeas = np.zeros(n_K,dtype = bool); pmeas[meas_idx["Pk"]] = True qmeas = np.zeros(n_K,dtype = bool); qmeas[meas_idx["Qk"]] = True vmeas = np.zeros(n_K,dtype = bool); vmeas[meas_idx["Vm"]] = True Cm,Dnm,Dm = get_system_matrices(pmeas,qmeas,vmeas) if not isinstance(Y,np.ndarray): Y = makeYbus(topology["baseMVA"],topology["bus"],topology["branch"]) Y00, Ys = separate_Yslack(Y,slack_idx) # transform voltages at slack node to real and imaginary parts Vs_ri = np.vstack((Vs[0,:]*np.cos(np.radians(Vs[1,:])), Vs[0,:]*np.sin(np.radians(Vs[1,:])))) Slack = np.linalg.solve(Y00,np.dot(Ys,Vs_ri)) Sm = np.r_[meas["Pk"], meas["Qk"]] Sfc = np.r_[pseudo_meas["Pk"], pseudo_meas["Qk"]] u = np.dot(Dm,Sm) + np.dot(Dnm,Sfc) #np.savetxt("/Users/makara01/Documents/oioi/Sm.out", u) # adjust uncertainties in case that their dimension is wrong if isinstance(meas_unc["Vm"],float): meas_unc["Vm"] = meas_unc["Vm"]*np.ones_like(meas["Vm"]) elif len(meas_unc["Vm"].shape)==1: meas_unc["Vm"] = np.tile(meas_unc["Vm"],(meas["Vm"].shape[1],1)).T if isinstance(meas_unc["Va"],float): meas_unc["Va"] = meas_unc["Va"]*np.ones_like(meas["Va"]) elif len(meas_unc["Va"].shape)==1: meas_unc["Va"] = np.tile(meas_unc["Va"],(meas["Va"].shape[1],1)).T def calcM(mu): divisor = 3*(mu[:n_K]**2 + mu[n_K:]**2) return np.r_[np.c_[np.diag(mu[:n_K]/divisor), np.diag(mu[n_K:]/divisor)], np.c_[np.diag(mu[n_K:]/divisor), -np.diag(mu[:n_K]/divisor)]] nT = Vs.shape[1] xhat = np.zeros((n,nT)) Vhat = np.zeros((2*n_K,nT)) Shat = np.zeros((2*n_K,nT)) DeltaS = np.zeros_like(xhat) uDeltaS= np.zeros_like(xhat) Pfilter = model.forecast_unc() Dnm = model.adjust_Dnm(Dnm) for k in range(nT): # transform voltages to real and imaginary parts yRe,yIm,R = amph_phase_to_real_imag(meas["Vm"][:,k],np.radians(meas["Va"][:,k]),meas_unc["Vm"][:,k]**2,meas_unc["Va"][:,k]**2) y = np.r_[yRe,yIm] if k==0: xhatfc = model.forecast_state() Pfilterfc = model.forecast_unc() mu = np.hstack((V0[:n_K]*np.cos(V0[n_K:]), V0[:n_K]*np.sin(V0[n_K:]))) else: xhatfc = model.forecast_state(xhat[:,k-1]) Pfilterfc = model.forecast_unc(Pfilter) mu = Vhat[:,k-1] eta = xhatfc varstop1 = 1 j1 = 1 while (varstop1 > accuracy) and (j1 < maxiter): M_U = calcM(mu) muiter = np.dot(np.dot(np.linalg.inv(Y00),M_U),u[:,k] + np.dot(Dnm,eta)) - Slack[:,k] varstop2 = 1 j2 = 1 while (varstop2 > accuracy) and (j2 < maxiter): M_U = calcM(muiter) temp2 = muiter muiter = np.dot(np.dot(np.linalg.inv(Y00),M_U),u[:,k] + np.dot(Dnm,eta)) - Slack[:,k] varstop2 = np.linalg.norm(muiter-temp2) j2 += 1 mu = muiter Dh = jacobian(Y00,Ys,mu,Vs_ri[:,k]) H = np.dot(Cm, np.dot(np.linalg.inv(Dh), Dnm)) K = np.dot(np.dot(Pfilterfc,H.T), np.linalg.pinv(np.dot(H,np.dot(Pfilterfc,H.T))+R)) temp1 = eta eta = xhatfc + np.dot(K, y - np.dot(Cm,mu) - np.dot(H, xhatfc-eta)) varstop1 = np.linalg.norm(temp1-eta) j1 += 1 # Data assimilation step Pfilter = np.dot( np.eye(n) - np.dot(K,H), Pfilterfc) xhat[:,k] = eta Shat[:,k] = u[:,k] + np.dot(Dnm,xhat[:,k]) Vhat[:,k] = mu DeltaS[:,k-1] = xhat[:,k] uDeltaS[:,k-1] = np.sqrt(np.diag(Pfilter)) uS = np.dot(Dnm,uDeltaS) return Shat, Vhat, uS, DeltaS, uDeltaS
def IteratedExtendedKalman(topology, meas, meas_unc, meas_idx, pseudo_meas, model, V0, Vs, slack_idx=0, Y=None, accuracy=1e-9, maxiter=50): """ Iterated Extended Kalman filter for the nodal load observer Real-valued matrices of complex-valued quantities are assumed to be structured as [ [real part], [imag part] ] :param topology: dict containing information on bus, branch, ... in PyPower format :param meas: dict containing measurements "Pk", "Qk", "Vm" and "Va" :param meas_unc: dict containing uncertainties with the values in `meas' :param meas_idx: dict containing the corresponding indices :param pseudo_meas: dict containing the corresponding pseudo-measurements :param model: DynamicModel object as defined in dynamic_models.py :param V0: initial estimate of nodal voltages at all nodes except slack :param Vs: voltage amplitude and phase at slack node as (2,nT)-dimensional vector :param slack_idx: index of slack node :param Y: (optional) admittance matrix :param accuracy: threshold for inner iteration of the iterated EKF :param maxiter: maximum number of inner iterations of the iterated EKF :return: Shat, Vhat, uShat, DeltaS, uDeltaS """ if issparse(Y): Y = Y.toarray() n = model.dim n_K = len(V0) / 2 pmeas = np.zeros(n_K, dtype=bool) pmeas[meas_idx["Pk"]] = True qmeas = np.zeros(n_K, dtype=bool) qmeas[meas_idx["Qk"]] = True vmeas = np.zeros(n_K, dtype=bool) vmeas[meas_idx["Vm"]] = True Cm, Dnm, Dm = get_system_matrices(pmeas, qmeas, vmeas) if not isinstance(Y, np.ndarray): Y = makeYbus(topology["baseMVA"], topology["bus"], topology["branch"]) Y00, Ys = separate_Yslack(Y, slack_idx) # transform voltages at slack node to real and imaginary parts Vs_ri = np.vstack((Vs[0, :] * np.cos(np.radians(Vs[1, :])), Vs[0, :] * np.sin(np.radians(Vs[1, :])))) Slack = np.linalg.solve(Y00, np.dot(Ys, Vs_ri)) Sm = np.r_[meas["Pk"], meas["Qk"]] Sfc = np.r_[pseudo_meas["Pk"], pseudo_meas["Qk"]] u = np.dot(Dm, Sm) + np.dot(Dnm, Sfc) #np.savetxt("/Users/makara01/Documents/oioi/Sm.out", u) # adjust uncertainties in case that their dimension is wrong if isinstance(meas_unc["Vm"], float): meas_unc["Vm"] = meas_unc["Vm"] * np.ones_like(meas["Vm"]) elif len(meas_unc["Vm"].shape) == 1: meas_unc["Vm"] = np.tile(meas_unc["Vm"], (meas["Vm"].shape[1], 1)).T if isinstance(meas_unc["Va"], float): meas_unc["Va"] = meas_unc["Va"] * np.ones_like(meas["Va"]) elif len(meas_unc["Va"].shape) == 1: meas_unc["Va"] = np.tile(meas_unc["Va"], (meas["Va"].shape[1], 1)).T def calcM(mu): divisor = 3 * (mu[:n_K]**2 + mu[n_K:]**2) return np.r_[np.c_[np.diag(mu[:n_K] / divisor), np.diag(mu[n_K:] / divisor)], np.c_[np.diag(mu[n_K:] / divisor), -np.diag(mu[:n_K] / divisor)]] nT = Vs.shape[1] xhat = np.zeros((n, nT)) Vhat = np.zeros((2 * n_K, nT)) Shat = np.zeros((2 * n_K, nT)) DeltaS = np.zeros_like(xhat) uDeltaS = np.zeros_like(xhat) Pfilter = model.forecast_unc() Dnm = model.adjust_Dnm(Dnm) for k in range(nT): # transform voltages to real and imaginary parts yRe, yIm, R = amph_phase_to_real_imag(meas["Vm"][:, k], np.radians(meas["Va"][:, k]), meas_unc["Vm"][:, k]**2, meas_unc["Va"][:, k]**2) y = np.r_[yRe, yIm] if k == 0: xhatfc = model.forecast_state() Pfilterfc = model.forecast_unc() mu = np.hstack( (V0[:n_K] * np.cos(V0[n_K:]), V0[:n_K] * np.sin(V0[n_K:]))) else: xhatfc = model.forecast_state(xhat[:, k - 1]) Pfilterfc = model.forecast_unc(Pfilter) mu = Vhat[:, k - 1] eta = xhatfc varstop1 = 1 j1 = 1 while (varstop1 > accuracy) and (j1 < maxiter): M_U = calcM(mu) muiter = np.dot(np.dot(np.linalg.inv(Y00), M_U), u[:, k] + np.dot(Dnm, eta)) - Slack[:, k] varstop2 = 1 j2 = 1 while (varstop2 > accuracy) and (j2 < maxiter): M_U = calcM(muiter) temp2 = muiter muiter = np.dot(np.dot(np.linalg.inv(Y00), M_U), u[:, k] + np.dot(Dnm, eta)) - Slack[:, k] varstop2 = np.linalg.norm(muiter - temp2) j2 += 1 mu = muiter Dh = jacobian(Y00, Ys, mu, Vs_ri[:, k]) H = np.dot(Cm, np.dot(np.linalg.inv(Dh), Dnm)) K = np.dot(np.dot(Pfilterfc, H.T), np.linalg.pinv(np.dot(H, np.dot(Pfilterfc, H.T)) + R)) temp1 = eta eta = xhatfc + np.dot(K, y - np.dot(Cm, mu) - np.dot(H, xhatfc - eta)) varstop1 = np.linalg.norm(temp1 - eta) j1 += 1 # Data assimilation step Pfilter = np.dot(np.eye(n) - np.dot(K, H), Pfilterfc) xhat[:, k] = eta Shat[:, k] = u[:, k] + np.dot(Dnm, xhat[:, k]) Vhat[:, k] = mu DeltaS[:, k - 1] = xhat[:, k] uDeltaS[:, k - 1] = np.sqrt(np.diag(Pfilter)) uS = np.dot(Dnm, uDeltaS) return Shat, Vhat, uS, DeltaS, uDeltaS
def LinearKalmanFilter(topology, meas, meas_unc, meas_idx, pseudo_meas, model, V0, Vs, slack_idx=0, Y=None): """ Quasi-Linear Kalman filter for the nodal load observer This version of the NLO state estimation method ignores the nonlinearity for the calculation of the Kalman gain and utilizes the fix point equation property for the calculation of voltages from powers. Estimated nodal voltages are returned as real and imaginary parts Real-valued matrices of complex-valued quantities are assumed to be structured as [ [real part], [imag part] ] :param topology: dict containing information on bus, branch, ... in PyPower format :param meas: dict containing measurements "Pk", "Qk", "Vm" and "Va" :param meas_unc: dict containing associated uncertainties :param meas_idx: dict containing the corresponding indices :param pseudo_meas: dict containing the corresponding pseudo-measurements :param model: DynamicModel object as defined in dynamic_models.py :param V0: initial estimate of nodal voltages (magnitude and phase) :param Vs: voltages at slack node (magnitude and phase) :param slack_idx: index of slack node :param Y: (optional) user defined admittance matrix """ if issparse(Y): Y = Y.toarray() if not isinstance(Y,np.ndarray): Y = makeYbus(topology["baseMVA"],topology["bus"],topology["branch"]) Yadm, Y_slack = separate_Yslack(Y,slack_idx) nK = len(V0)/2 pmeas = np.zeros(nK,dtype = bool); pmeas[meas_idx["Pk"]] = True qmeas = np.zeros(nK,dtype = bool); qmeas[meas_idx["Qk"]] = True vmeas = np.zeros(nK,dtype = bool); vmeas[meas_idx["Vm"]] = True Cm,Dnm,Dm = get_system_matrices(pmeas,qmeas,vmeas) # transform voltages at slack node to real and imaginary parts Vs_ri = np.vstack((Vs[0,:]*np.cos(Vs[1,:]), Vs[0,:]*np.sin(Vs[1,:]))) Slack = np.linalg.solve(Yadm,np.dot(Y_slack,Vs_ri)) # y = np.r_[meas["Vm"], meas["Va"]] + np.dot(Cm,Slack) S = np.dot(Dm, np.r_[meas["Pk"], meas["Qk"]]) + np.dot(Dnm, np.r_[pseudo_meas["Pk"], pseudo_meas["Qk"]]) # adjust uncertainties in case that their dimension is wrong if isinstance(meas_unc["Vm"],float): meas_unc["Vm"] = meas_unc["Vm"]*np.ones_like(meas["Vm"]) elif len(meas_unc["Vm"].shape)==1: meas_unc["Vm"] = np.tile(meas_unc["Vm"],(meas["Vm"].shape[1],1)).T if isinstance(meas_unc["Va"],float): meas_unc["Va"] = meas_unc["Va"]*np.ones_like(meas["Va"]) elif len(meas_unc["Va"].shape)==1: meas_unc["Va"] = np.tile(meas_unc["Va"],(meas["Va"].shape[1],1)).T nx = model.dim nm = 2*meas["Vm"].shape[0] t_f= meas["Vm"].shape[1] def calcM(mu): divisor = 3*(mu[:nK]**2 + mu[nK:]**2) return np.r_[np.c_[np.diag(mu[:nK]/divisor), np.diag(mu[nK:]/divisor)], np.c_[np.diag(mu[nK:]/divisor), -np.diag(mu[:nK]/divisor)]] def calcKs(V,Sh,k,accuracy=1e-12): # According to W. Heins' Thesis calculation of V using Ks is a fix point equation # We take that into account by doing a fixed number of iterations of the corresponding # fix point iterations Vn = V.copy() fp_diff = 1.0 maxiter = 20 count = 0 while fp_diff>accuracy and count < maxiter: tmp = Vn MU = calcM(Vn) Ks = np.linalg.solve(Yadm,MU) Vn = np.dot(Ks, np.dot(Dnm,Sh) + S[:,k]) - Slack[:,k] fp_diff = np.linalg.norm(tmp-Vn) count += 1 return Ks P = model.forecast_unc() # initialize solution matrices V_est = np.zeros((2*nK,t_f+1)) # Estimated voltages V_est[:nK,0] = V0[:nK]*np.cos(V0[nK:]) x_est = np.zeros((nx,t_f+1)) # Estimated active and reactive powers x_est[:,0] = model.forecast_state() DeltaS_est = np.zeros((nx,t_f)) # Estimated power deviation UncDeltaS = np.zeros_like(DeltaS_est) Dnm = model.adjust_Dnm(Dnm) #%% ########################### KALMAN ######################################## print '.', for k in range(1,t_f+1): # transform voltages to real and imaginary parts yRe,yIm,R = amph_phase_to_real_imag(meas["Vm"][:,k-1],np.radians(meas["Va"][:,k-1]),meas_unc["Vm"][:,k-1]**2,meas_unc["Va"][:,k-1]**2) y = np.r_[yRe,yIm] + np.dot(Cm,Slack[:,k-1]) # preparation of state space system matrices Ks = calcKs(V_est[:,k-1],x_est[:,k-1],k-1) C = np.dot(Cm,np.dot(Ks,Dnm)) D = np.dot(Cm,Ks) #========================== actual Kalman filter part ========================= # Kalman filter forecast step xf = model.forecast_state(x_est[:,k-1])[:,np.newaxis] Pf = model.forecast_unc(P) # Kalman gain matrix K K = np.linalg.solve(np.dot(C,np.dot(Pf,C.T)) + R, np.dot(C,P)).T # corrected state estimate x_est[:,k][:,np.newaxis] = xf + np.dot(K, y.reshape(nm,1) - (np.dot(C,xf) + np.dot(D,S[:,k-1].reshape(2*nK,1))) ) # corrected error covariance matrix P = np.dot(np.eye(nx) - np.dot(K,C),Pf) #============================================================================== # calculate voltage from estimated power V_est[:,k] = np.dot(Ks, np.dot(Dnm,x_est[:,k-1]) + S[:,k-1]) - Slack[:,k-1] DeltaS_est[:,k-1] = x_est[:,k] UncDeltaS[:,k-1] = np.sqrt(np.diag(P)) print '.', print '.' #%% # results S_est = S + np.dot(Dnm,DeltaS_est) # S_est = S + D_ng * DeltaS_est UncS = np.zeros_like(S) + np.dot(Dnm,UncDeltaS) return S_est, V_est[:,1:], UncS, DeltaS_est,UncDeltaS
def LinearKalmanFilter(topology, meas, meas_unc, meas_idx, pseudo_meas, model, V0, Vs, slack_idx=0, Y=None): """ Quasi-Linear Kalman filter for the nodal load observer This version of the NLO state estimation method ignores the nonlinearity for the calculation of the Kalman gain and utilizes the fix point equation property for the calculation of voltages from powers. Estimated nodal voltages are returned as real and imaginary parts Real-valued matrices of complex-valued quantities are assumed to be structured as [ [real part], [imag part] ] :param topology: dict containing information on bus, branch, ... in PyPower format :param meas: dict containing measurements "Pk", "Qk", "Vm" and "Va" :param meas_unc: dict containing associated uncertainties :param meas_idx: dict containing the corresponding indices :param pseudo_meas: dict containing the corresponding pseudo-measurements :param model: DynamicModel object as defined in dynamic_models.py :param V0: initial estimate of nodal voltages (magnitude and phase) :param Vs: voltages at slack node (magnitude and phase) :param slack_idx: index of slack node :param Y: (optional) user defined admittance matrix """ if issparse(Y): Y = Y.toarray() if not isinstance(Y, np.ndarray): Y = makeYbus(topology["baseMVA"], topology["bus"], topology["branch"]) Yadm, Y_slack = separate_Yslack(Y, slack_idx) nK = len(V0) / 2 pmeas = np.zeros(nK, dtype=bool) pmeas[meas_idx["Pk"]] = True qmeas = np.zeros(nK, dtype=bool) qmeas[meas_idx["Qk"]] = True vmeas = np.zeros(nK, dtype=bool) vmeas[meas_idx["Vm"]] = True Cm, Dnm, Dm = get_system_matrices(pmeas, qmeas, vmeas) # transform voltages at slack node to real and imaginary parts Vs_ri = np.vstack( (Vs[0, :] * np.cos(Vs[1, :]), Vs[0, :] * np.sin(Vs[1, :]))) Slack = np.linalg.solve(Yadm, np.dot(Y_slack, Vs_ri)) # y = np.r_[meas["Vm"], meas["Va"]] + np.dot(Cm,Slack) S = np.dot(Dm, np.r_[meas["Pk"], meas["Qk"]]) + np.dot( Dnm, np.r_[pseudo_meas["Pk"], pseudo_meas["Qk"]]) # adjust uncertainties in case that their dimension is wrong if isinstance(meas_unc["Vm"], float): meas_unc["Vm"] = meas_unc["Vm"] * np.ones_like(meas["Vm"]) elif len(meas_unc["Vm"].shape) == 1: meas_unc["Vm"] = np.tile(meas_unc["Vm"], (meas["Vm"].shape[1], 1)).T if isinstance(meas_unc["Va"], float): meas_unc["Va"] = meas_unc["Va"] * np.ones_like(meas["Va"]) elif len(meas_unc["Va"].shape) == 1: meas_unc["Va"] = np.tile(meas_unc["Va"], (meas["Va"].shape[1], 1)).T nx = model.dim nm = 2 * meas["Vm"].shape[0] t_f = meas["Vm"].shape[1] def calcM(mu): divisor = 3 * (mu[:nK]**2 + mu[nK:]**2) return np.r_[np.c_[np.diag(mu[:nK] / divisor), np.diag(mu[nK:] / divisor)], np.c_[np.diag(mu[nK:] / divisor), -np.diag(mu[:nK] / divisor)]] def calcKs(V, Sh, k, accuracy=1e-12): # According to W. Heins' Thesis calculation of V using Ks is a fix point equation # We take that into account by doing a fixed number of iterations of the corresponding # fix point iterations Vn = V.copy() fp_diff = 1.0 maxiter = 20 count = 0 while fp_diff > accuracy and count < maxiter: tmp = Vn MU = calcM(Vn) Ks = np.linalg.solve(Yadm, MU) Vn = np.dot(Ks, np.dot(Dnm, Sh) + S[:, k]) - Slack[:, k] fp_diff = np.linalg.norm(tmp - Vn) count += 1 return Ks P = model.forecast_unc() # initialize solution matrices V_est = np.zeros((2 * nK, t_f + 1)) # Estimated voltages V_est[:nK, 0] = V0[:nK] * np.cos(V0[nK:]) x_est = np.zeros((nx, t_f + 1)) # Estimated active and reactive powers x_est[:, 0] = model.forecast_state() DeltaS_est = np.zeros((nx, t_f)) # Estimated power deviation UncDeltaS = np.zeros_like(DeltaS_est) Dnm = model.adjust_Dnm(Dnm) #%% ########################### KALMAN ######################################## print '.', for k in range(1, t_f + 1): # transform voltages to real and imaginary parts yRe, yIm, R = amph_phase_to_real_imag(meas["Vm"][:, k - 1], np.radians(meas["Va"][:, k - 1]), meas_unc["Vm"][:, k - 1]**2, meas_unc["Va"][:, k - 1]**2) y = np.r_[yRe, yIm] + np.dot(Cm, Slack[:, k - 1]) # preparation of state space system matrices Ks = calcKs(V_est[:, k - 1], x_est[:, k - 1], k - 1) C = np.dot(Cm, np.dot(Ks, Dnm)) D = np.dot(Cm, Ks) #========================== actual Kalman filter part ========================= # Kalman filter forecast step xf = model.forecast_state(x_est[:, k - 1])[:, np.newaxis] Pf = model.forecast_unc(P) # Kalman gain matrix K K = np.linalg.solve(np.dot(C, np.dot(Pf, C.T)) + R, np.dot(C, P)).T # corrected state estimate x_est[:, k][:, np.newaxis] = xf + np.dot( K, y.reshape(nm, 1) - (np.dot(C, xf) + np.dot(D, S[:, k - 1].reshape(2 * nK, 1)))) # corrected error covariance matrix P = np.dot(np.eye(nx) - np.dot(K, C), Pf) #============================================================================== # calculate voltage from estimated power V_est[:, k] = np.dot( Ks, np.dot(Dnm, x_est[:, k - 1]) + S[:, k - 1]) - Slack[:, k - 1] DeltaS_est[:, k - 1] = x_est[:, k] UncDeltaS[:, k - 1] = np.sqrt(np.diag(P)) print '.', print '.' #%% # results S_est = S + np.dot(Dnm, DeltaS_est) # S_est = S + D_ng * DeltaS_est UncS = np.zeros_like(S) + np.dot(Dnm, UncDeltaS) return S_est, V_est[:, 1:], UncS, DeltaS_est, UncDeltaS