//H = [0 1];
//
//K = Pest*H'*inv(H*Pest*H'+1);
//Xk = Xest + K*(y-H*Xest);
//
//Pk = Pest - K*H*Pest;
//F = [1 0; Xk(2) Xk(1)];
//
//Pest = F*Pk*F';
//Xest = [Xk(1) ; Xk(1)*Xk(2)];

//initialization

Xest0 = exp(complex<float>(0, 2*PI/pFreq->real));
Xest1 = complex<float>(pIn->hReal[0][1], pIn->hImag[0][1]);
Pest00 = pP00->real; Pest01 = 0;
Pest10 = 0; Pest11 = pP11->real;

// actual filter iterations
K0 = Pest01/(Pest11 + R);
K1 = real(Pest11/(Pest11 + R));

Xk0 = Xest0 + K0*(y - Xest1);
Xk1 = Xest1 + K1*(y - Xest1);

Pk00 = real(Pest00 - K0*Pest10);
Pk01 = Pest01 - K0*Pest11;
Pk10 = Pest10 - K1*Pest10;
Pk11 = real(Pest11 - K1*Pest11);

Pest00 = Pk00;
Pest01 = Pk00*conj(Xk1)+Pk01*conj(Xk0);
Pest10 = Pk00*Xk1 + Pk10*Xk0;
Pest11 = real(Pk00*Xk1*conj(Xk1) + Pk10*Xk0*conj(Xk1) + Pk01*Xk1*conj(Xk0) + Pk11*Xk0*conj(Xk0));

Xest0 = Xk0;
Xest1 = Xk0*Xk1;

// R = 2, pP00 = .2 , pP11 = 1