GRFKalmanAGJW

#property copyright "Copyleft © 2007, GammaRat Forex"
#property link      "http://www.gammarat.com/Forex/"

   

//#include <GRFMatrixMath.mqh>
#property indicator_chart_window
#property indicator_buffers 7
#property indicator_color1 Yellow
#property indicator_color2 Yellow
#property indicator_color3 Yellow
#property indicator_color4 Red
#property indicator_color5 Red
#property indicator_color6 Red
#property indicator_color7 Red
#property indicator_style1 0
#property indicator_style2 2
#property indicator_style3 2
#property indicator_style4 2
#property indicator_style5 2
#property indicator_style6 0
#property indicator_style7 0


//---- input parameters
extern double Samples=60;
extern double DevLevel1 = 1;
extern double DevLevel2 = 2;
extern double Suppression_dB = 0;
extern double PredictBars1 = 4;
extern double PredictBars2 = 8;
extern bool PrintCurrentState = true;
extern bool AutoGainOn = true;


//---- buffers
double KalmanBuffer[];
double KalmanBufferPlus1[];
double KalmanBufferNeg1[];
double KalmanBufferPlus2[];
double KalmanBufferNeg2[];
double KalmanBufferPlus3[];
double KalmanBufferNeg3[];
double xkk[2][1],xkk1[2][1],xk1k1[2][1],yk[1][1],zk[1][1];
double Pkk[2][2],Pkk1[2][2],Pk1k1[2][2],Pkk_inv[2][2];
double Qkk[2][2],Hk[1][2],Hk_t[2][1],Sk[1][1],Sk_inv[1][1],Rk[1][1],Kk[2][1];
double Fk[2][2],Fk_t[2][2],GGT[2][2];
double Eye[2][2];
double temp22[2][2],temp21[2][1],temp12[1][2],temp11[1][1];
double xp[1][1],xpt[1][1],xpt_t[1][1];
double Phi1[1][2],Phi2[1][2];
double xdel[1][1],xdel_t[1][1];
double LookAhead=0;
static bool initiated=false;
static double c0_above=0, c0_below = 0;
static double c1_above=0, c1_below = 0;
static int last_time=-1;
static int tick_count=0;
//+------------------------------------------------------------------+
//| Custom indicator initialization function                         |
//+------------------------------------------------------------------+
int init()
  {
//---- indicators
   if(LookAhead <0)LookAhead=0;
   SetIndexStyle(0,DRAW_LINE);
   SetIndexBuffer(0,KalmanBuffer);
   SetIndexShift(0,LookAhead);
   SetIndexDrawBegin(0,LookAhead);
   SetIndexLabel(0,"Kalman Trend");
   if(MathAbs(DevLevel1) > 0) {
      SetIndexStyle(1,DRAW_LINE);
      SetIndexBuffer(1,KalmanBufferPlus1);
      SetIndexShift(1,0);
      SetIndexDrawBegin(1,1);
      SetIndexLabel(1,"Kalman +" + DoubleToStr(DevLevel1,1)  + " STD");
      SetIndexStyle(2,DRAW_LINE);
      SetIndexBuffer(2,KalmanBufferNeg1);
      SetIndexShift(2,0);
      SetIndexDrawBegin(2,1);
      SetIndexLabel(2,"Kalman -" + DoubleToStr(DevLevel1,1) + " STD");
    }
   if(MathAbs(DevLevel2) > 0){
      SetIndexStyle(3,DRAW_LINE);
      SetIndexBuffer(3,KalmanBufferPlus2);
      SetIndexShift(3,0);
      SetIndexDrawBegin(3,1);
      SetIndexLabel(3,"Kalman +" + DoubleToStr(DevLevel2,1) + " STD");
      SetIndexStyle(4,DRAW_LINE);
      SetIndexBuffer(4,KalmanBufferNeg2);
      SetIndexShift(4,0);
      SetIndexDrawBegin(4,1);
      SetIndexLabel(4,"Kalman -" + DoubleToStr(DevLevel2,1) + " STD");
      
      SetIndexStyle(5,DRAW_LINE);
      SetIndexBuffer(5,KalmanBufferPlus3);
      SetIndexShift(5,0);
      SetIndexDrawBegin(5,1);
      SetIndexLabel(5,"Kalman High +" + DoubleToStr(DevLevel2,1) + " STD");
      SetIndexStyle(6,DRAW_LINE);
      SetIndexBuffer(6,KalmanBufferNeg3);
      SetIndexShift(6,0);
      SetIndexDrawBegin(6,1);
      SetIndexLabel(6,"Kalman Low -" + DoubleToStr(DevLevel2,1) + " STD");
   }
//----
   return(0);
  }
