Cool Emerald: Adaptive Filter: BMFLC

Adaptive noise canceling techniques တွေဖြစ်တဲ့

Fourier Linear Combiner (FLC)
Weighted-frequency Fourier Linear Combiner (WFLC)
Bandlimited Multiple Fourier Linear Combiner (BMFLC)

အစရှိတဲ့ filter တွေ အကြောင်း ပြောချင်ပါတယ်။ FLC က frequency သိတဲ့ periodic signal တခုရဲ့ amplitude နဲ့ phase ကို least mean square (LMS) algorithm သုံးပြီး adapt လုပ်ယူ ခန့်မှန်းတာပါ။ WFLC ကတော့ FLC ကို ထပ်မံဖြည့်စွက်ပြီး frequency ကိုပါ သိစရာ မလိုပါဘူး။ ဒါကြောင့် FLC လို frequency အသေဖြစ်စရာ မလိုပဲ၊ frequency ပြောင်းလဲနေ တဲ့ reference signal တွေကို ပါ adapt လုပ်နိုင်ပါတယ်။ WFLC ရဲ့ အားနည်းချက်က reference signal မှာ dominant frequency တွေ အများကြီး ပါနေရင် အလုပ်ကောင်းကောင်း မလုပ်နိုင်တော့ ပါဘူး။ အဲ့ဒါကို ဖြေရှင်းဖို့ အတွက် ကြိုတင်သတ်မှတ်ထားတဲ့ frequency band ထဲက dominant frequency တွေ အများကြီးကို track လုပ်နိုင်တဲ့ BMFLC ကို သုံးနိုင်ပါတယ်။

Setup

Arduino zero pro ကို သုံးပြီး စမ်းကြည့်ပါမယ်။ Code တွေကို အခြား platform ပေါ်တွေမှာ ပါ အလွယ် တကူ ယူသုံးနိုင်အောင် C နဲ့ပဲ ရေးလိုက်ပါတယ်။ ပထမ အနေနဲ့ reference signal ကို generate လုပ်ကြည့်ပြီး noise ပေါင်းထည့်ပါမယ်။ အဲဒီ signal ကိုပဲ adaptive filter နဲ့ စစ်ပြီးတဲ့အခါ serial plotter မှာ မူရင်း reference signal နဲ့ နှိုင်းယှဉ် plot လုပ်ကြည့်ပါမယ်။ အခုနောက်ပိုင်း Arduino IDE တွေမှာ Serial Monitor နဲ့ အတူ၊ Serial Plotter ပါပါလာတော့ အဝင် signal ပြောင်းသွားတဲ့အခါ adaptive filter က ဘယ်လို adaptive လုပ်သွားတယ် ဆိုတာ serial plotter မှာ ကြည့်ရတာ ပိုပြီး ထင်သာမြင်သာ ရှိပါတယ်။

Figure. A simple setup using an Arduino Zero Pro board.

FLC

Fundamental frequency, $f_0$, ရှိတယ်ဆိုတာ သိထားတဲ့ periodic signal တခု ကို Fourier Linear Combiner (FLC) နဲ့ real-time model လုပ်ပြီး ခန့်မှန်း ဖော်ပြနိုင်ပါတယ် [Vaz, 1994]။ Least mean square (LMS) algorithm ကို သုံးပြီး Fourier components တွေကို adapt လုပ်တဲ့ FLC တခုရဲ့ တည်ဆောက်ပုံကို အောက်မှာ ပြထားပါတယ်။

Figure. FLC architecture.

သတ်မှတ်ထားတဲ့ Fourier components တွေအတွက် sine နဲ့ cosine components တွေပေါင်းထားတဲ့ Fourier linear combiner (FLC) ကိုအောက်ပါအတိုင်း ဖော်ပြနိုင်ပါတယ်။ $$ y_k=\sum^n_{r=1}a_{rk}\sin(r\omega_0 k)+b_{rk}\cos(r\omega_0 k) $$ အဲဒီမှာ k က sampling instant ကို ကိုယ်စားပြုပြီး၊ $y_k$ က အဲဒီ အချိန်မှာ ရှိတဲ့ estimated signal ဖြစ်ပါတယ်။ $a_{rk}$ နဲ့ $b_{rk}$ က သက်ဆိုင်ရာ harmonic frequency $r\omega_0$ အတွက် adaptive weights တွေ ဖြစ်ပါတယ်။

