From 6b1bc2af1b67d7edd6d9277c8281e40d78cd2758 Mon Sep 17 00:00:00 2001
From: pipeacosta <pipeacosta@gmail.com>
Date: Mon, 18 Mar 2024 17:30:52 +0000
Subject: [PATCH] Implemented code for i_ipDFT and e_ipDFT
Signed-off-by: pipeacosta <pipeacosta@gmail.com>
---
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<PhasorReIm> e_ipdft_3max;
+ std::vector<PhasorReIm> 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<double> e_ipdft_3mag;
- // for (unsigned k = 0; k < 3; k++) {
- // e_ipdft_3mag.push_back(0.);
- // e_ipdft_3mag[k] = phasor.r;
- // }
+ std::vector<double> 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<PhasorReIm> 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<PhasorReIm> 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