MATLAB and C Programs - Digital Signal Processing: A Practitioner's Approach - page 174

static float delp_k[3]={0, 0, 0},old_delp_k[3]={0, 0, 0};
static float alpha,alpha1=0.95, delx_k=0;
static float temp1[3][3],temp2[3] [3];
static float scalar;
alpha=0.98*alpha1;
xk2=xk1; xk1=xk;
xk=pk[0]*xk1+pk[1]*xk2;
sk3=sk2; sk2=sk1; sk1=sk;
sk=pk[0]*sk1+pk[1]*sk2+yk;
ek=yk-xk;
Jk=ek*ek-pk[2];
gk[0]=2*ek*sk1;
gk[1]=2*ek*sk2;
gk[2]= 1.0;
mat_eql(Hk,Hk_new);
scalar = 1.0/alpha;
mat_scl(Hk_new,scalar);
mat_mul_row_col(gk,gk,temp1);
mat_mul(Hk_new,temp1,temp2);
mat_mul(temp2,Hk_new,delH_num);
mat_mul_col(Hk_new,gk,temp1);
delH_den = 1 + mat_dot(gk,temp1);
mat_eql(delH_num,delH);
scalar = (-1.0/delH_den);
mat_scl(delH,scalar);
mat_add(Hk_new,delH,Hk);
ary_eql(delp_k,old_delp_k);
ary_add(pk,delp_k,pk);
Jk_hat=Jk - mat_dot(gk,old_delp_k)
mat_mul_col(Hk,gk,temp1);