$x_k$ က reference input vector၊ $w_k$ က weight vector ဆိုရင် estimated signal $y_k$ ကို အောက်ပါအတိုင်း ဖော်ပြနိုင်ပါတယ်။ $$ \begin{equation} \mathbf{x}_k=\begin{bmatrix} \begin{bmatrix} \sin(\omega_{0} k) & \sin(2\omega_{0} k) & \dots & \sin(n\omega_{0} k) \end{bmatrix}^T\\ \begin{bmatrix} \cos(\omega_{0} k) & \cos(2\omega_{0} k) & \dots & \cos(n\omega_{0} k) \end{bmatrix}^T\\ \end{bmatrix} \end{equation} $$ $$ \begin{equation} \mathbf{w}_k=\begin{bmatrix} \begin{bmatrix} a_{1k} & a_{2k} & \dots & a_{nk} \end{bmatrix}^T\\ \begin{bmatrix} b_{1k} & b_{2k} & \dots & b_{nk} \end{bmatrix}^T\\ \end{bmatrix} \end{equation} $$ $$ \begin{equation} y_k=\mathbf{w}_k^T \mathbf{x}_k \end{equation} $$ FLC ရဲ့ weight တွေကို တော့ အောက်ပါအတိုင်း update လုပ်ပါတယ်။ $$ \begin{equation} \epsilon_k=s_k-y_k \end{equation} $$ $$ \begin{equation} \mathbf{w}_{k+1}=\mathbf{w}_k +2 \mu \mathbf{x}_k \epsilon_k \end{equation} $$

အဲဒီမှာ $x_k$ က reference input vector၊ $s_k$ က reference signal၊ $\epsilon_k$ က error term ဖြစ်ပြီး $\mu$ ကတော့ adaptive gain parameter ဖြစ်ပါတယ်။ $\mu$ တန်ဖိုး နည်းရင် adapt လုပ်တာ နှေးပါတယ်။ $\mu$ တန်ဖိုး ကြီးလွန်းရင်တော့ adapt မလုပ်နိုင်တော့ပဲ unstable ဖြစ်တတ်ပါတယ်။ Convergence ဖြစ်တဲ့ time constant က $\tau = \frac{1}{2\mu} $ ပါ။ အကယ်၍ constant (DC) တန်ဖိုး component ပါပါချင်ရင် တော့ $r$ ကို 0 ကနေ စနိုင်ပါတယ်။ FLC filter အတွက် C code တွေကို အောက်မှာ ပြထားပါတယ်။

// FLC filter
// An implementation of an adaptive filter called 
// Fourier Linear Combiner
// Author: Yan Naing Aye
// http://coolemerald.blogspot.sg/
// Date: 2016 April 15
//--------------------------------------------------------
//Constants
//#define PI 3.141592654   // π (predefined)
#define N 5       // number of harmonics including a constant component
#define F0 1      // f0: fundamental frequency
#define MU 0.01   // μ: adaptive gain parameter
//--------------------------------------------------------
//External variables
float v[N];//array of angular frequencies
float x[2][N];//reference input vector, 1st row for sin and 2nd row for cos
float w[2][N];//weight vector, 1st row for sin and 2nd row for cos
//--------------------------------------------------------
//initialize FLC filter
//Get angular velocities and initialize weights
void InitFLC()
{
  int i;
  for (i=0;i<N; i++) v[i]=i*2*PI*F0;//assign DC,f0 and its harmonics
  for (i=0;i<N; i++) {w[0][i]=0; w[1][i]=0;} //init weights
}
//--------------------------------------------------------
//FLC filter
//input (k: time instant, s: reference signal)
//output (y: estimated signal)
float FLC(float k,float s)
{
  int i; float err,y;
  //-----------------------------------------------------------------
  //find reference input vector
  for(i=0;i<N; i++){ 
    x[0][i]=sin(v[i]*k);
    x[1][i]=cos(v[i]*k);
  }
  //-----------------------------------------------------------------
  //find estimated signal, y
  for(i=0,y=0;i<N;i++){y+=w[0][i]*x[0][i]+ w[1][i]*x[1][i];}
  //-----------------------------------------------------------------
  //adapt the weights   
  for(i=0,err=s-y;i<N;i++){
    w[0][i]+=2*MU*x[0][i]*err;
    w[1][i]+=2*MU*x[1][i]*err;
  }
  return y;
}
//--------------------------------------------------------

Arduino အတွက် program ကို အောက်က link မှာ download လုပ်ယူနိုင်ပါတယ်။
Implementation of adaptive filters on Arduino

အဝင် signal ရဲ့ amplitude, frequency, phase စတာတွေကို အချိန်တခုပြီးတိုင်း ပြောင်းပြောင်း ပေးကြည့်တဲ့အခါ မြန်ဆန်ကောင်းမွန်စွာနဲ့ adapt လုပ်သွားတာကို serial plotter မှာ အောက်ကအတိုင်းတွေ့ရပါတယ်။

Figure. A plot showing how FLC adapts its estimated output (orange line) to an abrupt change in the generated noisy input signal(blue).

WFLC

