RLS фильтр Опции

[rx9cim]

Написал код для RLS фильтра для подавления шумов, но фильтр не работает. Где-то пол секунды идет трескотня, а затем тишина. Посмотрел в отладчике -переменные имеют значение 0.NAN, но где они могли переполнится? Привожу свой код. Прошу помощи





#define Ord (3)
#define sigma (100000000)
#define lambda (0.98f)


struct common_rls
{
float H[Ord];
float G[Ord];
float Ri[Ord*Ord]; // (Ri)tr = (Ri)*
float X[Ord];
float Wrk[Ord];
};
struct common_rls rls;

void rls_init (void)
{
float diag=(float)100.0/sigma;
int ind=0;
volatile int i=0, j=0;
for (i=0; i<Ord; i++)
{
rls.H[i] = 0.0;
rls.G[i] =0.0;
rls.X[i] =0.0;
for (j=0; j<Ord; j++)
{
if (i==j)
{
rls.Ri[ind++] = diag;
}
else rls.Ri[ind++]=0;
}

}
}

float rls_flt(float x)
{
// n - filter length
// omega - forgetting factor
// x - filter input
// y - filter output
// X - input delay line
// G - Kalman gain, vector of length n
// Ri - inverse of correlation matrix n x n, might be simmetrical packed
// h - impulse response, vector of order n

volatile int i, j, ind=0;
volatile float ssum, csum;
volatile float error;
volatile float y=0;
static volatile float x_delay[2];

/*while (x!=0)
{*/

for (i=1; i>0; i--) {
x_delay[i] = x_delay[i-1];
}
x_delay[0]=x;
// error scalar
// input delay line shift
for (i=Ord-1; i>0; i--) {
rls.X[i] = rls.X[i-1];
}
rls.X[0] = x_delay[1];
error = x;//y;
for (i=0; i<Ord; i++)
{
y=rls.H[i]*rls.X[i];
}
error = x-y;

// adaptation
// calculate Kalman Gain
// ind=0
for (i=0; i<Ord; i++)
{
rls.Wrk[i] =0;
for (j=0; j<Ord; j++)
rls.Wrk[i] =rls.Wrk[i]+ rls.Ri[ind++]*(rls.X[j]);
}
csum = lambda;
for (i=0; i<Ord; i++)
{
csum = csum + rls.X[i]*rls.Wrk[i];
}
csum = 1/csum;
for (i=0; i<Ord; i++) {
rls.G[i] = rls.Wrk[i]*csum;
}
ssum = 1/lambda;
ind =0;
// calculate inverse correlation
for (i=0; i<Ord; i++)
{

for (j=0; j<Ord; j++)
{

rls.Ri[ind] = (rls.Ri[ind] - rls.G[i]* (rls.Wrk[j]))*ssum;
ind++;
}
}
// filtering

// impulse response update
for (i=0; i<Ord; i++)
{
rls.H[i] = rls.H[i] + rls.G[i]*error;
}

/*}*/
return y;
}