Merge pull request #7 from Mbrahim96/rocof-estimation

Rocof Estimation
chemsallioua · Feb 10, 2023 · 187478f · 187478f
2 parents 93eaa65 + 80840bf
commit 187478f
Show file tree

Hide file tree

Showing 4 changed files with 181 additions and 45 deletions.
diff --git a/README.md b/README.md
@@ -1,8 +1,10 @@
 # __m-class-pmu__
 A C implementtation of the M-Class Iterative Interpolated DFT Synchrophasor Estimattion Algorithm
-# Version 1.1.1
+# Version 1.2.0
 Updates:
-- feature: computation on multiple configurable channels
+- new feature: now the pmu_estimate() function computes also the ROCOF
+- added timer to keep track of the computational time vs input signal window length, number of iterations P and Q, and number of channels.
+
 ## __Activating Debug Logs__
 change the value of the _#define DEBUG 0_ in the __iter_e_ipdft_imp.h__ file
 
@@ -18,3 +20,10 @@ to compile and run the code simply run:
 
     gcc main.c iter_e_ipdft_imp.c -o main.exe
     ./main.exe
+
+## __Computational Time Evaluation__
+To keep track of the computational time per call of the __pmu_estimate()__ function for a certain number of calls, a timer is set.
+To change the number of calls to perform in order to compute the average time per call set the value of the following directive:
+
+    #define PERF_ITERATIONS 1000
+
diff --git a/iter_e_ipdft_imp.c b/iter_e_ipdft_imp.c
@@ -4,6 +4,8 @@
   Source for the implementation of the pmu estimator based on the Iterative
   Enhanced interpolated DFT Algorithm.
 
+  Authors: Chemseddine Allioua, Brahim Mazighi  
+
   Copyright (c) 2023.
   All Rights Reserved.
   Confidential and Proprietary - University of Bologna.
@@ -18,21 +20,30 @@
 // Synchrophasor Estimation Parameters
 static unsigned int g_n_channls;        // nuumber of input channels
 static unsigned int g_win_len;          // number of samples in observation window
+static unsigned int g_f0;               // fundamental frequency in Hz
+static unsigned int g_frame_rate;       // frame rate in Frames/second
 static double g_fs;                     // sample rate in Sample/s
 static unsigned int g_n_bins;           // number of bins to define estimation freq band
 static unsigned int g_P;                // number of iterations in e_ipDFT
 static unsigned int g_Q;                // number of iterations in iter_i_e_ipDFT
 static double g_interf_trig;            // trigger value for interference calculation
 static double g_df;                     // frequency resolution
-static synchrophasor g_phasor;          // global phasor bin
+static phasor g_phasor;                 // global phasor bin
 static double g_norm_factor;            // hann normalization factor 
 
 // Dynamically allocated arrays
-static double complex* g_Xf;            // Fundamental frequency spectrum bins arry ptr
-static double complex* g_Xi;            // Interference frequency spectrum bins arry ptr
-static double complex* g_dftbins;       // DFT bins array ptr
+static double complex* g_Xf;            // fundamental frequency spectrum bins arry ptr
+static double complex* g_Xi;            // interference frequency spectrum bins arry ptr
+static double complex* g_dftbins;       // dft bins array ptr
 static double* g_hann_window;           // hann coefficients array ptr
-static double** g_signal_windows;        // input signal windows pointer
+static double** g_signal_windows;       // input signal windows pointer
+
+/*ROCOF estimation variables*/
+static double* g_freq_old;              // represents f(n-1) and it is initialized to f0 Hz
+static double g_thresholds[3];       	// thresholds to trigger change in the g_state S1,S2 for rocof estimation
+static double g_low_pass_coeff[3];      // low pass filter coefficients a1 , b0, b1 respectively in a digital first-order IIR low-pass filter : y[n]=b0x[n]+b1x[n−1]−a1y[n−1]
+static double* g_delay_line[2];         // represents pre-filter rocof x(n-1) and after filter rocof y(n-1) and it is initialized to zero Hz/s
+static _Bool* g_state;                  // "0" zero for static conditions, "1" one for dynamic conditions 
 
 // Pmu estimator inizialization flag
 static _Bool g_pmu_initialized = 0;