Frequency မသိတဲ့ periodic signal တခု အတွက်ဆိုရင်တော့ Weighted-frequency Fourier Linear Combiner (WFLC) နဲ့ real-time model လုပ်ပြီး ခန့်မှန်း ဖော်ပြနိုင်ပါတယ် [Riviere, 1998]။ သတ်မှတ်ထားတဲ့ Fourier components တွေအတွက် sine နဲ့ cosine components တွေပေါင်းထားတဲ့ WFLC ကိုအောက်ပါအတိုင်း ဖော်ပြနိုင်ပါတယ်။ $$ y_k=\sum^n_{r=1}a_{rk}\sin(r\omega_{0_k} k)+b_{rk}\cos(r\omega_{0_k} k) $$ အဲဒီမှာ k က sampling instant ကို ကိုယ်စားပြုပြီး၊ $y_k$ က အဲဒီ အချိန်မှာ ရှိတဲ့ estimated signal ဖြစ်ပါတယ်။ $a_{rk}$ နဲ့ $b_{rk}$ က သက်ဆိုင်ရာ harmonic frequency $r\omega_{0_k}$ အတွက် adaptive weights တွေ ဖြစ်ပါတယ်။ FLC နဲ့ မတူတဲ့ အချက်က $ \omega_{0_k}$ က အချိန် ပေါ်မူတည်တဲ့ ကိန်းရှင် ဖြစ်နေတာပါ။

$x_k$ က reference input vector၊ $w_k$ က weight vector ဆိုရင် estimated signal $y_k$ ကို အောက်ပါအတိုင်း ဖော်ပြနိုင်ပါတယ်။ $$ \begin{equation} \mathbf{x}_k=\begin{bmatrix} \begin{bmatrix} \sin(\omega_{0_k} k) & \sin(2\omega_{0_k} k) & \dots & \sin(n\omega_{0_k} k) \end{bmatrix}^T\\ \begin{bmatrix} \cos(\omega_{0_k} k) & \cos(2\omega_{0_k} k) & \dots & \cos(n\omega_{0_k} k) \end{bmatrix}^T\\ \end{bmatrix} \end{equation} $$ $$ \begin{equation} \mathbf{w}_k=\begin{bmatrix} \begin{bmatrix} a_{1k} & a_{2k} & \dots & a_{nk} \end{bmatrix}^T\\ \begin{bmatrix} b_{1k} & b_{2k} & \dots & b_{nk} \end{bmatrix}^T\\ \end{bmatrix} \end{equation} $$ $$ \begin{equation} y_k=\mathbf{w}_k^T \mathbf{x}_k \end{equation} $$ FLC ရဲ့ weight တွေကို တော့ Least mean square (LMS) algorithm ကိုသုံးပြီးအောက်ပါအတိုင်း update လုပ်ပါတယ်။ $$ \begin{equation} \epsilon_k=s_k-y_k \end{equation} $$ $$ \begin{equation} \mathbf{w}_{k+1}=\mathbf{w}_k +2 \mu_1 \mathbf{x}_k \epsilon_k \end{equation} $$

အဲဒီမှာ $x_k$ က reference input vector၊ $s_k$ က reference signal၊ $\epsilon_k$ က error term ဖြစ်ပြီး $\mu_1$ ကတော့ adaptive gain parameter ဖြစ်ပါတယ်။
Fundamental angular frequencyကို တော့ modified least mean square algorithm ကိုသုံးပြီးအောက်ပါအတိုင်း update လုပ်ပါတယ်။ $$ \begin{equation} \omega_{0_{k+1}}=\omega_{0_k} +2 \mu_0 \epsilon_k \sum_{r=1}^n r [a_{rk}\cos(r\omega_{0_k} k)-b_{rk}\sin(r\omega_{0_k} k)] \end{equation} $$ အဲဒီမှာ $\epsilon_k$ က error term ဖြစ်ပြီး $\mu_0$ ကတော့ fundamental frequency အတွက် adaptive gain parameter ဖြစ်ပါတယ်။ WFLC တခုရဲ့ တည်ဆောက်ပုံကို အောက်မှာ ပြထားပါတယ်။

Figure. WFLC architecture.

WFLC filter အတွက် C code တွေကို အောက်မှာ ပြထားပါတယ်။

