rls.c

递归最小二乘滤波器算法

#include "rls.h"

extern struct rlshist hist_rls;

void set_zero(double *x, int length)
{
	int i;

	for(i = 0; i < length; i++)
		x[i] = 0.0;
}

/*初始化RLS算法*/
void init_rls()
{
	int i, j;

	set_zero(hist_rls.buffer, RANK);
	set_zero(hist_rls.weight, RANK);

	for(i = 0; i < RANK; i++)
		for(j = 0; j < RANK; j++){
			if(i == j) hist_rls.p[i][j] = 0.001;
			else hist_rls.p[i][j] = 0.0;
		}

	hist_rls.err = 0.0;
}

/*rlS自适应滤波算法*/
void rls(double *input, double *refer, double *output, int temp)
{
	int i, j;
	double pi[RANK];												//临时变量PI
	double k[RANK];													//增益向量
	double prierr;													//滤波器输出误差的先验估计值
	double temp1;
	double temp2[RANK];
	double temp3[RANK][RANK];

	/*将信号输入到RLS算法的输入端*/
	for(i = 0; i < RANK-1; i++){
		hist_rls.buffer[i] = hist_rls.buffer[i+1];
	}
	hist_rls.buffer[RANK-1] = *input;

	*output = 0.0;

	/*缓存信号，长度为滤波器的阶数RANK*/
	if(temp >= RANK){
		/*计算临时变量PI*/
		set_zero(pi, RANK);
		for(i = 0; i < RANK; i++)
			for(j = 0; j < RANK; j++)
				pi[i] += hist_rls.p[i][j] * hist_rls.buffer[j];

		temp1 = 0.0;
		for(i = 0; i < RANK; i++)
			temp1 += hist_rls.buffer[i] * pi[i];
		temp1 += LAMDA;

		/*计算增益向量*/
		set_zero(k, RANK);
		for(i = 0; i < RANK; i++)
			k[i] = pi[i] / temp1;

		/*计算输出*/
		*output = 0.0;
		for(i = 0; i < RANK; i++)
			*output += hist_rls.weight[i] * hist_rls.buffer[i];

		/*计算滤波器输出误差的先验估计*/
		prierr = *refer - *output;

		/*更新滤波器权矢量*/
		for(i = 0; i < RANK; i++)
			hist_rls.weight[i] += k[i] * prierr;

		/*更新逆相关矩阵P*/
		set_zero(temp2, RANK);
		for(i = 0; i < RANK; i++)
			for(j = 0; j < RANK; j++)
				temp2[i] += hist_rls.buffer[j] * hist_rls.p[j][i];

		for(i = 0; i < RANK; i++)
			for(j = 0; j < RANK; j++)
				temp3[i][j] = k[i] * temp2[j];

		for(i = 0; i < RANK; i++)
			for(j = 0; j < RANK; j++)
				hist_rls.p[i][j] = (hist_rls.p[i][j] - temp3[i][j]) / LAMDA;
	}
}