title: Implementation of Matlab's filter function with initial conditions in C++
body: <p>I'm trying to implement <a href="https://de.mathworks.com/help/matlab/ref/filter.html" rel="nofollow noreferrer">MATLAB's filter function</a> with initial conditions in C++. It seems that a lot of people are having trouble to achieve the exact same results as in MATLAB, but I could not find a solution. Furthermore, most people ignore the initial conditions, which are fundamental for me.</p>
  
  <p>This is the code:</p>
  
  <pre><code>typedef std::vector&lt;int&gt; vectori;
  typedef std::vector&lt;double&gt; vectord;
  
  void filter(vectord B, vectord A, const vectord &amp;X, vectord &amp;Y, vectord &amp;Zi) {
  
  //check coeffecients:
  if (A.empty())
      qCritical() &lt;&lt; "The feedback filter coefficients are empty.";
  if (std::all_of(A.begin(), A.end(), [](double coef){ return coef == 0; }))
      qCritical() &lt;&lt; "At least one of the feedback filter coefficients has to be non-zero.";
  if (A[0] == 0)
      qCritical() &lt;&lt; "First feedback coefficient has to be non-zero.";
  
  //Normalize feedback coefficients if a[0] != 1;
  auto a0 = A[0];
  if (a0 != 1.0) {
      std::transform(A.begin(), A.end(), A.begin(), [a0](double v) { return v / a0; });
      std::transform(B.begin(), B.end(), B.begin(), [a0](double v) { return v / a0; });
  }
  
  int filterN = std::max(A.size(), B.size());
  B.resize(filterN, 0);
  A.resize(filterN, 0);
  Zi.resize(filterN, 0);
  Y.resize(X.size());
  
  const double *x = &amp;X[0];
  const double *b = &amp;B[0];
  const double *a = &amp;A[0];
  double *z = &amp;Zi[0];
  double *y = &amp;Y[0];
  
  //Implementation of Direct Form II:
  for (unsigned int m = 0; m &lt; X.size(); ++m) {
  
      y[m]=b[0] * x[m] + z[0];
  
      for (int i=1; i&lt;filterN; ++i)
          z[i-1]=b[i]*x[m] + z[i] - a[i] * y[m];
  
  }//outter for
  Zi.resize(filterN - 1);
  </code></pre>
  
  <p>}</p>
  
  <p>If I filter the exact same data, with same coefficients and initial conditions I get different results by the C++-code and MATLAB:</p>
  
  <p><a href="https://i.stack.imgur.com/XeTmy.jpg" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/XeTmy.jpg" alt="C++ and Matlab Results"></a></p>
  
  <p>Testing data and call of the filter function:</p>
  
  <pre><code>double sig[512]={0.0301553000000000,-0.00801991000000000,-0.0153981000000000,0.0202091000000000,0.0588846000000000,0.0768242000000000,0.0649313000000000,0.0238998000000000,0.0680403000000000,0.0340116000000000,0.0226322000000000,0.0328156000000000,-0.00146162000000000,0.0205671000000000,-0.0643990000000000,-0.0292278000000000,0.0447549000000000,0.0702186000000000,0.0376394000000000,0.0631467000000000,0.00374337000000000,0.0200464000000000,0.0670593000000000,0.0676816000000000,0.0422013000000000,0.0895179000000000,0.0699540000000000,0.00834636000000000,0.0618981000000000,0.0163444000000000,0.0637089000000000,0.0280489000000000,-0.0338468000000000,-0.00169661000000000,-0.0144219000000000,0.0461677000000000,0.0914735000000000,0.0774692000000000,0.0124293000000000,0.0556118000000000,0.0782004000000000,0.0864486000000000,0.198791000000000,0.205387000000000,0.146939000000000,0.0935380000000000,0.111979000000000,0.144112000000000,0.160979000000000,0.0955876000000000,0.0451640000000000,0.0857292000000000,0.0256333000000000,0.126769000000000,0.0891468000000000,0.0926968000000000,0.0366997000000000,0.0336009000000000,0.110258000000000,0.144939000000000,0.0894883000000000,0.0963139000000000,0.109508000000000,0.178156000000000,0.0741350000000000,0.101345000000000,0.0994547000000000,0.0637726000000000,0.165592000000000,-0.0110471000000000,0.0706953000000000,0.0548908000000000,0.0198740000000000,0.0794155000000000,0.0153542000000000,0.00320441000000000,0.0856460000000000,0.0824409000000000,0.0724669000000000,0.0705595000000000,0.0639445000000000,0.124914000000000,0.174332000000000,0.00388818000000000,0.119490000000000,0.160685000000000,0.133566000000000,0.0556858000000000,0.111291000000000,0.0958105000000000,0.0989478000000000,0.0667650000000000,0.0403985000000000,0.0351970000000000,0.0581811000000000,0.0454274000000000,0.0722400000000000,0.0510337000000000,0.100385000000000,0.0391950000000000,0.0639040000000000,0.143856000000000,0.0576045000000000,0.0438217000000000,0.112237000000000,0.0337487000000000,0.127032000000000,0.0614564000000000,0.0130552000000000,0.0214976000000000,0.0132415000000000,-0.0541594000000000,-0.00519632000000000,-0.0220990000000000,-0.0158140000000000,-0.0210716000000000,0.0604699000000000,0.0262074000000000,0.000682440000000000,-0.0236334000000000,0.0681717000000000,0.0208382000000000,0.0492043000000000,0.0348530000000000,-0.0886231000000000,-0.00147218000000000,-0.0435247000000000,0.00295584000000000,0.0183652000000000,-0.0144335000000000,-0.112745000000000,-0.0449680000000000,-0.100988000000000,-0.136776000000000,-0.0151994000000000,-0.0590707000000000,-0.0586963000000000,-0.0767725000000000,-0.0421415000000000,0.00846702000000000,-0.0239340000000000,0.0797944000000000,0.00844740000000000,-0.0513360000000000,0.0206669000000000,-0.00955059000000000,-0.0909315000000000,-0.155879000000000,-0.112039000000000,-0.0875029000000000,-0.123504000000000,-0.155892000000000,-0.0541159000000000,-0.105131000000000,-0.0613991000000000,-0.103661000000000,-0.0393360000000000,-0.0464205000000000,-0.0489061000000000,0.0113535000000000,-0.139501000000000,-0.0912677000000000,-0.0430320000000000,-0.0919059000000000,-0.170332000000000,-0.0402789000000000,-0.243246000000000,-0.182610000000000,-0.206052000000000,-0.145159000000000,-0.149846000000000,-0.104331000000000,-0.130689000000000,-0.0550775000000000,-0.0847037000000000,-0.0906652000000000,-0.0611655000000000,-0.0319579000000000,-0.0473531000000000,-0.0688338000000000,-0.103798000000000,-0.101008000000000,-0.120274000000000,-0.181519000000000,-0.189958000000000,-0.166088000000000,-0.171507000000000,-0.183625000000000,-0.125481000000000,-0.0857660000000000,-0.0736183000000000,-0.122247000000000,-0.0244709000000000,-0.0558359000000000,-0.0889673000000000,-0.0476182000000000,-0.0349962000000000,-0.148264000000000,-0.155345000000000,-0.125096000000000,-0.147017000000000,-0.155252000000000,-0.135466000000000,-0.163400000000000,-0.161364000000000,-0.204172000000000,-0.143225000000000,-0.0267562000000000,-0.0896427000000000,-0.0764977000000000,0.00428936000000000,-0.0715688000000000,0.0274398000000000,-0.000759624000000000,0.0179847000000000,-0.0132681000000000,-0.0287426000000000,-0.0978525000000000,-0.0805246000000000,-0.123476000000000,-0.123752000000000,-0.112998000000000,-0.118351000000000,-0.0656219000000000,-0.102996000000000,-0.0709688000000000,-0.0357844000000000,-0.0759794000000000,-0.0243623000000000,-0.0437790000000000,-0.0166207000000000,0.0229510000000000,0.0462987000000000,-0.0253879000000000,-0.0298303000000000,-0.00639451000000000,-0.0591668000000000,-0.149709000000000,-0.150842000000000,-0.150918000000000,-0.149163000000000,-0.153020000000000,-0.211794000000000,-0.157762000000000,-0.0675193000000000,-0.0985220000000000,-0.00242723000000000,0.114470000000000,0.128633000000000,0.147230000000000,0.201037000000000,0.195665000000000,0.199930000000000,0.186694000000000,0.118064000000000,0.142066000000000,0.110167000000000,0.0877817000000000,-0.0246267000000000,0.00267231000000000,-0.0658368000000000,-0.0714339000000000,-0.0802351000000000,-0.0895437000000000,-0.0346873000000000,-0.0340830000000000,-0.0697372000000000,-0.0559497000000000,-0.0137439000000000,-0.0785795000000000,-0.00567654000000000,-0.0379433000000000,-0.0976015000000000,-0.0945133000000000,-0.0655420000000000,-0.0855820000000000,-0.0400805000000000,0.00388486000000000,0.0342981000000000,0.0372252000000000,0.0146965000000000,-0.0127386000000000,0.0266062000000000,-0.0775236000000000,-0.0793422000000000,-0.0333086000000000,-0.0676361000000000,-0.0861102000000000,-0.100790000000000,-0.123455000000000,-0.116006000000000,-0.0280329000000000,-0.121147000000000,-0.141377000000000,-0.0834159000000000,-0.0597028000000000,-0.0153403000000000,-0.0181605000000000,-0.150602000000000,-0.0346134000000000,-0.0419538000000000,-0.0614610000000000,-0.0636917000000000,-0.164691000000000,-0.146294000000000,-0.132169000000000,-0.0181037000000000,-0.0572773000000000,0.0240336000000000,0.0195491000000000,-0.0297035000000000,-0.00891267000000000,-0.0235229000000000,0.0207131000000000,-0.00834759000000000,0.0600199000000000,-0.00980419000000000,-0.0207487000000000,-0.0749560000000000,-0.0506684000000000,-0.0355983000000000,-0.101271000000000,-0.0235542000000000,-0.0535405000000000,0.0260970000000000,-0.0159564000000000,-0.0315985000000000,0.0598761000000000,-0.0183607000000000,0.0618681000000000,-0.00142361000000000,0.00228268000000000,0.0236103000000000,0.0775528000000000,0.0321272000000000,0.0294343000000000,0.0103265000000000,-0.00667698000000000,0.0497296000000000,0.0769773000000000,0.0592501000000000,0.0408687000000000,0.0369409000000000,0.101000000000000,0.104389000000000,0.0983778000000000,0.133323000000000,0.0210208000000000,0.134726000000000,0.0730400000000000,0.0319604000000000,0.0814245000000000,0.0941598000000000,-0.00182081000000000,0.0425920000000000,0.0166802000000000,0.0583799000000000,0.0627635000000000,0.0739540000000000,0.131406000000000,0.123182000000000,0.145404000000000,0.0975905000000000,0.161993000000000,0.105066000000000,0.139453000000000,0.130792000000000,0.115084000000000,0.109607000000000,0.122310000000000,0.177461000000000,0.131937000000000,0.0690271000000000,0.123451000000000,0.199143000000000,0.104880000000000,0.155044000000000,0.0328337000000000,0.119657000000000,0.125927000000000,0.141303000000000,0.0759913000000000,0.0585626000000000,0.187292000000000,0.164067000000000,0.124541000000000,0.112823000000000,0.120399000000000,0.0819125000000000,0.0503885000000000,0.0598993000000000,0.0731133000000000,0.0796579000000000,0.0927431000000000,0.220712000000000,0.0827544000000000,0.101341000000000,0.139683000000000,0.154708000000000,0.188515000000000,0.105614000000000,0.0704782000000000,0.131886000000000,0.0997564000000000,0.125620000000000,0.0591369000000000,0.00955653000000000,0.0858687000000000,0.0732559000000000,0.0699862000000000,0.0360095000000000,0.0580970000000000,0.0839835000000000,0.0954270000000000,0.132291000000000,0.122760000000000,0.121034000000000,0.0251457000000000,0.0732390000000000,0.0281497000000000,0.0959885000000000,0.103115000000000,-0.0241668000000000,0.0140084000000000,-0.0292540000000000,-0.0327745000000000,0.0372446000000000,0.0299187000000000,0.0586155000000000,0.0506208000000000,0.0410404000000000,0.0125520000000000,0.0460507000000000,0.0465461000000000,0.0253696000000000,0.0109578000000000,-0.0194491000000000,-0.000751329000000000,-0.00500415000000000,0.0138312000000000,-0.0415195000000000,-0.0996424000000000,-0.0639008000000000,-0.0956486000000000,0.00611232000000000,-0.0400543000000000,-0.0700538000000000,0.0489084000000000,-0.0879531000000000,-0.0183493000000000,-0.0286791000000000,-0.00958466000000000,0.0173814000000000,-0.0802508000000000,-0.0951832000000000,-0.0829035000000000,-0.103259000000000,-0.0882276000000000,-0.105796000000000,-0.102774000000000,-0.0735422000000000,-0.135660000000000,-0.137493000000000,-0.114738000000000,-0.0838888000000000,-0.0796277000000000,-0.0855221000000000,-0.0268337000000000,0.00617851000000000,-0.0354986000000000,-0.117464000000000,-0.0923495000000000,-0.131204000000000,-0.125495000000000,-0.190157000000000,-0.0972211000000000,-0.189701000000000,-0.146600000000000,-0.160379000000000,-0.100232000000000,-0.0720795000000000,-0.119566000000000,-0.109223000000000,-0.134893000000000,-0.0747050000000000,-0.0509545000000000,-0.0348200000000000,-0.0384818000000000,-0.201293000000000,-0.125113000000000,-0.184330000000000,-0.0973027000000000,-0.126920000000000,-0.0867127000000000,-0.239812000000000,-0.132211000000000,-0.139374000000000,-0.150527000000000,-0.120978000000000,-0.118257000000000,-0.0822779000000000,0.0175771000000000,-0.0147237000000000,-0.0178227000000000,-0.0471514000000000,-0.0572225000000000,-0.119575000000000,-0.105603000000000,-0.143081000000000,-0.0607441000000000,-0.0908197000000000,-0.130008000000000};
  const double b[17] = {
      0.0002729280916688,-0.002818178884024,  0.01385249678719, -0.04329539107102,
         0.0971334355414,  -0.1680540396757,   0.2365723388665,  -0.2836197063858,
         0.2999122334602,  -0.2836197063858,   0.2365723388665,  -0.1680540396757,
         0.0971334355414, -0.04329539107102,  0.01385249678719,-0.002818178884024,
      0.0002729280916688
  };
  
  const double a[17] = {
          1,    -13.3212188251,    83.86489461498,   -331.2051347314,
  918.3569208444,   -1895.670808779,    3013.314032863,   -3762.532414523,
  3729.566584511,     -2944.5940727,    1845.629360615,   -908.7381606851,
  344.5861604764,   -97.28521252224,    19.28726185588,   -2.399297565806,
  0.1411045568471
  };
  
  //Initial Conditions (extracted from debugging session of filtfilt in matlab)
  //they do not depend on the input signal, just on the coefficients
  double zi[16]={-0.000173016035126118,0.00131421248042663,-0.00415917021404714,0.00604483470845739,
                 0.000666327768834688,-0.0206800015884750,0.0438140615773069,-0.0484885833804114,
                 0.0242278506306987,0.0136471075296590,-0.0385246063021973,0.0387355348806551,
                 -0.0239695887113909,0.00960583670533444,-0.00231963008475990,0.000258830045206669};
  
  int length=17;
  vectord aV(a, a+length);
  vectord bV(b, b+length);
  vectord ziV(zi, zi+(length-1));
  vectord xV(sig, sig+512); //copy input to xV
  vectord yV;
  yV.resize(512);
  
  filter(bV, aV, xV, yV,ziV);
  </code></pre>
  
  <p>Does anyone have a solution or a hint for me?
  I guessed that different precision in calculations could be the problem!?</p>