// WFLC filter
// An implementation of an adaptive filter called 
// Weighted-frequency Fourier Linear Combiner
// Author: Yan Naing Aye
// http://coolemerald.blogspot.sg/
// Date: 2016 April 15
//--------------------------------------------------------
//Constants
//#define PI 3.141592654   // π (predefined)
#define N 2            // number of harmonics
#define MU0 0.000001   // μ0: adaptive gain for fundamental frequency
#define MU1 0.01       // μ1: adaptive gain for reference input vector
//--------------------------------------------------------
//External variables
float v0=(2*PI);//ω0: fundamental angular frequency
float v[N];//array of angular frequencies
float x[2][N];//reference input vector, 1st row for sin and 2nd row for cos
float w[2][N];//weight vector, 1st row for sin and 2nd row for cos
//--------------------------------------------------------
//initialize WFLC filter
//Initialize weights
void InitWFLC()
{
  int i;
  for (i=0;i<N; i++) {w[0][i]=0; w[1][i]=0;} //init weights
}
//--------------------------------------------------------
//WFLC filter
//input (k: time instant, s: reference signal)
//output (y: estimated signal)
float WFLC(float k,float s)
{
  int i; float err,y,z;
  //-----------------------------------------------------------------
  //Get angular velocities depending on adjusted fundamental angular frequency
  for (i=0;i<N; i++) v[i]=(i+1)*v0;//assign v0 and its harmonics
  //-----------------------------------------------------------------
  //find reference input vector
  for(i=0;i<N; i++){ 
    x[0][i]=sin(v[i]*k);
    x[1][i]=cos(v[i]*k);
  }
  //-----------------------------------------------------------------
  //find estimated signal, y
  for(i=0,y=0;i<N;i++){y+=w[0][i]*x[0][i]+ w[1][i]*x[1][i];}
  //-----------------------------------------------------------------
  //adapt the weights 
  err=s-y;
  for(i=0,z=0;i<N;i++){z+=(i+1)*(w[0][i]*x[1][i]- w[1][i]*x[0][i]);}
  v0=v0+2*MU0*err*z;
  for(i=0;i<N;i++){
    w[0][i]+=2*MU1*x[0][i]*err;
    w[1][i]+=2*MU1*x[1][i]*err;
  }
  return y;
}
//--------------------------------------------------------

Arduino အတွက် program ကို အောက်က link မှာယူနိုင်ပါတယ်။
Implementation of adaptive filters on Arduino

WFLC ရဲ့ အားနည်းချက်က dominant frequency တခုမက ပိုနေတာမျိုး၊ noise များတာမျိုး ဆိုရင် ကောင်းကောင်း အလုပ်မလုပ်နိုင် တာကို တွေ့ရပါတယ်။ Arduino program မှာ input signal ကို generate လုပ်တဲ့ နေရာမှာ အမျိုးမျိုးပြောင်းပြီး စမ်းကြည့်နိုင်ပါတယ်။ ဒါ့ကြောင့် လက်တွေ့မှာ input signal ကို WFLC နဲ့ filter မလုပ်ခင် bandpass filter တို့ ဘာတို့နဲ့ အရင် pre-filter လုပ်လေ့ရှိပါတယ်။ WFLC နဲ့ fundamental frequency ကို estimate လုပ်ပြီး FLC နဲ့ တွဲသုံးတာလဲ တွေ့ရပါတယ် [Riviere, 1996]။

BMFLC

ကြိုတင်သတ်မှတ်ထားတဲ့ frequency band $ [\omega_1 - \omega_n] $ အတွက် sine နဲ့ cosine components တွေပေါင်းထားတဲ့ bandlimited multiple-Fourier linear combiner (BMFLC) ကိုအောက်ပါအတိုင်း ဖော်ပြနိုင်ပါတယ် [Veluvolu, 2008] [Veluvolu, 2010] ။ $$ y_k=\sum^n_{r=1}a_{rk}\sin(\omega_r k)+b_{rk}\cos(\omega_r k) $$ အဲဒီမှာ k က sampling instant ကို ကိုယ်စားပြုပြီး၊ $y_k$ က အဲဒီ အချိန်မှာ ရှိတဲ့ estimated signal ဖြစ်ပါတယ်။ $a_{rk}$ နဲ့ $b_{rk}$ က သက်ဆိုင်ရာ frequency $\omega_r$ အတွက် adaptive weights တွေ ဖြစ်ပါတယ်။

Figure. BMFLC architecture.

$x_k$ က reference input vector၊ $w_k$ က weight vector ဆိုရင် estimated signal $y_k$ ကို အောက်ပါအတိုင်း ဖော်ပြနိုင်ပါတယ်။ $$ \begin{equation} \mathbf{x}_k=\begin{bmatrix} \begin{bmatrix} \sin(\omega_1 k) & \sin(\omega_2 k) & \dots & \sin(\omega_n k) \end{bmatrix}^T\\ \begin{bmatrix} \cos(\omega_1 k) & \cos(\omega_2 k) & \dots & \cos(\omega_n k) \end{bmatrix}^T\\ \end{bmatrix} \end{equation} $$ $$ \begin{equation} \mathbf{w}_k=\begin{bmatrix} \begin{bmatrix} a_{1k} & a_{2k} & \dots & a_{nk} \end{bmatrix}^T\\ \begin{bmatrix} b_{1k} & b_{2k} & \dots & b_{nk} \end{bmatrix}^T\\ \end{bmatrix} \end{equation} $$ $$ \begin{equation} y_k=\mathbf{w}_k^T \mathbf{x}_k \end{equation} $$

