simple windowed sinc filter

src/lock-in.cc

@@ -8,80 +8,111 @@ namespace mri {
   void LockIn::Analyse(const std::vector<double> &signal,double period_ms,
                        const std::vector<double> &timesteps,
                        int samples_per_period,
-                       std::vector<double> &avg_sin,
-                       std::vector<double> &avg_cos) {
+                       std::vector<double> &out_sin,
+                       std::vector<double> &out_cos) {
     period_ms_=period_ms;
     total_samples_ = signal.size();
+    mag_sin_.resize(total_samples_);
+    mag_cos_.resize(total_samples_);
+    samples_per_period_ = samples_per_period;
     // Stage 1: Multiply
     GenerateSinCos(timesteps);
-    std::vector<double> mul_sin, mul_cos;
-    mul_sin.resize(total_samples_);
-    mul_cos.resize(total_samples_);
+    Multiply(signal);
+    // Stage 2: Low-pas filter:
+    //FilterLowPassFirstOrder();
+    //FilterLowPassForthOrder();
+    //FilterLowPassPeriodAverage();
+    FilterLowPassWindowedSinc();
+
+
+    unsigned long filtered_size = mag_sin_.size();
+    out_sin.resize(filtered_size);
+    out_cos.resize(filtered_size);
+    for (unsigned long i = 0; i<filtered_size; ++i) {
+      out_sin[i] = mag_sin_[i];
+      out_cos[i] = mag_cos_[i];
+      // out_sin[i] = mul_sin_[i];
+      // out_cos[i] = mul_cos_[i];
+    }
+    
+    
+  };
+  // ********************************************************************** //
+  // ********************************************************************** //
+  // ********************************************************************** //
+  void LockIn::Multiply(const std::vector<double> &signal) {
+    mul_sin_.resize(total_samples_);
+    mul_cos_.resize(total_samples_);
     for (unsigned long i = 0; i < total_samples_; ++i) {
-      mul_sin[i] = signal[i]*lockin_sin_[i];
-      mul_cos[i] = signal[i]*lockin_cos_[i];
+      // mul_sin_[i] = signal[i]*lockin_sin_[i];
+      // mul_cos_[i] = signal[i]*lockin_cos_[i];
+      mul_sin_[i] = lockin_sin_[i]+1;
+      mul_cos_[i] = lockin_cos_[i]+2;
     }
-    // Stage 2: Low-pas filter:
-    //y[n] = a0 x[n] + b1 y[n-1]
-    // a0 = 1-x
-    // b1 = x
-    // x = exp (-1/samples)
-    /// a0 = (1-x)^4
-    // b1 = 4x
-    // b2 = -6 x^2
-    // b3 = 4 x^3
-    // b4 = - x^4
-    double x = std::exp(-1.0/(samples_per_period*2));
-    std::vector<double> mag_sin, mag_cos;
-    mag_sin.resize(total_samples_);
-    mag_cos.resize(total_samples_);
-    avg_sin.resize(total_samples_);
-    avg_cos.resize(total_samples_);
+  };
+  // ********************************************************************** //
+  // ********************************************************************** //
+  // ********************************************************************** //
+  void LockIn::FilterLowPassPeriodAverage() {
     double mean_sin = 0.0, mean_cos = 0.0;
-    for (int i = 0; i < samples_per_period; ++i){
-      mag_sin[i] = mul_sin[i];
-      mag_cos[i] = mul_cos[i];
-      avg_sin[i] = mul_sin[i];
-      avg_cos[i] = mul_cos[i];
-      mean_sin += mul_sin[i];
-      mean_cos += mul_cos[i];
+    for (int i = 0; i < samples_per_period_; ++i){
+      mag_sin_[i] = mul_sin_[i];
+      mag_cos_[i] = mul_cos_[i];
+      mean_sin += mul_sin_[i];
+      mean_cos += mul_cos_[i];
     }
-    mean_sin /= samples_per_period;
-    mean_cos /= samples_per_period;
-    avg_sin[samples_per_period] = mean_sin;
-    avg_cos[samples_per_period] = mean_cos;
-    //First order low-pass
-    double a0 = 1-x,  b1 = x;
-    for (unsigned long n = samples_per_period+1; n<total_samples_; ++n) {
-      mag_sin[n] = a0*mul_sin[n] + b1*mag_sin[n-1];
-      mag_cos[n] = a0*mul_cos[n] + b1*mag_cos[n-1];
-      avg_sin[n] = avg_sin[n-1]
-        + (mul_sin[n] - mul_sin[n-samples_per_period])/samples_per_period;
-      avg_cos[n] = avg_cos[n-1]
-        + (mul_cos[n] - mul_cos[n-samples_per_period])/samples_per_period;
+    mean_sin /= samples_per_period_;
+    mean_cos /= samples_per_period_;
+    mag_sin_[samples_per_period_] = mean_sin;
+    mag_cos_[samples_per_period_] = mean_cos;
+    for (unsigned long n = samples_per_period_+1; n<total_samples_; ++n) {
+      mag_sin_[n] = mag_sin_[n-1]
+        + (mul_sin_[n] - mul_sin_[n-samples_per_period_])/samples_per_period_;
+      mag_cos_[n] = mag_cos_[n-1]
+        + (mul_cos_[n] - mul_cos_[n-samples_per_period_])/samples_per_period_;
     }
-    // // Forth order low-pass
-    // double a0 = pow2(pow2(1-x)), b1 = 4*x, b2 = -6*pow2(x), b3 = 4*x*x*x, b4 = - pow2(pow2(x));
-    // for (unsigned long n = 1; n<total_samples_; ++n) {
-    //   mag_sin[n] = a0*mul_sin[n] + b1*mag_sin[n-1]
-    //     + b2*mag_sin[n-2] + b3*mag_sin[n-3] + b4*mag_sin[n-4];
-    //   mag_cos[n] = a0*mul_cos[n] + b1*mag_cos[n-1]
-    //     + b2*mag_cos[n-2] + b3*mag_cos[n-3] + b4*mag_cos[n-4];
-    // }
 
-    // Stage 3: Evaluate magnitude and phase
-    std::vector<double> out_magnitude, out_phase;
-    out_magnitude.resize(total_samples_);
-    out_phase.resize(total_samples_);
-    for (unsigned long i = 0; i<total_samples_; ++i) {
-      out_magnitude[i] = std::sqrt(
-                                   pow2(avg_sin[i]) +
-                                   pow2(avg_cos[i])
-                                   );
-      out_phase[i] = std::atan(avg_sin[i]/avg_cos[i]);
+  };
+  // ********************************************************************** //
+  // ********************************************************************** //
+  // ********************************************************************** //
+  //First order low-pass filter
+  // y[n] = a0 x[n] + b1 y[n-1]
+  // a0 = 1-x
+  // b1 = x
+  // x = exp (-1/samples)
+  void LockIn::FilterLowPassFirstOrder() {
+    double x = std::exp(-1.0/(samples_per_period_*2));
+    mag_sin_[0] = mul_sin_[0];
+    mag_cos_[0] = mul_cos_[0];
+    double a0 = 1-x,  b1 = x;
+    for (unsigned long n = 1; n<total_samples_; ++n) {
+      mag_sin_[n] = a0*mul_sin_[n] + b1*mag_sin_[n-1];
+      mag_cos_[n] = a0*mul_cos_[n] + b1*mag_cos_[n-1];
+    }
+  };
+  // ********************************************************************** //
+  // ********************************************************************** //
+  // ********************************************************************** //
+  // Forth order low-pass filter
+  /// a0 = (1-x)^4
+  // b1 = 4x
+  // b2 = -6 x^2
+  // b3 = 4 x^3
+  // b4 = - x^4
+  void LockIn::FilterLowPassForthOrder() {
+    double x = std::exp(-1.0/(samples_per_period_*2));
+    for (int i = 0; i < 5; ++i){
+      mag_sin_[i] = mul_sin_[i];
+      mag_cos_[i] = mul_cos_[i];
+    }
+    double a0 = pow2(pow2(1-x)), b1 = 4*x, b2 = -6*pow2(x), b3 = 4*x*x*x, b4 = - pow2(pow2(x));
+    for (unsigned long n = 4; n<total_samples_; ++n) {
+      mag_sin_[n] = a0*mul_sin_[n] + b1*mag_sin_[n-1]
+        + b2*mag_sin_[n-2] + b3*mag_sin_[n-3] + b4*mag_sin_[n-4];
+      mag_cos_[n] = a0*mul_cos_[n] + b1*mag_cos_[n-1]
+        + b2*mag_cos_[n-2] + b3*mag_cos_[n-3] + b4*mag_cos_[n-4];
     }
-    
-    
   };
   // ********************************************************************** //
   // ********************************************************************** //
@@ -101,5 +132,44 @@ namespace mri {
   // ********************************************************************** //
   // ********************************************************************** //
   // ********************************************************************** //
+  void LockIn::FilterLowPassWindowedSinc() {
+    double fc = 0.05; // Cutoff frequency as a fraction of the sampling rate (in (0, 0.5)).
+    int N = 1001; // Kernel size, should be odd.
+    int M = N-1;
+    int mid = static_cast<int>(M/2);
+    //Evaluate kernel
+    std::vector<double> h(N);
+    for (int i = 0; i<N; ++i) {
+      double x = static_cast<double>(i-mid);
+      if (i == mid) {h[i] = 2.0*PI_*fc;}
+      else {h[i] = std::sin(2*PI_*fc *x)/x;}
+      //Blackman window
+      h[i] *= 0.42 - 0.5*std::cos(2.0*PI_*i/M)+0.08*cos(4.0*PI_*i/M);
+      //std::cout<< x<< ":  "<< h[i] <<std::endl;
+    }
+    //Normalize kernel
+    double sum = 0.0;
+    for (auto kern:h) sum+=kern;
+    for (auto &kern:h) kern/=sum;
+    // for (int i = 0; i<N; ++i) std::cout<< h[i] <<std::endl;
+    //Convolution
+    for (long j = 0; j < N; ++j) {
+      mag_sin_[j] = 0;
+      mag_cos_[j] = 0;
+      mul_sin_[j] = 0;
+      mul_cos_[j] = 0;
+    }
+    for (unsigned long j = N-1; j < total_samples_; ++j) {
+      mag_sin_[j] = 0;
+      mag_cos_[j] = 0;
+      for (int i =0; i<N; ++i) {
+        mag_sin_[j] += mul_sin_[j-i]*h[i];        
+        mag_cos_[j] += mul_cos_[j-i]*h[i];        
+      }      
+    }
+  };
+  // ********************************************************************** //
+  // ********************************************************************** //
+  // ********************************************************************** //
 
 }  // end of namespace mri

src/lock-in.h

@@ -15,9 +15,18 @@ namespace mri {
                  std::vector<double> &avg_cos);
   private:
     std::vector<double> lockin_sin_, lockin_cos_;
+    std::vector<double> mul_sin_, mul_cos_;
+    std::vector<double> mag_sin_, mag_cos_;
     double period_ms_, omega_;
     unsigned long total_samples_;
+    int samples_per_period_;
     void GenerateSinCos(const std::vector<double> &timesteps);
+    void Multiply(const std::vector<double> &signal);
+    void FilterLowPassFirstOrder();
+    void FilterLowPassForthOrder();
+    void FilterLowPassPeriodAverage();
+    void FilterLowPassWindowedSinc();
+
   };  // end of class LockIn
 }  // end of namespace mri
 #endif  // SRC_LOCK_IN_H_

src/main.cc

@@ -14,7 +14,7 @@
 
 const double PI_=3.14159265358979323846;
 
-int voxel_num = 3;
+int voxel_num = 30;
 //double phase_range = 0;
 double phase_range = PI_/2;
 double phase_init = 0.0;
@@ -24,8 +24,8 @@ double noise_ratio = 1e8;
 
 // B0=1.5T freq=64Mhz, period = 15.6 ns
 double period_ms = 15.6/1000/1000; //ms
-double total_periods = 2000;
-int rf_samples_per_period = 20;
+double total_periods = 500;
+int rf_samples_per_period = 10;
 // double period_ms = 10; //ms
 // double total_periods = 2;
 // int rf_samples_per_period = 3;
@@ -53,21 +53,23 @@ int main(int argc, char *argv[]) {
     generator_rf.EvaluateDemodulated(mag_sin,mag_cos);
     auto timesteps = generator_rf.GetTimesteps();
     mri::LockIn lock_in;
-    std::vector<double> avg_sin, avg_cos;
+    std::vector<double> out_sin, out_cos;
     lock_in.Analyse(signal_rf, period_ms, timesteps,
-                    rf_samples_per_period, avg_sin, avg_cos);
+                    rf_samples_per_period, out_sin, out_cos);
     for (unsigned long i = 0; i<timesteps.size(); ++i) {
-      if (i > static_cast<unsigned long>(rf_samples_per_period)*2) 
+      //      if (i > static_cast<unsigned long>(rf_samples_per_period)*2) 
         std::cout << timesteps[i] << "\t"
                   << signal_rf[i] << "\t"
         // << mul_sin[i] << "\t"
         // << mul_cos[i] << "\t"
-          //         << std::sqrt(pow2(mag_sin[i])+pow2(mag_cos[i]))
-          // /std::sqrt(pow2(avg_sin[i])+pow2(avg_cos[i])) << "\t"
+                  << std::sqrt(pow2(mag_sin[i])+pow2(mag_cos[i]))
+          /std::sqrt(pow2(out_sin[i])+pow2(out_cos[i])) << "\t"
                   << std::sqrt(pow2(mag_sin[i])+pow2(mag_cos[i])) << "\t"
-                  << 2.0*std::sqrt(pow2(avg_sin[i])+pow2(avg_cos[i])) << "\t"
-                  << avg_sin[i] << "\t"
-                  << avg_cos[i]
+                  << std::sqrt(pow2(out_sin[i])+pow2(out_cos[i])) << "\t"
+                  << out_sin[i] << "\t"
+                  << out_cos[i] << "\t"
+                  << mag_sin[i] << "\t"
+                  << mag_cos[i]
                   << std::endl;
     }
     //std::cout<< timesteps.size() <<std::endl;