@@ -46,10 +57,10 @@ static int dft_r(double* in_ptr, double complex* out_ptr , unsigned int out_len,
 static double hann(double* out_ptr, unsigned int out_len);
 
 // phasor and frequency estimation main functions 
-static void pureTone(double complex* Xpure, synchrophasor phasor);
-static int ipDFT(double complex* Xdft, synchrophasor* phasor);
-static void e_ipDFT(double complex* Xdft, synchrophasor* phasor);
-static void iter_e_ipDFT(complex* dftbins, complex* Xi, complex* Xf, synchrophasor* f_phsr);
+static void pureTone(double complex* Xpure, phasor phasor);
+static int ipDFT(double complex* Xdft, phasor* phasor);
+static void e_ipDFT(double complex* Xdft, phasor* out_phasor);
+static void iter_e_ipDFT(complex* dftbins, complex* Xi, complex* Xf, phasor* f_phsr);
 
 // phasor and frequency estimation helping functions
 inline static double complex whDFT(double k, int N); 
@@ -73,19 +84,37 @@ double wrap_angle(double rad_angle){
 //pmu initialization function implementation
 int pmu_init(void* cfg){
 
+    // check if the pmu estimator is already initialized
+    if(g_pmu_initialized){
+        printf("[%s] ERROR: pmu estimator already initialized\n",__FUNCTION__);
+        return -1;
+    }
+
     debug("[%s] Initializing pmu estimator\n",__FUNCTION__);
 
     estimator_config* config = (estimator_config*)cfg;
-
+
+    // pmu estimator parameters initialization from input config
     g_win_len = config->win_len;
     g_fs = config->fs;
+    g_f0 = config->f0;
+    g_frame_rate = config->frame_rate;
     g_n_bins = config->n_bins;
     g_P = config->P;
     g_Q = config->Q;
     g_interf_trig = config->interf_trig;
     g_df = g_fs/(double)g_win_len;
     g_n_channls = config->n_chanls;
 
+    g_thresholds[0] = config->rocof_thresh[0];
+    g_thresholds[1] = config->rocof_thresh[1];
+    g_thresholds[2] = config->rocof_thresh[2];
+
+    g_low_pass_coeff[0] = config->rocof_low_pass_coeffs[0];
+    g_low_pass_coeff[1] = config->rocof_low_pass_coeffs[1];
+    g_low_pass_coeff[2] = config->rocof_low_pass_coeffs[2];
+
+    // allocate memory for the global arrays
     if (NULL == (g_Xf = malloc(g_n_bins*sizeof(double complex))  )){
 		printf("[%s] ERROR: g_Xf memory allocation failed\n",__FUNCTION__);
 		return -1;}
@@ -102,16 +131,38 @@ int pmu_init(void* cfg){
     if (NULL == (g_signal_windows = (double **)malloc(g_n_channls * sizeof(double *))) ){
 		printf("[%s] ERROR: g_signal_windows memory allocation failed\n",__FUNCTION__);
 		return -1;}
-
     int i;
     for (i = 0; i < g_n_channls; i++){
         if (NULL == (g_signal_windows[i] = (double *)malloc(g_win_len * sizeof(double)) )){
             printf("[%s] ERROR: g_signal_windows memory allocation failed\n",__FUNCTION__);
             return -1;}
     }
+
+    if (NULL == (g_delay_line[0] = malloc(g_n_channls*sizeof(double))  )){
+        printf("[%s] ERROR: g_delay_line memory allocation failed\n",__FUNCTION__);
+        return -1;}
+    if (NULL == (g_delay_line[1] = malloc(g_n_channls*sizeof(double))  )){
+        printf("[%s] ERROR: g_delay_line memory allocation failed\n",__FUNCTION__);
+        return -1;}
+    if (NULL == (g_freq_old = malloc(g_n_channls*sizeof(double))  )){
+        printf("[%s] ERROR: g_freq_old memory allocation failed\n",__FUNCTION__);
+        return -1;}
+    if (NULL == (g_state = malloc(g_n_channls*sizeof(_Bool))  )){
+        printf("[%s] ERROR: g_state memory allocation failed\n",__FUNCTION__);
+        return -1;}
+
+    // initialize the global arrays
+    for(i = 0; i < g_n_channls; i++){
+        g_delay_line[0][i] = 0.0;
+        g_delay_line[1][i] = 0.0;
+        g_freq_old[i] = g_f0;
+        g_state[i] = 0;
+    }
 
+    // initialize the hann coefficients
     g_norm_factor = hann(g_hann_window, g_win_len);
 
+    // pmu estimator is initialized successfully
     g_pmu_initialized = 1;
 
     debug("[%s] Pmu estimator initialized successfully\n",__FUNCTION__);
@@ -120,15 +171,17 @@ int pmu_init(void* cfg){
 }
 
 //pmu estimation function implementation
-int pmu_estimate(double* in_signal_windows[], synchrophasor* out_phasor){
+int pmu_estimate(double* in_signal_windows[], pmu_frame* out_frame){
 
     debug("[%s] pmu_estimate() started\n", __FUNCTION__);
 
+    // check if pmu estimator is initialized
     if(!g_pmu_initialized){
         printf("[%s] ERROR: pmu estimator not initialized, first initialize with pmu_init function\n",__FUNCTION__);
         return -1;
     }
 
+    // input signal windowing
     int i,j, chnl;
     for (chnl = 0; chnl < g_n_channls; chnl++) {
         for(i=0; i<g_win_len; i++){
@@ -138,18 +191,22 @@ int pmu_estimate(double* in_signal_windows[], synchrophasor* out_phasor){
 
     debug("[%s] windowing on all channels done successfully\n", __FUNCTION__);
 
+    // synchrophasor estimation for each channel 
     for (chnl = 0; chnl < g_n_channls; chnl++){
 
+        // compuute DFT of input signal
         dft_r(g_signal_windows[chnl], g_dftbins, g_win_len , g_n_bins);
 
         debug_bins(g_dftbins, g_n_bins, g_df, "Input Signal DFT BINS");
 
+        // perform enhanced interpolated DFT
         e_ipDFT(g_dftbins, &g_phasor);
         pureTone(g_Xf, g_phasor);
 
         double E_diff = 0;
         double E = 0;
 
+        // compute energy of both input signal and interference frequencies 
         for ( j = 0; j < g_n_bins; j++){
 
             g_Xi[j] = g_dftbins[j] - g_Xf[j];
@@ -160,13 +217,37 @@ int pmu_estimate(double* in_signal_windows[], synchrophasor* out_phasor){
 
         debug("Energy of Signal Spectrum = %lf | Energy of Difference= %lf\n",E, E_diff);
 
+        // check if interference is present to trigger iterative enhanced interpolated DFT
         if (E_diff > g_interf_trig*E){
             iter_e_ipDFT(g_dftbins, g_Xi, g_Xf, &g_phasor);
         }
 
-        out_phasor[chnl].freq = g_phasor.freq;
-        out_phasor[chnl].amp = 2*g_phasor.amp/g_norm_factor;
-        out_phasor[chnl].ph = g_phasor.ph;
+        // two state rocof estimation
+        double rocof = (g_phasor.freq - g_freq_old[chnl])*(float)g_frame_rate;
+        double rocof_der = (rocof - g_delay_line[0][chnl])*(float)g_frame_rate;
+
+        // update state
+        g_state[chnl] = (!g_state[chnl] && ( fabs(rocof) > g_thresholds[0] || fabs(rocof_der) > g_thresholds[1] )) ? 1 : g_state[chnl];
+        g_state[chnl] = (g_state[chnl] && fabs(rocof) < g_thresholds[2]) ? 0 : g_state[chnl];
+
+        // apply low pass filter if state is 0
+        if(!g_state[chnl]){
+                out_frame[chnl].rocof = g_low_pass_coeff[1]*rocof + 
+                                            g_low_pass_coeff[2]*g_delay_line[0][chnl] - 
+                                                g_low_pass_coeff[0]*g_delay_line[1][chnl];
+        }else {out_frame[chnl].rocof = rocof;}
+
+        // update old frequency
+        g_freq_old[chnl] = g_phasor.freq;
+
+        //update delay line
+        g_delay_line[0][chnl] = rocof;    //x(n-1)
+        g_delay_line[1][chnl] = out_frame[chnl].rocof; //y(n-1)
+
+        // populate output frame
+        out_frame[chnl].synchrophasor.freq = g_phasor.freq;
+        out_frame[chnl].synchrophasor.amp = 2*g_phasor.amp/g_norm_factor;
+        out_frame[chnl].synchrophasor.ph = g_phasor.ph;
 
     }
 
@@ -179,16 +260,22 @@ int pmu_deinit(){
 
     debug("[%s] Deinitializing pmu estimator\n",__FUNCTION__);
 
+    // check if pmu estimator is initialized
     if(!g_pmu_initialized){
         printf("[%s] ERROR: pmu estimator not initialized, first initialize with pmu_init function\n",__FUNCTION__);
         return -1;
     }
 
+    // free all allocated memory
 	free(g_Xf);
 	free(g_Xi);
 	free(g_dftbins);
     free(g_hann_window);
     free(g_signal_windows);
+    free(g_delay_line[0]);
+    free(g_delay_line[1]);
+    free(g_freq_old);
+    free(g_state);
 
     int i;
     for (i = 0; i < g_n_channls; i++){
@@ -198,15 +285,15 @@ int pmu_deinit(){
 
     debug("[%s] Pmu estimator deinitialized successfully\n",__FUNCTION__);
 
+    // set pmu estimator to uninitialized state
     g_pmu_initialized = 0;
     return 0;
 }
 
 static int dft_r(double* in_ptr, double complex* out_ptr , unsigned int out_len, unsigned int n_bins){
     // debug("dft------------------------\n");
     int k,n;
-    double E = 0;
-    double temp_abs;
+
     for (k = 0 ; k < n_bins ; ++k)
     {
         out_ptr[k] = 0;
@@ -227,7 +314,7 @@ static double hann(double* out_ptr, unsigned int out_len){
 
 }
 
-static void pureTone(double complex* Xpure, synchrophasor phasor){
+static void pureTone(double complex* Xpure, phasor phasor){
     debug("\n[pureTone] ===============================================\n");
     int i;
     for (i = 0; i < g_n_bins; i++)
@@ -241,7 +328,7 @@ static void pureTone(double complex* Xpure, synchrophasor phasor){
 
 }
 
-static int ipDFT(double complex* Xdft, synchrophasor* phasor){
+static int ipDFT(double complex* Xdft, phasor* phasor){
 
     int j, k1, k2,k3;
     double Xdft_mag[g_n_bins]; //magnitude of dft
@@ -284,11 +371,11 @@ static int ipDFT(double complex* Xdft, synchrophasor* phasor){
     }
 }
 
-static void e_ipDFT(double complex* Xdft, synchrophasor* phasor){
+static void e_ipDFT(double complex* Xdft, phasor* out_phasor){
 
     debug("\n[e_ipDFT] ===============================================\n");
 
-    synchrophasor phsr = *phasor;  
+    phasor phsr = *out_phasor;  
 
     if(!ipDFT(Xdft, &phsr)){        
         int i,j, p;
@@ -313,14 +400,14 @@ static void e_ipDFT(double complex* Xdft, synchrophasor* phasor){
     }
     debug("\n[END e_ipDFT]========================================================\n\n");
 
-    *phasor = phsr;
+    *out_phasor = phsr;
 
 }
 
-static void iter_e_ipDFT(complex* dftbins, complex* Xi, complex* Xf, synchrophasor* f_phsr){
+static void iter_e_ipDFT(complex* dftbins, complex* Xi, complex* Xf, phasor* f_phsr){
 
-        synchrophasor f_phasor = *f_phsr;
-        synchrophasor i_phasor;
+        phasor f_phasor = *f_phsr;
+        phasor i_phasor;
         double complex Xi_pure[g_n_bins];
 
         debug("\n[iter-e-ipDFT] ###############################################\n");

diff --git a/iter_e_ipdft_imp.h b/iter_e_ipdft_imp.h
@@ -3,6 +3,8 @@
 
   Pmu estimator header file. 
 
+  Authors: Chemseddine Allioua, Brahim Mazighi 
+
   Copyright (c) 2023.
   All Rights Reserved.
   Confidential and Proprietary - University of Bologna.
@@ -29,27 +31,36 @@
 typedef struct {
     unsigned int n_chanls;
     unsigned int win_len;
+    unsigned int f0;
+    unsigned int frame_rate;
     double fs;
     unsigned int n_bins;
     unsigned int P;
     unsigned int Q;
     double interf_trig;
+    double rocof_thresh[3];
+    double rocof_low_pass_coeffs[3];
 } estimator_config;
 
 typedef struct{
     double amp;
     double ph;
     double freq;
-}synchrophasor;
+}phasor;
+
+typedef struct{
+    phasor synchrophasor;
+    double rocof;
+}pmu_frame;
 
 //angle [-pi; pi] wrap in radiants
 double wrap_angle(double rad_angle);
 
 //pmu estimator initialization
 int pmu_init(void* cfg);
 
-//synchrophasor estimation
-int pmu_estimate(double* in_signal_windows[], synchrophasor* out_phasor);
+//synchrophasor, frequency, rocof estimation
+int pmu_estimate(double* in_signal_windows[], pmu_frame* out_frame);
 
 //pmu estimator deinitialization
 int pmu_deinit();