BMFLC ရဲ့ weight တွေကို တော့ အောက်ပါအတိုင်း update လုပ်ပါတယ်။ $$ \begin{equation} \epsilon_k=s_k-y_k \end{equation} $$ $$ \begin{equation} \mathbf{w}_{k+1}=\mathbf{w}_k +2 \mu \mathbf{x}_k \epsilon_k \end{equation} $$

အဲဒီမှာ $x_k$ က reference input vector၊ $s_k$ က reference signal၊ $\epsilon_k$ က error term ဖြစ်ပြီး $\mu$ ကတော့ adaptive gain parameter ဖြစ်ပါတယ်။

BMFLC filter အတွက် C code တွေကို အောက်မှာ ပြထားပါတယ်။

// BMFLC filter
// An implementation of an adaptive filter called 
// Band-limited Multiple Fourier Linear Combiner
// Author: Yan Naing Aye
// http://coolemerald.blogspot.sg/
// Date: 2016 April 15
//--------------------------------------------------------
//Constants
//#define PI 3.141592654   // π (predefined)
#define N 4         // number of dominant frequency components
#define F0 1      // starting frequency
#define dF 1      // ΔF: frequency step (spacing between the dominant components)
#define MU 0.01    // μ: adaptive gain parameter
//i.e. 1 Hz to 4 Hz band
//--------------------------------------------------------
//External variables
float v[N];//array of angular frequencies
float x[2][N];//reference input vector, 1st row for sin and 2nd row for cos
float w[2][N];//weight vector, 1st row for sin and 2nd row for cos
//--------------------------------------------------------
//initialize BMFLC filter
//Get angular velocities and initialize weights
void InitBMFLC()
{
  int i;
  for (i=0;i<N; i++) {v[i]=2*PI*(F0+dF*i);} //assign a band of frequencies
  for (i=0;i<N; i++) {w[0][i]=0; w[1][i]=0;}//init weights
}
//--------------------------------------------------------
//BMFLC filter
//input (k: time instant, s: reference signal)
//output (y: estimated signal)
float BMFLC(float k,float s)
{
  int i; float err,y;
  //-----------------------------------------------------------------
  //find reference input vector
  for(i=0;i<N; i++){ 
    x[0][i]=sin(v[i]*k);
    x[1][i]=cos(v[i]*k);
  }
  //-----------------------------------------------------------------
  //find estimated signal, y
  for(i=0,y=0;i<N;i++){y+=w[0][i]*x[0][i]+ w[1][i]*x[1][i];}
  //-----------------------------------------------------------------
  //adapt the weights   
  for(i=0,err=s-y;i<N;i++){
    w[0][i]+=2*MU*x[0][i]*err;
    w[1][i]+=2*MU*x[1][i]*err;
  }
  return y;
}
//--------------------------------------------------------

Arduino အတွက် program ကို အောက်က link မှာယူနိုင်ပါတယ်။
Implementation of adaptive filters on Arduino

Compensated BMFLC

BMFLC နဲ့ estimate လုပ်လို့ရတဲ့ signal components တွေရဲ့ frequency က အသေဖြစ်တဲ့ အတွက် acceleration ကနေ displacement ရအောင် integrate လုပ်မယ် ဆိုရင် analytically အလွယ်တကူ လုပ်လို့ရပါတယ်။ ဥပမာ ပြောရရင် amplitude A ရှိတဲ့ sinusoidal signal တခုကို double integration လုပ်လိုက်ရင် amplitude $ \frac{A}{\omega^2} $ ဖြစ်သွားတဲ့မူရင်း sinusoidal ကို out of phase ရလာပါမယ်။ ဒါကြောင့် inertial measurement system တွေအတွက် အထူးသင့်တော်ပါတယ်။
ဒါပေမယ့် accelerometer အစရှိတဲ့ sensor တွေမှာ intrinsic ပါနေတဲ့ low pass filter ၊ system တွေရဲ့ pre-filtering စတာ တွေကြောင့် phase difference ရှိတာ၊ amplitude က attenuate ဖြစ်တာ အစရှိတဲ့ ပြဿနာတွေ ရှိနေပါသေးတယ်။ အဲဒါတွေကို ဖြေရှင်းဖို့ အတွက် ကိုဝင်းထွန်းလတ် proposed လုပ်ခဲ့တဲ့ Compensated BMFLC filter အကြောင်း ဆက်ပြောချင်ပါတယ် [Latt, 2011]။

Figure . The block diagram of the compensation using the modified reference input in BMFLC. (Credit: Image from [Latt, 2011])