//| Point and figure                                |
//+------------------------------------------------------------------+
int start()
{
   compute();
   return(0);
}
int compute(){
   int    i,j,counted_bars=IndicatorCounted();
   static bool initiated=false;
   double g,g1;
   double c0;
   static double c0_above=0, c0_below = 0;
   static double c1_above=0, c1_below = 0;
   static double acc_sigma2;
   static double z_sigma2;
   static double normalization_time;
   static double working_samples;
   double z,diff; 
   double ts[3][1];
   static double dev_level1,dev_level2; 
   double temp11a[1][1],temp21a[2][1];
   double temp22a[2][2]; 
   static double ZGain = 0;
   static double AGain=1;
   static double tgain=1;
   static double time_last;
   static double zk_saved[2][1];
   int trial;
   //----
   //give it some time to warm up
   if(Bars<Samples*2) 
      return(0);
   //rewrite the last bar
   if(Time[0] <= last_time) return(0);
   tick_count--;
   if(tick_count>0)return(0);
   tick_count=2;
   last_time=Time[0];
   
   if(!initiated){
      
      Phi1[0][0] = 1;
      Phi1[0][1] = PredictBars1;
      Phi2[0][0] = 1;
      Phi2[0][1] = PredictBars2;
            
      dev_level1 = DevLevel1*MathSqrt(2);
      dev_level2 = DevLevel2*MathSqrt(2);
      ArrayCopy(KalmanBuffer,High);
      xkk1[0][0] = get_avg(Bars-1);
      xkk1[1][0] = 0;
      //MatrixPrint(xkk);
      MatrixEye(Pkk1);
      Fk[0][0] = 1;
      Fk[0][1]=1;
      Fk[1][0]=0;
      Fk[1][1] =1;
      MatrixTranspose(Fk,Fk_t);
      Qkk[0][0] = 1;
      Qkk[0][1] = 0;
      Qkk[1][0]=0;
      Qkk[1][1] = 1;
      GGT[0][0]=.25;
      GGT[1][0]=.5;
      GGT[0][1]=.5;
      GGT[1][1]=1;
      Hk[0][0] = 1;
      Hk[0][1] = 0;
      MatrixTranspose(Hk,Hk_t);
      
      Rk[0][0] = .1;
      MatrixEye(Eye);
      ZGain = 0;
      z_sigma2 = MathPow(10,-ZGain);
      acc_sigma2 = Point*MathPow(10,-Suppression_dB/20.);//*MathSqrt(0.0001/Point);
      MatrixScalarMul(acc_sigma2,Pkk1);
      working_samples = Samples;
      acc_sigma2 *= acc_sigma2;
       Rk[0][0] = z_sigma2;
      initiated = true;
      MatrixScalarMul(acc_sigma2,Pkk);
      //Print(1./MathSqrt(acc_sigma2));
      //Print(" ");
      AGain=1;
      zk_saved[0][0] = get_avg(Bars-1);
      zk_saved[1][0] = 0;
      
   }
 
   for(i=Bars-counted_bars; i> 0;i--){ //ticks are handled elsewhere
     
     //Start the update procedure
      MatrixZero(yk);
      zk[0][0] = get_avg(i);
      zk[1][0]= zk[0][0]-zk_saved[0][0];
      ArrayCopy(zk_saved,zk);
      diff = (zk[0][0]*Point-KalmanBuffer[i]);
      if(diff>=0) {
         diff = diff*diff;
         c0_above = (c0_above*(working_samples-1)+MathPow(get_avg(i)*Point-KalmanBuffer[i],2))/working_samples;
         c0_below = c0_below*(working_samples-1)/working_samples;
         c1_above = (c1_above*(working_samples-1)+MathPow(High[i]-KalmanBuffer[i],2))/working_samples;
         c1_below = c1_below*(working_samples-1)/working_samples;
      }else{
         diff = diff*diff;
         c0_below = (c0_below*(working_samples-1)+MathPow(get_avg(i)*Point-KalmanBuffer[i],2))/working_samples;
         c0_above = c0_above*(working_samples-1)/working_samples;
         c1_below = (c1_below*(working_samples-1)+MathPow(Low[i]-KalmanBuffer[i],2))/working_samples;
         c1_above = c1_above*(working_samples-1)/working_samples;
      }  
      if(MathAbs(DevLevel1)>0){
         KalmanBufferPlus1[i-1] = KalmanBuffer[i]+dev_level1*MathSqrt(c0_above);
         KalmanBufferNeg1[i-1] = KalmanBuffer[i]-dev_level1*MathSqrt(c0_below);
      }
      if(MathAbs(DevLevel2)>0){   
         KalmanBufferPlus2[i-1] = KalmanBuffer[i]+dev_level2*MathSqrt(c0_above);
         KalmanBufferNeg2[i-1] = KalmanBuffer[i]-dev_level2*MathSqrt(c0_below);
         KalmanBufferPlus3[i-1] = KalmanBuffer[i]+dev_level2*MathSqrt(c1_above);
         KalmanBufferNeg3[i-1] = KalmanBuffer[i]-dev_level2*MathSqrt(c1_below);
      } 
      //this is always left alone, for now
           
     
      ArrayCopy(Qkk,GGT);
      
      // set the auto gain
      if(AutoGainOn && MathSqrt((c0_above+c0_below)/2)> Point){   
         if(c0_below >0 || c0_above>0)
            tgain = MathAbs((c0_below)/(c0_below+c0_above));
         else tgain = 0.5;
         if(tgain< 0.2 || tgain > 0.8)
            AGain = AGain*MathPow(10,0.005);
         else if(tgain> 0.4 && tgain < 0.6)
            AGain=AGain*MathPow(10,-0.005);
      }else{
         AGain = 1;
      }   
      tgain = AGain*acc_sigma2;
      MatrixScalarMul(tgain,Qkk);
      MatrixMul(Hk,xkk1,temp11);
      //MatrixPrint(xkk1);
      MatrixAdd(zk,temp11,yk, -1); 
      MatrixMul(Hk,Pkk1,temp12);
      MatrixMul(temp12,Hk_t,temp11);
      MatrixAdd(temp11,Rk,Sk,1);
      MatrixInvert(Sk,Sk_inv);
      MatrixMul(Pkk1,Hk_t,temp21);
      MatrixMul(temp21,Sk_inv,Kk);
      MatrixMul(Kk,yk,temp21);
      MatrixAdd(temp21,xkk1,xkk,1);
      
      MatrixMul(Kk,Hk,temp22);
      MatrixAdd(Eye,temp22,temp22a,-1);
      MatrixMul(temp22a,Pkk1,Pkk);
      
      //predict cycle
          //make copies of the last iteration;
      ArrayCopy(Pk1k1,Pkk);
      ArrayCopy(xk1k1,xkk);
      //g = MathSqrt(MathPow(xkk[0][0]-get_avg(i),2))/Pkk[0][0]*Point;
      //g = xkk[1][0];
      //predict the state
      //Print("Doing Predict");
      MatrixMul(Fk,xk1k1,xkk1);
      //MatrixPrint(xkk1);
      MatrixMul(Fk,Pk1k1,temp22);
      MatrixMul(temp22,Fk_t,Pk1k1);
      MatrixAdd(Pk1k1,Qkk,Pkk1,1);
      //Print("Predict Ended");
      KalmanBuffer[i-1] = (xkk1[0][0]*Point);
 
      if(i==1){
         if(PrintCurrentState){
            Print("Suppression (dB):", -20*MathLog(MathSqrt(tgain)/Point)/MathLog(10.),"dB; Value:",xkk1[0][0]*Point
            ,"; Speed (pips/bar):",xkk1[1][0],"; dp/p:"
            , xkk1[1][0]/xkk1[0][0]*100.*240.*(24.*60.)/Period());
         }
         if(PredictBars1 > 0){
            MatrixMul(Phi1,xkk,xpt);
            xp[0][0] = xkk[0][0];
         
            MatrixTranspose(Phi1,temp21);
            MatrixMul(Pkk,temp21,temp21a);
            MatrixMul(Phi1,temp21a,temp11a);
            MatrixInvert(temp11a,temp11);
            MatrixAdd(xp,xpt,xdel,-1);
            MatrixTranspose(xdel,xdel_t);
            MatrixMul(temp11,xdel,temp11a);
            MatrixMul(xdel_t,temp11a,temp11);
            g1=MathSqrt(temp11[0][0]);
            Print(Phi1[0][1], " bar bounds ",(xpt[0][0]-g1)*Point
               ,",",(xpt[0][0]+g1)*Point);
            Print(Phi1[0][1], " bar center, r(t) (in pips) ",(xpt[0][0])*Point
               ,",",g1);
         }
         if(PredictBars2 > 0){
            MatrixMul(Phi2,xkk,xpt);
            xp[0][0] = xkk[0][0];
            MatrixTranspose(Phi2,temp21);
            MatrixMul(Pkk,temp21,temp21a);
            MatrixMul(Phi2,temp21a,temp11a);
            MatrixInvert(temp11a,temp11);
            MatrixAdd(xp,xpt,xdel,-1);
            MatrixTranspose(xdel,xdel_t);
            MatrixMul(temp11,xdel,temp11a);
            MatrixMul(xdel_t,temp11a,temp11);
            g1=MathSqrt(temp11[0][0]);
            Print(Phi2[0][1], " bar bounds ",(xpt[0][0]-g1)*Point
               ,",",(xpt[0][0]+g1)*Point);
            Print(Phi2[0][1], " bar center, r(t) (in pips) ",(xpt[0][0])*Point
               ,",",g1);
         }         
      }else g= -1;
      
   
   }
   //Print("24 hours:", xpt[0][0]*Point," ,Gate ",g);
   //Print ("Gate 2 ", MathSqrt(xdel[0][0]*xdel[0][0]*g)*Point," g1 ",g1);

}
 
 double get_avg(int k){
   return(MathPow((High[k]*Low[k]*Close[k]*Close[k]),1/4.)/Point);
}        
//+++++++-----------------------------------------------------+
double determinant_fixed(double a[][], int k){
   double z;
    switch(k){
      case 1: return(a[0][0]);
         break;
      case 2: return( a[0][0]*a[1][1]-a[1][0]*a[0][1]);
         break;
      case 3:
         z = a[0][0]*a[1][1]*a[2][2] + a[0][1]*a[1][2]*a[2][0] + 
             a[0][2]*a[1][0]*a[2][1] -
            (a[2][0]*a[1][1]*a[0][2] + a[1][0]*a[0][1]*a[2][2] + 
             a[0][0]*a[2][1]*a[1][2]);
         return(z);
         break;
      default:      
          Print("array_range too large for determinant calculation");
         return(0);
   }  
}
double determinant(double a[][]){
   double z;
   if(ArrayRange(a,0) != ArrayRange(a,1)){
      Print("Non-square array entered into determinant");
      return(0);
    }
    switch(ArrayRange(a,0)){
      case 1: return(a[0][0]);
         break;
      case 2: return( a[0][0]*a[1][1]-a[1][0]*a[0][1]);
         break;
      case 3:
         z = a[0][0]*a[1][1]*a[2][2] + a[0][1]*a[1][2]*a[2][0] + 
             a[0][2]*a[1][0]*a[2][1] -
            (a[2][0]*a[1][1]*a[0][2] + a[1][0]*a[0][1]*a[2][2] + 
             a[0][0]*a[2][1]*a[1][2]);
         return(z);
         break;
      default:      
          Print("array_range too large for determinant calculation");
         return(0);
   }  
}
  
