GRFKalman

#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=120;
extern double DevLevel1 = 1;
extern double DevLevel2 = 2;
extern double Suppression_dB = 0;
extern double PredictBars1 = 0;
extern double PredictBars2 = 0;
extern bool PrintCurrentState = 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 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,LookAhead);
      SetIndexDrawBegin(1,LookAhead);
      SetIndexLabel(1,"Kalman +" + DoubleToStr(DevLevel1,1)  + " STD");
      SetIndexStyle(2,DRAW_LINE);
      SetIndexBuffer(2,KalmanBufferNeg1);
      SetIndexShift(2,LookAhead);
      SetIndexDrawBegin(2,LookAhead+1);
      SetIndexLabel(2,"Kalman -" + DoubleToStr(DevLevel1,1) + " STD");
    }
   if(MathAbs(DevLevel2) > 0){
      SetIndexStyle(3,DRAW_LINE);
      SetIndexBuffer(3,KalmanBufferPlus2);
      SetIndexShift(3,LookAhead);
      SetIndexDrawBegin(3,LookAhead);
      SetIndexLabel(3,"Kalman +" + DoubleToStr(DevLevel2,1) + " STD");
      SetIndexStyle(4,DRAW_LINE);
      SetIndexBuffer(4,KalmanBufferNeg2);
      SetIndexShift(4,LookAhead);
      SetIndexDrawBegin(4,LookAhead);
      SetIndexLabel(4,"Kalman -" + DoubleToStr(DevLevel2,1) + " STD");
      
      SetIndexStyle(5,DRAW_LINE);
      SetIndexBuffer(5,KalmanBufferPlus3);
      SetIndexShift(5,LookAhead);
      SetIndexDrawBegin(5,LookAhead);
      SetIndexLabel(5,"Kalman High +" + DoubleToStr(DevLevel2,1) + " STD");
      SetIndexStyle(6,DRAW_LINE);
      SetIndexBuffer(6,KalmanBufferNeg3);
      SetIndexShift(6,LookAhead);
      SetIndexDrawBegin(6,LookAhead);
      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;
   //----
   if(Bars<5) return(0);
   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);
      xkk[0][0] = get_avg(Bars-1);
      xkk[1][0] = get_avg(Bars-1)-get_avg(Bars-15);
      //MatrixPrint(xkk);
      MatrixEye(Pkk);
      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);
      working_samples = Samples;
      acc_sigma2 *= acc_sigma2;
      initiated = true;
      MatrixScalarMul(acc_sigma2,Pkk);
      //Print(1./MathSqrt(acc_sigma2));
      //Print(" ");
      
   }

   
   for(i=Bars-counted_bars; i>0;i--){
  
      
      //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):", Suppression_dB,"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) ",(xpt[0][0])*Point
               ,",",g1*Point);
         }
         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) ",(xpt[0][0])*Point
               ,",",g1*Point);
         }         
      }else g= -1;
     
     
      //update cycle
      //update
      //innovation
     
      MatrixZero(yk);
      zk[0][0] = get_avg(i);
      zk[1][0]= get_avg(i)-get_avg(i+1);
      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);
      } 
      Rk[0][0] = z_sigma2;
      
      //acc += zk[1][0];
      //acc=MathAbs(acc);
      //acc_avg= (acc+(Samples-1)*acc_avg)/Samples;
      //      acc_sigma2 = (MathPow(acc-acc_avg,2)/Samples+(Samples-1)*acc_sigma2)/Samples;
      //Print(acc_sigma2);
      ArrayCopy(Qkk,GGT);
      MatrixScalarMul(acc_sigma2,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);
      
    
   }
 
   //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);
}        
//+++++++-----------------------------------------------------+