BMFLC ရဲ့ frequency component တွေက အသေဖြစ်တဲ့ အတွက်၊ frequency component တခုချင်းစီ အတွက် filter အခု အရေအတွက် $F$ ကြောင့်ပြောင်းသွားတဲ့ phase shift စုစုပေါင်း $ \sum_{l=1}^F (\phi_r^l) $ ရယ်၊ magnitude ပြောင်းလဲမှု စုစုပေါင်း $ \Pi_{l=1}^F (g_r^l) $ တွေကို ကြိုတွက်ထားလို့ ရပါတယ်။ ဒါကြောင့် BMFLC ရဲ့ weight တွေကို adapt လုပ်တဲ့ အခါ၊ အရင် reference input vector, $ \mathbf{x}_k $, နဲ့ပဲ adapt လုပ်ပြီး compensated estimated signal ထုတ်ဖို့ အတွက်ကျ ခုနက ကြိုတွက်ထားတဲ့ phase စုစုပေါင်းကို ပြင်ထားတဲ့ phase compensated reference input vector, $ \mathbf{x}_k^c $, ရယ်၊ magnitude တွေကိုပြင်ထားတဲ့ magnitude compensated weight vector,$ \mathbf{w}_k^c $ , ကိုသုံးပါမယ်။

$$ \begin{equation} \mathbf{x}_k=\begin{bmatrix} \begin{bmatrix} \sin(\omega_1 k) & \sin(\omega_2 k) & \dots & \sin(\omega_n k) \end{bmatrix}^T\\ \begin{bmatrix} \cos(\omega_1 k) & \cos(\omega_2 k) & \dots & \cos(\omega_n k) \end{bmatrix}^T\\ \end{bmatrix} \end{equation} $$ $$ \begin{equation} \mathbf{x}_k^c=\begin{bmatrix} \begin{bmatrix} \sin(\omega_1 k-\sum^F_{l=1}\phi_1^l) & \sin(\omega_2 k-\sum_{l=1}^F\phi_2^l) & \dots & \sin(\omega_n k-\sum_{l=1}^F\phi_n^l) \end{bmatrix}^T\\ \begin{bmatrix} \cos(\omega_1 k-\sum_{l=1}^F\phi_1^l) & \cos(\omega_2 k-\sum_{l=1}^F\phi_2^l) & \dots & \cos(\omega_n k-\sum_{l=1}^F\phi_n^l) \end{bmatrix}^T\\ \end{bmatrix} \end{equation} $$

$$ \begin{equation} \mathbf{w}_k=\begin{bmatrix} \begin{bmatrix} a_{1k} & a_{2k} & \dots & a_{nk} \end{bmatrix}^T\\ \begin{bmatrix} b_{1k} & b_{2k} & \dots & b_{nk} \end{bmatrix}^T\\ \end{bmatrix} \end{equation} $$ $$ \begin{equation} \mathbf{w}_k^c=\begin{bmatrix} \begin{bmatrix} a_{1k} . \Pi_{l=1}^F \frac{1}{g_1^l} & a_{2k} . \Pi_{l=1}^F \frac{1}{g_2^l} & \dots & a_{nk} . \Pi_{l=1}^F \frac{1}{g_n^l} \end{bmatrix}^T\\ \begin{bmatrix} b_{1k} . \Pi_{l=1}^F \frac{1}{g_1^l} & b_{2k} . \Pi_{l=1}^F \frac{1}{g_2^l} & \dots & b_{nk} . \Pi_{l=1}^F \frac{1}{g_n^l} \end{bmatrix}^T\\ \end{bmatrix} \end{equation} $$

ဒါဆိုရင် estimated output $y_k $ နဲ့ compensated estimated output $y_k^c $ တို့ကို အောက်ပါအတိုင်းရနိုင်ပါတယ်။ $$ \begin{equation} y_k=\mathbf{w}_k \cdot \mathbf{x}_k \end{equation} $$ $$ \begin{equation} y_k^c=\mathbf{w}^c_k \cdot \mathbf{x}_k \end{equation} $$

Compensated BMFLC filter အတွက် C code တွေကို အောက်မှာ ပြထားပါတယ်။

// Compensated BMFLC filter
// An implementation of an adaptive filter called 
// Compensated Band-limited Multiple Fourier Linear Combiner
// Author: Yan Naing Aye
// http://coolemerald.blogspot.sg/
// Date: 2016 April 16