int MatrixAdd(double a[][], double b[][],double & c[][],double w=1){
   int i,j;
   for(i=0;i<ArrayRange(c,0);i++){
      for(j=0;j<ArrayRange(c,1);j++){
         c[i][j] = a[i][j]+w*b[i][j];
      }
   }
    return(0);   
}
int MatrixMul(double a[][], double b[][],double & c[][]){
   int i,j, k;
   if(ArrayRange(a,1) != ArrayRange(b,0)){
      Print("array Range Mismatch in Matrix Mul");
      return(-1);
   }
   for(i=0;i<ArrayRange(a,0);i++){
      for(j=0;j<ArrayRange(b,1);j++){
         c[i][j] = 0;
         for(k=0;k<ArrayRange(a,1);k++){
           c[i][j] += (a[i][k]*b[k][j]);
         }  
      }
   }
  return(0);   
}
int MatrixScalarMul(double b,double & a[][]){
   int i,j;
   for(i =0;i<ArrayRange(a,0);i++){
      for(j=0;j<ArrayRange(a,1);j++){
         a[i][j] *= b;
      }
   }
}         
int MatrixInvert(double a[][],double & b[][]){
   double d;
   double temp1[4][4],temp2[4][4];
   int i,j,i1,j1,k;
   
   if(ArrayRange(a,0) != ArrayRange(a,1)){
      Print("Matrix Inverse attempted on non square matrix. Not implemented yet");
      return(-1);
   }
   if(ArrayRange(a,1) > 3){
      Print("Bugger off, not implemented yet");
      return(-1);
   }
   d = determinant(a);
   if(MathAbs(d)<0.000001){
    Print("unstable matrix");
    d = 1;
   }
   k = ArrayRange(a,0);
   if(k==0){
      b[0][0]=1./a[0][0];
      return(0);
   }
   d = determinant(a);
   for(i1=0;i1<k;i1++){
      for(j1=0;j1<k;j1++){
         temp1[i1][j1] = a[i1][j1];
      }
   }   
   for(i=0;i<k;i++){
      for(j=0;j<k;j++){
         //copy the arry. There must be a better way!!
         ArrayCopy(temp2,temp1);
         for(j1=0;j1<k;j1++){
            if(j1==j){
               temp2[i][j1] = 1;
            }else{
               temp2[i][j1] = 0;
            }
         }   
         b[i][j]=determinant_fixed(temp2,k)/d;
      }
   }
}   
int MatrixTranspose(double a[][],double & b[][]){
   int i,j;
   for(i=0;i<ArrayRange(a,0);i++){
      for(j=0;j<ArrayRange(a,1);j++){
         b[j][i] = a[i][j];
      }
   }
   return(0);
 }
int MatrixZero(double & a[][]){
   int i,j; 
   for(i= 0;i<ArrayRange(a,0);i++){
      for(j=0;j<ArrayRange(a,1);j++){
         a[i][j]=0;
      }
   }
   return(0);
} 
int MatrixEye(double & a[][]){
   int i,j; 
   for(i= 0;i<ArrayRange(a,0);i++){
      for(j=0;j<ArrayRange(a,1);j++){
         a[i][j]=0;
         if(i==j)a[i][j]=1;
      }
   }
   return(0);
} 
int MatrixPrint(double a[][]){
   int i,j; 
   for(i= 0;i<ArrayRange(a,0);i++){
      for(j=0;j<ArrayRange(a,1);j++){
      Print(i," ,",j," ,",a[i][j]);
      }
   }
   return(0);
}                                     
//+------------------------------------------------------------------+