def NLOextended(topology, meas, meas_unc, meas_idx, pseudo_meas, model, V0,
slack_idx=0, Y=None, accuracy=1e-9, maxiter=5):
"""
Iterated Extended Kalman filter for the nodal load observer (extended to all kind of measurements)
Real-valued matrices of complex-valued quantities are assumed to be structured as [ [real part], [imag part] ]
In this variant of the NLO, calculation of voltages in each time step is carried out by fitting to bus power.
The Kalman correction step uses nodal voltages, bus power and line power.
: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 standard 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 at all nodes except slack
:param Vs: voltage amplitude and phase at slack node
: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 slack_idx > 0:
raise NotImplementedError("Only slack node at bus index 0 implemented yet.")
meas_names = ["Pk","Qk","Pl","Ql","Vm","Va"]
meas, meas_idx, meas_unc = repair_meas(meas, meas_idx, meas_unc, expected_indices = meas_names)
n = model.dim
n_K = len(V0)/2 # assume that V0 does not contain slack bus voltage
# remove index of slack bus
non_ref = range(1, n_K) + range(n_K + 1, 2 * n_K)
# generate matrices to map measured values to full list for all buses
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)
Dnm = model.adjust_Dnm(Dnm) # adjust for the case of AR(2) process
Dnm_nB = Dnm[non_ref, :]
# generate admittance matrix from network data if not provided by the user
if not isinstance(Y,np.ndarray):
Y = makeYbus(topology["baseMVA"],topology["bus"],topology["branch"])
y, cap = calc_admittance(topology["branch"])[1:]
# calculate network functions and their Jacobians
J_dSdV, J_dHdV, f_hSK, f_hSl = network_equations(Y, y, cap, n_K, meas_idx)
# calculate vector of nodal powers from actual and pseudo measurements
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)
nT = u.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)
P = []
# helper function
def calcV(V, eta, meas_k, umeas_k, k, PF_accuracy = 1e-16):
"""Calculate nodal voltages by minimizing the difference between measured and calculated bus power using Newton's method
:rtype: tuple
:param meas_k: measured values
:param umeas_k: uncertainties associated with measured values
:return: V, h_V, Jacobian_V
"""
def voltage2buspower(v):
EqPF = f_hSK(*v)[non_ref]
JacSE = J_dSdV(*v)[non_ref]
return EqPF, JacSE
pf_stop = 1; pf_iter = 0
Jac_SfromV = None
SfromV = None
S = np.dot(Dm,np.r_[meas_k['Pk'],meas_k['Qk']]) + np.dot(Dnm,Sfc[:,k]) + Dnm.dot(eta)
while pf_stop > PF_accuracy and pf_iter < 20:
SfromV, Jac_SfromV = voltage2buspower(V)
delta_V = np.linalg.solve(Jac_SfromV, S[non_ref] - SfromV)
V[non_ref] = V[non_ref] + delta_V
pf_stop = np.linalg.norm(delta_V)
pf_iter += 1
return V
# Iterated Extended Kalman Filter
for k in range(nT):
meas_k, umeas_k = meas_at_time(meas,meas_unc,k,meas_names)
if k == 0:
xhatfc = model.forecast_state()
Pfc = model.forecast_unc()
mu = V0
if len(meas_idx['Vm'])>0:
mu[meas_idx["Vm"]] = meas_k["Vm"][:]
if len(meas_idx['Va'])>0:
mu[n_K+np.asarray(meas_idx["Va"])] = meas_k["Va"][:]
# xhat[:,k] = xhatfc[:]
# P.append(Pfc)
# Shat[:, k] = u[:, k] + np.dot(Dnm, xhat[:, k])
# Vhat[:,k] = mu.copy()
# continue # at time 0 use forecast as estimate
else:
xhatfc = model.forecast_state(xhat[:, k - 1])
Pfc = model.forecast_unc(P[k - 1])
mu = Vhat[:, k - 1].copy()
if len(meas_idx['Vm'])>0:
mu[meas_idx["Vm"]] = meas_k["Vm"]
if len(meas_idx['Va'])>0:
mu[n_K+np.asarray(meas_idx["Va"])] = meas_k["Va"]
Meas, R, meas_inds = construct_meas_vector(meas_k,umeas_k,meas_names)
iekf_stop = 1; j1 = 1
eta = xhatfc
while (iekf_stop > accuracy) and (j1 < maxiter):
V = calcV(mu, eta, meas_k, umeas_k, k)
Eq1 = np.dot(Dm.T, f_hSK(*V)) # bus power from nodal voltage at measured buses
Eq2 = f_hSl(*V) # from/to power and voltage magnitude at measured buses
h = np.r_[Eq1, Eq2]
JdSdV = J_dSdV(*V)
JdVdDS = np.linalg.solve(JdSdV[non_ref,:], Dnm[non_ref,:]) # Jacobian of inverse of 'bus power from nodal voltage'
JdhdV = np.r_[np.dot(Dm.T, JdSdV), J_dHdV(*V)]
H = np.dot(JdhdV, JdVdDS)
K = np.dot(Pfc, np.linalg.solve((np.dot(H, np.dot(Pfc, H.T)) + R).T, H).T)
temp = eta.copy()
eta = xhatfc + np.dot(K, Meas - h - np.dot(H, xhatfc - eta))
iekf_stop = np.linalg.norm(temp-eta)
j1 += 1
P.append(np.dot(np.eye(n) - np.dot(K, H), Pfc))
xhat[:, k] = eta
Shat[:, k] = u[:, k] + np.dot(Dnm, xhat[:, k])
Vhat[:, k] = V[:]
DeltaS[:,k-1] = xhat[:,k]
uDeltaS[:,k-1] = np.sqrt(np.diag(P[k]))
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 NLOextended(topology,
meas,
meas_unc,
meas_idx,
pseudo_meas,
model,
V0,
slack_idx=0,
Y=None,
accuracy=1e-9,
maxiter=5):
"""
Iterated Extended Kalman filter for the nodal load observer (extended to all kind of measurements)
Real-valued matrices of complex-valued quantities are assumed to be structured as [ [real part], [imag part] ]
In this variant of the NLO, calculation of voltages in each time step is carried out by fitting to bus power.
The Kalman correction step uses nodal voltages, bus power and line power.
: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 standard 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 at all nodes except slack
:param Vs: voltage amplitude and phase at slack node
: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 slack_idx > 0:
raise NotImplementedError(
"Only slack node at bus index 0 implemented yet.")
meas_names = ["Pk", "Qk", "Pl", "Ql", "Vm", "Va"]
meas, meas_idx, meas_unc = repair_meas(meas,
meas_idx,
meas_unc,
expected_indices=meas_names)
n = model.dim
n_K = len(V0) / 2 # assume that V0 does not contain slack bus voltage
# remove index of slack bus
non_ref = range(1, n_K) + range(n_K + 1, 2 * n_K)
# generate matrices to map measured values to full list for all buses
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)
Dnm = model.adjust_Dnm(Dnm) # adjust for the case of AR(2) process
Dnm_nB = Dnm[non_ref, :]
# generate admittance matrix from network data if not provided by the user
if not isinstance(Y, np.ndarray):
Y = makeYbus(topology["baseMVA"], topology["bus"], topology["branch"])
y, cap = calc_admittance(topology["branch"])[1:]
# calculate network functions and their Jacobians
J_dSdV, J_dHdV, f_hSK, f_hSl = network_equations(Y, y, cap, n_K, meas_idx)
# calculate vector of nodal powers from actual and pseudo measurements
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)
nT = u.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)
P = []
# helper function
def calcV(V, eta, meas_k, umeas_k, k, PF_accuracy=1e-16):
"""Calculate nodal voltages by minimizing the difference between measured and calculated bus power using Newton's method
:rtype: tuple
:param meas_k: measured values
:param umeas_k: uncertainties associated with measured values
:return: V, h_V, Jacobian_V
"""
def voltage2buspower(v):
EqPF = f_hSK(*v)[non_ref]
JacSE = J_dSdV(*v)[non_ref]
return EqPF, JacSE
pf_stop = 1
pf_iter = 0
Jac_SfromV = None
SfromV = None
S = np.dot(Dm, np.r_[meas_k['Pk'], meas_k['Qk']]) + np.dot(
Dnm, Sfc[:, k]) + Dnm.dot(eta)
while pf_stop > PF_accuracy and pf_iter < 20:
SfromV, Jac_SfromV = voltage2buspower(V)
delta_V = np.linalg.solve(Jac_SfromV, S[non_ref] - SfromV)
V[non_ref] = V[non_ref] + delta_V
pf_stop = np.linalg.norm(delta_V)
pf_iter += 1
return V
# Iterated Extended Kalman Filter
for k in range(nT):
meas_k, umeas_k = meas_at_time(meas, meas_unc, k, meas_names)
if k == 0:
xhatfc = model.forecast_state()
Pfc = model.forecast_unc()
mu = V0
if len(meas_idx['Vm']) > 0:
mu[meas_idx["Vm"]] = meas_k["Vm"][:]
if len(meas_idx['Va']) > 0:
mu[n_K + np.asarray(meas_idx["Va"])] = meas_k["Va"][:]
# xhat[:,k] = xhatfc[:]
# P.append(Pfc)
# Shat[:, k] = u[:, k] + np.dot(Dnm, xhat[:, k])
# Vhat[:,k] = mu.copy()
# continue # at time 0 use forecast as estimate
else:
xhatfc = model.forecast_state(xhat[:, k - 1])
Pfc = model.forecast_unc(P[k - 1])
mu = Vhat[:, k - 1].copy()
if len(meas_idx['Vm']) > 0:
mu[meas_idx["Vm"]] = meas_k["Vm"]
if len(meas_idx['Va']) > 0:
mu[n_K + np.asarray(meas_idx["Va"])] = meas_k["Va"]
Meas, R, meas_inds = construct_meas_vector(meas_k, umeas_k, meas_names)
iekf_stop = 1
j1 = 1
eta = xhatfc
while (iekf_stop > accuracy) and (j1 < maxiter):
V = calcV(mu, eta, meas_k, umeas_k, k)
Eq1 = np.dot(
Dm.T,
f_hSK(*V)) # bus power from nodal voltage at measured buses
Eq2 = f_hSl(
*V) # from/to power and voltage magnitude at measured buses
h = np.r_[Eq1, Eq2]
JdSdV = J_dSdV(*V)
JdVdDS = np.linalg.solve(
JdSdV[non_ref, :], Dnm[non_ref, :]
) # Jacobian of inverse of 'bus power from nodal voltage'
JdhdV = np.r_[np.dot(Dm.T, JdSdV), J_dHdV(*V)]
H = np.dot(JdhdV, JdVdDS)
K = np.dot(
Pfc,
np.linalg.solve((np.dot(H, np.dot(Pfc, H.T)) + R).T, H).T)
temp = eta.copy()
eta = xhatfc + np.dot(K, Meas - h - np.dot(H, xhatfc - eta))
iekf_stop = np.linalg.norm(temp - eta)
j1 += 1
P.append(np.dot(np.eye(n) - np.dot(K, H), Pfc))
xhat[:, k] = eta
Shat[:, k] = u[:, k] + np.dot(Dnm, xhat[:, k])
Vhat[:, k] = V[:]
DeltaS[:, k - 1] = xhat[:, k]
uDeltaS[:, k - 1] = np.sqrt(np.diag(P[k]))
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