// Credit: Based on code and papers of Ko Win Tun Latt
// Reference: 
// W. T. Latt, K. C. Veluvolu, and W. T. Ang, 
// "Drift-free position estimation of periodic or quasi-periodic motion using inertial sensors," 
// Sensors, vol. 11, no. 6, pp. 5931-5951, 2011.
//--------------------------------------------------------
//Constants
//#define PI 3.141592654   // π (predefined)
#define N 3         // number of dominant frequency components
#define F0 2      // starting frequency
#define dF 1      // ΔF: frequency step (spacing between the dominant components)
#define MU 0.01    // μ: adaptive gain parameter
//i.e. 2 Hz to 4 Hz band
//--------------------------------------------------------
//External variables
float v[N];//array of angular frequencies
float x[2][N];//reference input vector, 1st row for sin and 2nd row for cos
float w[2][N];//weight vector, 1st row for sin and 2nd row for cos
float p[N];//array of phase shift for compensation (*to add)
float g[N];//array of gain for compensation (*to multiply)
//--------------------------------------------------------
//initialize Compensated BMFLC filter
//Get angular velocities and initialize weights
void InitCBMFLC()
{
  int i;
  float v1=2*PI*1;//cutoff frequency for high pass filter
  float v2=2*PI*10;//cutoff frequency for low pass filter
  for (i=0;i<N; i++) {
    v[i]=2*PI*(F0+dF*i); //assign a band of frequencies
    w[0][i]=0; w[1][i]=0;//init weights
    g[i]=sqrt(1+(v[i]/v2)*(v[i]/v2))*sqrt(1+(v1/v[i])*(v1/v[i]));
    p[i]=atan(v[i]/v2)-atan(v1/v[i]);
  }
}
//--------------------------------------------------------
//Compensated BMFLC filter
//input (k: time instant, s: reference signal)
//output (yc: compensated estimated signal)
float CBMFLC(float k,float s)
{
  int i; float err,y,yc;
  //-----------------------------------------------------------------
  //find reference input vector
  for(i=0;i<N; i++){ 
    x[0][i]=sin(v[i]*k);
    x[1][i]=cos(v[i]*k);
  }
  //-----------------------------------------------------------------
  //find estimated signal, y
  for(i=0,y=0;i<N;i++){y+=w[0][i]*x[0][i]+ w[1][i]*x[1][i];}
  //-----------------------------------------------------------------
  //adapt the weights   
  for(i=0,err=s-y;i<N;i++){
    w[0][i]+=2*MU*x[0][i]*err;
    w[1][i]+=2*MU*x[1][i]*err;
  }
  //-----------------------------------------------------------------
  //find uncompensated estimated displacement, y
  //for(i=0,y=0;i<N;i++){y-=(w[0][i]*x[0][i]+ w[1][i]*x[1][i])/(v[i]*v[i]);}
  //-----------------------------------------------------------------
  //find phase compensated reference input vector
  for(i=0;i<N; i++){ 
    x[0][i]=sin(v[i]*k+p[i]);
    x[1][i]=cos(v[i]*k+p[i]);
  }
  //-----------------------------------------------------------------
  //find compensated estimated integrated displacemet, yc
  for(i=0,yc=0;i<N;i++){yc-=(g[i]/(v[i]*v[i]))*(w[0][i]*x[0][i]+w[1][i]*x[1][i]);}
  //-----------------------------------------------------------------
  return yc;
}
//--------------------------------------------------------

နမူနာ အနေနဲ့ amplitude $A$ က 100 $\mu$m နဲ့ 50 $\mu$m ၊ frequency $f$ က 2 Hz နဲ့ 4 Hz အကြား 15 seconds ခြား တခါပြောင်းနေတဲ့ input ရဲ့ displacement $d$ ကို accelerometer သုံးပြီး track လုပ်မယ့် simulation ကို Arduino program လေးရေးကြည့်ပါမယ်။ Reference displacement ရဲ့ time instant $k$ မှာရှိမယ့်တန်ဖိုးကို အောက်က အတိုင်း သတ်မှတ်ပါမယ်။ $$ \begin{equation} d_k=A \sin(2\pi f k) \end{equation} $$ အဲဒါဆိုရင် သူ့ရဲ့ acceleration $a$ က အောက်ပါအတိုင်း ဖြစ်သွားပါမယ်။ $$ \begin{equation} a_k=-A.(2 \pi f)^2. \sin(2\pi f k) \end{equation} $$ Accelerometer မှာ သုံးထားတဲ့ low pass filter က cutoff frequency $ f_2$ 10 Hz သုံးထားပြီး၊ gravity component, low frequency drift အစရှိတာတွေကို prefilter လုပ်ဖို့ သုံးထားတဲ့ system ရဲ့ highpass filter က cutoff frequency $ f_1 $ 1 Hz ရှိတယ် လို့ ထားပါမယ်။ နမူနာ program မှာ reference displacement ရော၊ acceleraion ရောကို generate လုပ်ပါမယ်။ ပြီးရင် acceleration signal ကို low pass နဲ့ high pass filter တွေထဲ ဖြတ်ပါမယ်။ ရလာတဲ့ signal ကို Compensated BMFLC filter ထဲဖြတ်ပြီး displacement ရအောင် analytically integrated လုပ်ထားတဲ့ compensated estimated displacement $ y_k^c$ ကို ရှာပါမယ်။ ပြီးတဲ့ အခါ ရလာတဲ့ $ y_k^c$ ကို မူရင်း $d_k$ နဲ့ နှိုင်းယှဉ်ပြီး serial plotter မှာပြပါမယ်။ အဲဒီ ကုဒ်မှာပဲ $ y_k^c$ အစား၊ $ y_k$ ကို integrated လုပ်ထားတဲ့ displacement output ကိုထုတ်ကြည့်လိုက်ရင် ဘယ်လောက်ထိပို မှန်ကန်သွားတယ် ဆိုတာ တွေ့နိုင်ပါတယ်။
Arduino အတွက် program ကို အောက်က link မှာယူနိုင်ပါတယ်။
Implementation of adaptive filters on Arduino

