mirror of
https://git.rwth-aachen.de/acs/public/villas/node/
synced 2025-03-09 00:00:00 +01:00
Implemented code for i_ipDFT and e_ipDFT
Signed-off-by: pipeacosta <pipeacosta@gmail.com>
This commit is contained in:
pipeacosta
parent
a08b2fb2d3
commit
6b1bc2af1b
1 changed files with 142 additions and 117 deletions
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue