From 6b1bc2af1b67d7edd6d9277c8281e40d78cd2758 Mon Sep 17 00:00:00 2001 From: pipeacosta Date: Mon, 18 Mar 2024 17:30:52 +0000 Subject: [PATCH] Implemented code for i_ipDFT and e_ipDFT Signed-off-by: pipeacosta --- lib/hooks/pmu_truncated_ipdft.cpp | 259 ++++++++++++++++-------------- 1 file changed, 142 insertions(+), 117 deletions(-) diff --git a/lib/hooks/pmu_truncated_ipdft.cpp b/lib/hooks/pmu_truncated_ipdft.cpp index d6b6a1fde..55526d41a 100644 --- a/lib/hooks/pmu_truncated_ipdft.cpp +++ b/lib/hooks/pmu_truncated_ipdft.cpp @@ -44,17 +44,17 @@ public: {} - // PhasorReIm hann_frt(double k, unsigned M) { - // PhasorReIm val; - // double m = M_PI / M; - // double a = 0.5 * sin(M_PI * k) / sin(k * M); - // double b = 0.25 * sin(M_PI * (k+1)) / sin((k+1) * m); - // double c = 0.25 * sin(M_PI * (k-1)) / sin((k-1) * m); + PhasorReIm hann_frt(double k, unsigned M) { + PhasorReIm val; + double m = M_PI / M; + double a = 0.5 * sin(M_PI * k) / sin(k * M); + double b = 0.25 * sin(M_PI * (k+1)) / sin((k+1) * m); + double c = 0.25 * sin(M_PI * (k-1)) / sin((k-1) * m); - // val.r = cos(k*(M-1)*m) * a - cos((k+1)*(M-1)*m) * b - cos((k-1)*(M-1)*m) * c; - // val.i = -sin(k*(M-1)*m) * a + sin((k+1)*(M-1)*m) * b + sin((k-1)*(M-1)*m) * c; - // return val; - // } + val.r = cos(k*(M-1)*m) * a - cos((k+1)*(M-1)*m) * b - cos((k-1)*(M-1)*m) * c; + val.i = -sin(k*(M-1)*m) * a + sin((k+1)*(M-1)*m) * b + sin((k-1)*(M-1)*m) * c; + return val; + } void prepare() { PmuHook::prepare(); @@ -137,6 +137,15 @@ public: int km = 0; + double dfold = 0.0; // last df/dt of frequency + double df = 0.0; // df/dt of frequency + double d2fold = 0.0; // last d2f/dt2 of frequency + double d2f = 0.0; // d2f/dt2 of frequency + double rocof = 0.0; // ROCOF + double fold = 0.0; // last frequency value + double f = 0.0; // current frequency value + double state_rocof = false; // TRUE for Dynamic, False for Static State + for (unsigned n = 0; n < windowSize; n++) { for (unsigned k = 0; k < frequencyCount; k++) { // Real part of X[k] @@ -178,146 +187,162 @@ public: a = 2 * M_PI * delta * (1 - pow(delta, 2)) / sin(M_PI * delta); } - estimatedPhasor.frequency=(km + 1 + startBin + delta) * phasorRate; - estimatedPhasor.amplitude = Xk[km].mag * a; - estimatedPhasor.phase = atan2(Xk[km].i , Xk[km].r) - M_PI*(delta); - estimatedPhasor.rocof = 0; - // i_ipDFT starts here /*i_ipDFT works well for higher reporting rates i.e. also Nr=25, but e_ipDFT outperforms i_ipDFT for lower reporting rates (e.g. Nr=10) i_ipDFT (variables definition) starts here*/ - // PhasorReIm phasor; + PhasorReIm phasor; - // if (phasorRate > 10) { - // phasor.r = Xk[km].r; - // phasor.i = Xk[km].i; - // PhasorReIm phasor_epsil = phasor; - // PhasorReIm Xneg = phasor; - // PhasorReIm Xneg_epsil = phasor; - // phasor_epsil.r = Xk[km+epsil].r; - // phasor_epsil.i = Xk[km+epsil].i; - // double delta_old = delta; + if (phasorRate > 10) { + phasor.r = Xk[km].r; + phasor.i = Xk[km].i; + PhasorReIm phasor_epsil = phasor; + PhasorReIm Xneg = phasor; + PhasorReIm Xneg_epsil = phasor; + phasor_epsil.r = Xk[km+epsil].r; + phasor_epsil.i = Xk[km+epsil].i; + double delta_old = delta; - // // i_ipDFT - // for (unsigned q = 0; q < P; q++) { - // PhasorReIm hann_ft = hann_frt(delta + 2*(km+startBin-1), windowSize); - // Xneg.r = 0.5*(phasor.r*hann_ft.r + phasor.i*hann_ft.i)*b; - // Xneg.i = 0.5*(phasor.r*hann_ft.i - phasor.i*hann_ft.r)*b; + // i_ipDFT + for (unsigned q = 0; q < P; q++) { + PhasorReIm hann_ft = hann_frt(delta + 2*(km+startBin-1), windowSize); + Xneg.r = 0.5*(phasor.r*hann_ft.r + phasor.i*hann_ft.i)*b; + Xneg.i = 0.5*(phasor.r*hann_ft.i - phasor.i*hann_ft.r)*b; - // hann_ft = hann_frt(delta + epsil + 2*(km+startBin-1), windowSize); - // Xneg_epsil.r = 0.5*(phasor.r*hann_ft.r + phasor.i*hann_ft.i)*b; - // Xneg_epsil.i = 0.5*(phasor.r*hann_ft.i - phasor.i*hann_ft.r)*b; - // phasor.r = phasor.r-Xneg.r; - // phasor.i = phasor.i-Xneg.i; + hann_ft = hann_frt(delta + epsil + 2*(km+startBin-1), windowSize); + Xneg_epsil.r = 0.5*(phasor.r*hann_ft.r + phasor.i*hann_ft.i)*b; + Xneg_epsil.i = 0.5*(phasor.r*hann_ft.i - phasor.i*hann_ft.r)*b; + phasor.r = phasor.r-Xneg.r; + phasor.i = phasor.i-Xneg.i; - // phasor_epsil.r = phasor_epsil.r - Xneg_epsil.r; - // phasor_epsil.i = phasor_epsil.i - Xneg_epsil.i; + phasor_epsil.r = phasor_epsil.r - Xneg_epsil.r; + phasor_epsil.i = phasor_epsil.i - Xneg_epsil.i; - // Xk[km].mag = pow(pow(phasor.r, 2) + pow(phasor.i, 2), 0.5); - // Xk[km+epsil].mag = pow(pow(phasor_epsil.r, 2) + pow(phasor_epsil.i, 2), 0.5); + Xk[km].mag = pow(pow(phasor.r, 2) + pow(phasor.i, 2), 0.5); + Xk[km+epsil].mag = pow(pow(phasor_epsil.r, 2) + pow(phasor_epsil.i, 2), 0.5); - // alpha = abs(Xk[km].mag / Xk[km+epsil].mag); - // delta = epsil*(2-alpha) / (1+alpha); + alpha = abs(Xk[km].mag / Xk[km+epsil].mag); + delta = epsil*(2-alpha) / (1+alpha); - // if (abs(delta - delta_old)==0){ - // q = P+1; - // } - // } - // } else { + if (abs(delta - delta_old)==0){ + q = P+1; + } + } + } else { - // // end of i_ipDFT + // end of i_ipDFT - // /*e_ipDFT - // e_ipDFT is very good from Nr=10 and Fs=10k; Not good otherwise. - // Nonetheless, for Nr>=10 it presents better precision than i_ipDFT - // */ + /*e_ipDFT + e_ipDFT is very good from Nr=10 and Fs=10k; Not good otherwise. + Nonetheless, for Nr>=10 it presents better precision than i_ipDFT + */ - // phasor.r = Xk[km].r; - // phasor.i = Xk[km].i; + phasor.r = Xk[km].r; + phasor.i = Xk[km].i; - // PhasorReIm v_ipdft; - // v_ipdft.r = a*(Xk[km].r*cos(M_PI*delta)+Xk[km].i*sin(M_PI*delta)); - // v_ipdft.i = a*(-Xk[km].r*sin(M_PI*delta)+Xk[km].i*cos(M_PI*delta)); + PhasorReIm v_ipdft; + v_ipdft.r = a*(Xk[km].r*cos(M_PI*delta)+Xk[km].i*sin(M_PI*delta)); + v_ipdft.i = a*(-Xk[km].r*sin(M_PI*delta)+Xk[km].i*cos(M_PI*delta)); - // // This part is iterated P times - // // variables allocation - // PhasorReIm v1; - // v1.r = v_ipdft.r; - // v1.i = v_ipdft.i; - // double e_delta_corr = delta; + // This part is iterated P times + // variables allocation + PhasorReIm v1; + v1.r = v_ipdft.r; + v1.i = v_ipdft.i; + double e_delta_corr = delta; - // std::vector e_ipdft_3max; + std::vector e_ipdft_3max; - // for (unsigned k = 0; k < 3; k++) { - // e_ipdft_3max.push_back({0., 0.}); - // e_ipdft_3max[k] = phasor; - // } + for (unsigned k = 0; k < 3; k++) { + e_ipdft_3max.push_back({0., 0.}); + e_ipdft_3max[k] = phasor; + } - // e_ipdft_3max[0].r = Xk[km-1].r; - // e_ipdft_3max[0].i = Xk[km-1].i; - // e_ipdft_3max[1].r = Xk[km].r; - // e_ipdft_3max[1].i = Xk[km].i; - // e_ipdft_3max[2].r = Xk[km+1].r; - // e_ipdft_3max[2].i = Xk[km+1].i; + e_ipdft_3max[0].r = Xk[km-1].r; + e_ipdft_3max[0].i = Xk[km-1].i; + e_ipdft_3max[1].r = Xk[km].r; + e_ipdft_3max[1].i = Xk[km].i; + e_ipdft_3max[2].r = Xk[km+1].r; + e_ipdft_3max[2].i = Xk[km+1].i; - // std::vector e_ipdft_3mag; - // for (unsigned k = 0; k < 3; k++) { - // e_ipdft_3mag.push_back(0.); - // e_ipdft_3mag[k] = phasor.r; - // } + std::vector e_ipdft_3mag; + for (unsigned k = 0; k < 3; k++) { + e_ipdft_3mag.push_back(0.); + e_ipdft_3mag[k] = phasor.r; + } - // PhasorReIm v_e_ipdft_max = phasor; - // double e_a = phasor.r; - // PhasorReIm hann_ft = phasor; + PhasorReIm v_e_ipdft_max = phasor; + double e_a = phasor.r; + PhasorReIm hann_ft = phasor; - // std::vector e_ipdft_3max_new; - // for (unsigned k = 0; k < 3; k++) { - // e_ipdft_3max_new.push_back({0., 0.}); - // e_ipdft_3max_new[k] = phasor; - // } + std::vector e_ipdft_3max_new; + for (unsigned k = 0; k < 3; k++) { + e_ipdft_3max_new.push_back({0., 0.}); + e_ipdft_3max_new[k] = phasor; + } - // if (abs(delta) > 0) { - // for (unsigned q = 0; q < P; q++) { - // for (unsigned j = 0; j < 3; j++) { - // hann_ft = hann_frt(j-2+e_delta_corr+2*(km+startBin), windowSize); + if (abs(delta) > 0) { + for (unsigned q = 0; q < P; q++) { + for (unsigned j = 0; j < 3; j++) { + hann_ft = hann_frt(j-2+e_delta_corr+2*(km+startBin), windowSize); - // e_ipdft_3max_new[j].r = e_ipdft_3max[j].r-(v1.r*hann_ft.r + v1.i*hann_ft.i)*b; - // e_ipdft_3max_new[j].i = e_ipdft_3max[j].i+(v1.i*hann_ft.r - v1.r*hann_ft.i)*b; - // e_ipdft_3mag[j] = pow(pow(e_ipdft_3max_new[j].r, 2) + pow(e_ipdft_3max_new[j].i, 2), 0.5); - // } + e_ipdft_3max_new[j].r = e_ipdft_3max[j].r-(v1.r*hann_ft.r + v1.i*hann_ft.i)*b; + e_ipdft_3max_new[j].i = e_ipdft_3max[j].i+(v1.i*hann_ft.r - v1.r*hann_ft.i)*b; + e_ipdft_3mag[j] = pow(pow(e_ipdft_3max_new[j].r, 2) + pow(e_ipdft_3max_new[j].i, 2), 0.5); + } - // // interpolating the three bins to get the fractional correction term "delta_corr" - // e_delta_corr = 2*epsil*(abs(e_ipdft_3mag[2]-e_ipdft_3mag[0]))/(e_ipdft_3mag[1]*2 + e_ipdft_3mag[0]+ e_ipdft_3mag[2]); + // interpolating the three bins to get the fractional correction term "delta_corr" + e_delta_corr = 2*epsil*(abs(e_ipdft_3mag[2]-e_ipdft_3mag[0]))/(e_ipdft_3mag[1]*2 + e_ipdft_3mag[0]+ e_ipdft_3mag[2]); - // v_e_ipdft_max = e_ipdft_3max_new[1]; + v_e_ipdft_max = e_ipdft_3max_new[1]; - // if (abs(e_delta_corr)<0.00001) { - // e_delta_corr = 0; - // v1.r = v_e_ipdft_max.r; - // v1.i = v_e_ipdft_max.i; - // q = P+1; - // } else { - // e_a = (1-pow(e_delta_corr, 2))*abs((M_PI*e_delta_corr)/sin(M_PI*e_delta_corr)); - // v1.r = e_a*(v_e_ipdft_max.r*cos(M_PI*e_delta_corr)+v_e_ipdft_max.i*sin(M_PI*e_delta_corr)); - // v1.i = e_a*(-v_e_ipdft_max.r*sin(M_PI*e_delta_corr)+v_e_ipdft_max.i*cos(M_PI*e_delta_corr)); - // } + if (abs(e_delta_corr)<0.00001) { + e_delta_corr = 0; + v1.r = v_e_ipdft_max.r; + v1.i = v_e_ipdft_max.i; + q = P+1; + } else { + e_a = (1-pow(e_delta_corr, 2))*abs((M_PI*e_delta_corr)/sin(M_PI*e_delta_corr)); + v1.r = e_a*(v_e_ipdft_max.r*cos(M_PI*e_delta_corr)+v_e_ipdft_max.i*sin(M_PI*e_delta_corr)); + v1.i = e_a*(-v_e_ipdft_max.r*sin(M_PI*e_delta_corr)+v_e_ipdft_max.i*cos(M_PI*e_delta_corr)); + } - // } - // phasor = v1; - // delta = e_delta_corr; - // } + } + phasor = v1; + delta = e_delta_corr; + } - // } + } // end e_ipDFT - // double f = 0; - // double fold = f; - // Xk[km].f = (km + 1 + startBin + delta) * phasorRate; - // if (fold > 0) { + fold = f; + Xk[km].f = (km + 1 + startBin + delta) * phasorRate; + if (fold > 0) { + dfold = df; + df = (f - fold)*phasorRate; + d2fold = d2f; + d2f = (d2f - d2fold)*phasorRate; - // } + if (abs(df)>3 || abs(d2f>25 || state_rocof)) { + state_rocof = true; + rocof = 0.2043*df + 0.2043*dfold + 0.5913*rocof; + if (abs(rocof) < 0.035) { + state_rocof = false; + } + } else { + rocof = df; + } + } + + if (abs(delta) < 0.00001) { + estimatedPhasor.amplitude = 2*Xk[km].mag; + } else { + estimatedPhasor.amplitude = 2*Xk[km].mag*M_PI*delta*(1-pow(delta, 2)) / sin(M_PI * delta); + } + estimatedPhasor.phase = atan2(Xk[km].i, Xk[km].r) - M_PI*delta; + estimatedPhasor.frequency = Xk[km].f; + estimatedPhasor.rocof = rocof; if (lastPhasor.frequency != 0) // Check if we already calculated a phasor before