Practical Application

Accelerometer တွေသုံးပြီး လက်ရဲ့ တုန်ခါမှု (physiological hand tremor) ကို ဖျောက်ပေးတဲ့ လက်ကိုင် ကိရိယာ တခု မှာ သုံးခဲ့ပါတယ်။ လက်ရဲ့ တုန်ခါမှုက 6 Hz ကနေ 12 Hz ကြားမှာ ရှိတာကြောင့် Compensated BMFLC သုံးလို့ အဆင်ပြေပါတယ်။ အဲဒီနည်းကို သုံးတာ တုန်ခါမှု 60% လောက် လျှော့ချပေးနိုင်ပြီး RMS error က 10 $\mu$m ထက်နည်းသွားတာကို တွေ့ရပါတယ် [Aye, 2013]။

Accelerometer တွေရဲ့ အားနည်းချက်က low frequency drift တွေကို sensing မလုပ်နိုင်ပါဘူး။ Adaptive filter တွေရဲ့ အားနည်းချက်က အဝင် signal က ပုံမှန် pattern ရှိနေမှ adapt လုပ်နိုင်တာပါ။ ရုတ်တရုက် ပြောင်းလဲမှု ရှိခဲ့ရင်လည်း ပြန် adapt လုပ်ဖို့ အချိန် ပြန်လိုပါတယ်။ ဒါကြောင့် အဲဒီလက်ကိုင် ကိရိယာ မှာ accelerometer ကို complement လုပ်ပေးဖို့ vision system ကိုပါထည့်သုံးလိုက်ပြီး sensor fusion technique အသစ်ကို လည်း သုံးခဲ့ပါတယ် [Aye, 2013 B]။

References

[Aye, 2013] Y. N. Aye, S. Zhao, and W. T. Ang, "An Enhanced Intelligent Handheld Instrument with Visual Servo Control for 2-DOF Hand Motion Error Compensation," International Journal of Advanced Robotic Systems, vol. 10, 2013.

[Aye, 2013 B] Y. N. Aye, S. Zhao, C. Y. Shee, and W. T. Ang, "Fusion of inertial measurements and vision feedback for microsurgery," in Intelligent Autonomous Systems 12 (S. Lee, H. Cho, K.-J. Yoon, and J. Lee, eds.),vol. 194 of Advances in Intelligent Systems and Computing, pp. 27-35, Springer Berlin Heidelberg, 2013.

[Latt, 2011] W. T. Latt, K. C. Veluvolu, and W. T. Ang, "Drift-free position estimation of periodic or quasi-periodic motion using inertial sensors," Sensors, vol. 11, no. 6, pp. 5931-5951, 2011.

[Riviere, 1996] C. Riviere and N. Thakor, "Modeling and canceling tremor in humanmachine interfaces," Engineering in Medicine and Biology Magazine, IEEE, vol. 15, pp. 29-36, May 1996.

[Riviere, 1998] C. Riviere, R. Rader, and N. Thakor, "Adaptive cancelling of physiological tremor for improved precision in microsurgery," Biomedical Engineering, IEEE Transactions on, vol. 45, pp. 839-846, July 1998.

[Vaz, 1994] C. Vaz, X. Kong, and N. Thakor, "An adaptive estimation of periodic signals using a fourier linear combiner," Signal Processing, IEEE Transactions on, vol. 42, pp. 1 -10, jan 1994.

[Veluvolu, 2010] K. C. Veluvolu and W. T. Ang, "Estimation and fi ltering of physiological tremor for real-time compensation in surgical robotics applications," The International Journal of Medical Robotics and Computer Assisted Surgery, vol. 6, no. 3, pp. 334-342, 2010.

[Veluvolu, 2008] K. Veluvolu, U.-X. Tan, W. Latt, C. Shee, and W. Ang, "Adaptive filtering of physiological tremor for real-time compensation," in Robotics REFERENCES 162 and Biomimetics, 2008. ROBIO 2008. IEEE International Conference on, pp. 524-529, Feb 2009.

Cool Emerald

Pages

Search This Blog

Sunday, April 17, 2016

Adaptive Filter: BMFLC

Setup

FLC

WFLC

BMFLC

Compensated BMFLC

Practical Application

No comments:

Post a Comment