使用递推最小二乘法（RLS算法）进行自适应噪声对消

自适应噪声对消器的信号模型如下图所示

Matlab代码如下：

clear;
clc;

Ts=0.1;
num=100;%?采样点数?
k=1:num;
x=10*cos(2*pi*k*Ts+0.25*pi);%正弦干扰
%s=0.5*randn(1,num);
%x=x+s;
xr=cos(2*pi*k*Ts);%参考信号

p=2;%滤波器长度
xita=zeros(p,num-p+1); %保存权值
err=zeros(1,num-p+1);  %保存误差
kn=zeros(2,num-p+1);

lamda=0.99; %遗忘因子
diag_I=[1,0;0,1];
for i=1:num-p+1   
    
    if i==1
        hn=[xr(1,i);0];
        corr=10^5*diag_I;        
        kn(:,1)=(corr*hn)./(lamda^i+(hn')*corr*hn);%是列向量
        err(1,i)=x(1,i)-xita(1,1)*xr(1,i);
        xita(:,i)=err(1,i).*kn(:,i);   
         corr=(diag_I-kn(:,i)*hn')*corr;
         aaa=1;
    else
       hn=[xr(1,i); xr(1,i-1)];
        kn(:,i)=(corr*hn)./(lamda^i+(hn')*corr*hn);%是列向量
      % fprintf( '%d',(lamda^i+(hn')*corr*hn));
        err(1,i)=x(1,i)-xita(:,i-1)'*hn;
       fprintf('xi=%d tmp=%d\n',x(1,i),xita(:,i-1)'*hn);
      disp(err(1,i));
        xita(:,i)=xita(:,i-1)+err(1,i).*kn(:,i);
        corr=(diag_I-kn(:,i)*hn')*corr;
        disp(corr);
    end
   
   
end

subplot(2,1,2);
plot(k(1,1:num-p+1),xita,'r-');
subplot(2,1,1);
plot(k(1,1:num-p+1),err,'b-');

C++代码如下：

#include <stdio.h>
#include <iostream>
#include <stdlib.h>
#include <math.h>
#include <fstream>
#include "_Matrix.h"

using namespace std;

const double pi = 3.14159;

int main()
{
	cout<<"基于最小二乘自适应滤波算法的正弦信号干扰对消实验"<<endl;
	
	double Ts=0.1;//采样间隔
	int num = 200;//样本点数
	double *x=new double[num];
	double *xr=new double[num];
	
	int i,j;
	for (i=0;i<num;i++){
		xr[i]=cos(2.0*pi*double(i+1)*Ts);//参考信号	
	}
	int mode;
	cout<<"实验1：普通正弦干扰信号对消"<<endl;
	cout<<"实验2：幅值和相位差中途变化的正弦干扰信号对消"<<endl;
	cout<<"请输入数字1或2（若输入数字不为2，则默认进行实验1）:";

	cin >>mode;
	if (mode==2){
		for(i=0; i<num;i++){
			if (i<=50){
				x[i]=10*cos(2.0*pi*double(i+1)*Ts+0.25*pi);//正弦干扰
			}
			else{
				x[i]=5*cos(2.0*pi*double(i+1)*Ts+0.1*pi);//正弦干扰
			}
		}
	}
	else{
		for(i=0; i<num;i++){
				x[i]=10*cos(2.0*pi*double(i+1)*Ts+0.25*pi);//正弦干扰	
		}	
	}
	int p=2;//滤波器长度
	_Matrix xita(p,num-p+1);
	xita.init_matrix();
	double *err = new double[num-p+1];//储存误差
	//double *kn = new double[2];

	double lamda = 0.99;//遗忘因子
	_Matrix diag_I(p,p);
	diag_I.init_matrix();
	for (i=0;i<p; i++)
		for (j=0;j<p;j++){
			if (i==j)
				diag_I.write(i,j,1);//生成对角阵，特征值为1
		}

		
	_Matrix corr(p,p);
	corr.init_matrix();
	_Matrix hn(p,1);//列数据向量
	hn.init_matrix();
	_Matrix hn_det(1,p);//列数据向量
	hn_det.init_matrix();


	for (i=0;i<num-p+1;i++){
			_Matrix kn(p,1);
	kn.init_matrix();

		_Matrix tmp11(1,1);
		tmp11.init_matrix();

		_Matrix tmp21(2,1);
		tmp21.init_matrix();

		_Matrix tmp22(2,2);
		tmp22.init_matrix();
		if (i==0){
			for (j=0;j<p-1;j++){//hn[p-1]=0;
				hn.write(j,0,xr[p-2-j]);
				hn_det.write(0,j,xr[p-2-j]);
			}

			int ii,jj;
			for (ii=0;ii<p; ii++)
				for (jj=0;jj<p;jj++)
					corr.write(ii,jj,(100000.0)*diag_I.read(ii,jj));
			//corr.printff_matrix();
			tmp21.multiply(&corr,&hn,&tmp21);
			tmp11.multiply(&hn_det,&tmp21,&tmp11);

			double weight = pow(lamda,i)+tmp11.read(0,0);
			weight=1.0/weight;
			for (ii=0;ii<p; ii++)
				kn.write(ii,0,weight*tmp21.read(ii,0));

			double kn1=kn.read(0,0);
			double kn2=kn.read(1,0);
			err[i]=x[i]-xita.read(0,0)*xr[0];

			for (ii=0;ii<p; ii++)
				xita.write(ii,0,err[i]*kn.read(ii,0));
			
			tmp22.multiply(&kn,&hn_det,&tmp22);
			tmp22.subtract(&diag_I,&tmp22,&tmp22);
			corr.multiply(&tmp22,&corr,&corr);
			//corr.printff_matrix();
			//int aaa; 正确！
		}
		else{
			for (j=0;j<p;j++){//hn[j]=xr;
				hn.write(j,0,xr[p-2-j+i]);	
				hn_det.write(0,j,xr[p-2-j+i]);
			}
			//hn.printff_matrix();
			int ii,jj;
			//corr.printff_matrix();
			tmp21.multiply(&corr,&hn,&tmp21);
			//corr.printff_matrix();
			tmp11.multiply(&hn_det,&tmp21,&tmp11);
			double ddd=tmp11.read(0,0);
			double weight = pow(lamda,i)+tmp11.read(0,0);
			weight=1.0/weight;//正确
			for (ii=0;ii<p; ii++)
				kn.write(ii,0,weight*tmp21.read(ii,0));

			//cout<<"kn"<<endl;
			//kn.printff_matrix();//正确
			double tmp=0;
			//cout<<"xita"<<endl;
			//cout<<xita.read(0,i-1)<<endl;
			//cout<<xita.read(1,i-1)<<endl;
			//hn.printff_matrix();
			for(ii=0;ii<p;ii++){
				tmp=tmp+xita.read(ii,i-1)*hn.read(ii,0);
			}

			err[i]=x[i]-tmp;
			//cout<<"xi="<<x[i]<<"tmp="<<tmp<<endl;
			cout<<"第"<<i<<"次迭代 err = "<<err[i]<<endl;//正确

			//更新xita
			for (ii=0;ii<p; ii++)
				xita.write(ii, i, xita.read(ii,i-1)+err[i]*kn.read(ii,0));

			tmp22.multiply(&kn,&hn_det,&tmp22);
			tmp22.subtract(&diag_I,&tmp22,&tmp22);
			corr.multiply(&tmp22,&corr,&corr);
			//cout<<"corr"<<endl;
			//corr.printff_matrix();
			//double aaaaa=1;
		}
		//cout<<xita.read(0,i)<<' '<<xita.read(1,i)<<endl;
	}

	ofstream x_fs,xr_fs,err_fs,xita_fs;
	x_fs.open("x.txt");
	xr_fs.open("xr.txt");
	err_fs.open("err.txt");
	xita_fs.open("xita.txt");

	for(i=0;i<num;i++){
		x_fs <<x[i]<< endl;
		xr_fs <<xr[i]<< endl;
	}
	for(i=0;i<num-p+1;i++){
		err_fs <<err[i] <<endl;
		for(j=0;j<p;j++){
			xita_fs <<xita.read(j,i)<<' ';
		}
		xita_fs<<endl;
	}
	x_fs.close();
	xr_fs.close();
	err_fs.close();
	xita_fs.close();
	cout << "程序执行完毕，过程数据存储在txt文件中" << endl << endl;

	system("pause");
}

矩阵运算的库我是在优快云的一个博客里找的，但是我忘了作者是谁了，总之不是我写的，在这里先向作者致谢和致歉

#ifndef _MATRIX_H
#define _MATRIX_H

//头文件
#include <stdio.h>
#include <stdlib.h>

//矩阵数据结构
//二维矩阵
class _Matrix
{
public:
	int m;
	int n;
	double *arr;
	
	//初始化
	_Matrix(int mm = 0,int nn = 0);
	//设置m
	void set_m(int mm);
	//设置n
	void set_n(int nn);
	//初始化
	void init_matrix();
	//释放
	void free_matrix();
	//读取i,j坐标的数据
	//失败返回-31415,成功返回值
	double read(int i,int j);
	//写入i,j坐标的数据
	//失败返回-1,成功返回1
	int write(int i,int j,double val);
	//C = A + B
	//成功返回1,失败返回-1
	int add(_Matrix *A,_Matrix *B,_Matrix *C);
	//C = A - B
	//成功返回1,失败返回-1
	int subtract(_Matrix *A,_Matrix *B,_Matrix *C);
	//C = A * B
	//成功返回1,失败返回-1
	int multiply(_Matrix *A,_Matrix *B,_Matrix *C);
	//行列式的值,只能计算2 * 2,3 * 3
	//失败返回-31415,成功返回值
	double det(_Matrix *A);
	//求转置矩阵,B = AT
	//成功返回1,失败返回-1
	int transpos(_Matrix *A,_Matrix *B);
	//求逆矩阵,B = A^(-1)
	//成功返回1,失败返回-1
	int inverse(_Matrix *A,_Matrix *B);
	//矩阵行扩展，C = [A B]
	//成功返回1，失败返回-1
	int linesExpand(_Matrix *A,_Matrix*B,_Matrix *C);
	//矩阵列扩展，C = [A B]'
	//成功返回1，失败返回-1
	int rowsExpand(_Matrix *A,_Matrix*B,_Matrix *C);
	//打印2维矩阵
	void printff_matrix();
};
#endif

#include "_Matrix.h"

//矩阵类方法
//初始化
_Matrix::_Matrix(int mm,int nn)
{
	m = mm;
	n = nn;
}

//设置m
void _Matrix::set_m(int mm)
{
	m = mm;
}

//设置n
void _Matrix::set_n(int nn)
{
	n = nn;
}

//初始化
void _Matrix::init_matrix()
{
	arr = new double[m * n];
	for(int i=0;i<m*n;i++)
		arr[i] = 0;
}

//释放
void _Matrix::free_matrix()
{
	delete []arr;
}

//读取i,j坐标的数据
//失败返回-31415,成功返回值
double _Matrix::read(int i,int j)
{
	if (i >= m || j >= n)
	{
		return -31415;
	}
	
	return *(arr + i * n + j);
}

//写入i,j坐标的数据
//失败返回-1,成功返回1
int _Matrix::write(int i,int j,double val)
{
	if (i >= m || j >= n)
	{
		return -1;
	}
	
	*(arr + i * n + j) = val;
	return 1;
}

//C = A + B
//成功返回1,失败返回-1
int _Matrix::add(_Matrix *A,_Matrix *B,_Matrix *C)
{
	int i = 0;
	int j = 0;
	
	//判断是否可以运算
	if (A->m != B->m || A->n != B->n || \
		A->m != C->m || A->n != C->n)
	{
		return -1;
	}
	//运算
	for (i = 0;i < C->m;i++)
	{
		for (j = 0;j < C->n;j++)
		{
			C->write(i,j,A->read(i,j) + B->read(i,j));
		}
	}
	
	return 1;
}

//C = A - B
//成功返回1,失败返回-1
int _Matrix::subtract(_Matrix *A,_Matrix *B,_Matrix *C)
{
	int i = 0;
	int j = 0;
	
	//判断是否可以运算
	if (A->m != B->m || A->n != B->n || \
		A->m != C->m || A->n != C->n)
	{
		return -1;
	}
	//运算
	for (i = 0;i < C->m;i++)
	{
		for (j = 0;j < C->n;j++)
		{
			C->write(i,j,A->read(i,j) - B->read(i,j));
		}
	}
	
	return 1;
}

//C = A * B
//成功返回1,失败返回-1
int _Matrix::multiply(_Matrix *A,_Matrix *B,_Matrix *C)
{
	int i = 0;
	int j = 0;
	int k = 0;
	double temp = 0;
	
	//判断是否可以运算
	if (A->m != C->m || B->n != C->n || \
		A->n != B->m)
	{
		return -1;
	}
	//运算
	for (i = 0;i < C->m;i++)
	{
		for (j = 0;j < C->n;j++)
		{
			temp = 0;
			for (k = 0;k < A->n;k++)
			{
				temp += A->read(i,k) * B->read(k,j);
			}
			C->write(i,j,temp);
		}
	}
	
	return 1;
}

//行列式的值,只能计算2 * 2,3 * 3
//失败返回-31415,成功返回值
double _Matrix::det(_Matrix *A)
{
	double value = 0;
	
	//判断是否可以运算
	if (A->m != A->n || (A->m != 2 && A->m != 3))
	{
		return -31415;
	}
	//运算
	if (A->m == 2)
	{
		value = A->read(0,0) * A->read(1,1) - A->read(0,1) * A->read(1,0);
	}
	else
	{
		value = A->read(0,0) * A->read(1,1) * A->read(2,2) + \
				A->read(0,1) * A->read(1,2) * A->read(2,0) + \
				A->read(0,2) * A->read(1,0) * A->read(2,1) - \
				A->read(0,0) * A->read(1,2) * A->read(2,1) - \
				A->read(0,1) * A->read(1,0) * A->read(2,2) - \
				A->read(0,2) * A->read(1,1) * A->read(2,0);
	}
	
	return value;
}

//求转置矩阵,B = AT
//成功返回1,失败返回-1
int _Matrix::transpos(_Matrix *A,_Matrix *B)
{
	int i = 0;
	int j = 0;
	
	//判断是否可以运算
	if (A->m != B->n || A->n != B->m)
	{
		return -1;
	}
	//运算
	for (i = 0;i < B->m;i++)
	{
		for (j = 0;j < B->n;j++)
		{
			B->write(i,j,A->read(j,i));
		}
	}
	
	return 1;
}

//打印2维矩阵
void _Matrix::printff_matrix()
{
	for (int i = 0;i < m;i++)
	{
		for (int j = 0;j < n;j++)
		{
			printf("%f ",*(arr + i * n + j));
		}
		printf("\n");
	}
}

//求逆矩阵,B = A^(-1)
//成功返回1,失败返回-1
int _Matrix::inverse(_Matrix *A,_Matrix *B)
{
	int i = 0;
	int j = 0;
	int k = 0;
	_Matrix m(A->m,2 * A->m);
	double temp = 0;
	double b = 0;
	
	//判断是否可以运算
	if (A->m != A->n || B->m != B->n || A->m != B->m)
	{
		return -1;
	}
	
	/*
	//如果是2维或者3维求行列式判断是否可逆
	if (A->m == 2 || A->m == 3)
	{
		if (det(A) == 0)
		{
			return -1;
		}
	}
	*/
	
	//增广矩阵m = A | B初始化
	m.init_matrix();
	for (i = 0;i < m.m;i++)
	{
		for (j = 0;j < m.n;j++)
		{
			if (j <= A->n - 1)
			{
				m.write(i,j,A->read(i,j));
			}
			else
			{
				if (i == j - A->n)
				{
					m.write(i,j,1);
				}
				else
				{
					m.write(i,j,0);
				}
			}
		}
	}
	
	//高斯消元
	//变换下三角
	for (k = 0;k < m.m - 1;k++)
	{
		//如果坐标为k,k的数为0,则行变换
		if (m.read(k,k) == 0)
		{
			for (i = k + 1;i < m.m;i++)
			{
				if (m.read(i,k) != 0)
				{
					break;
				}
			}
			if (i >= m.m)
			{
				return -1;
			}
			else
			{
				//交换行
				for (j = 0;j < m.n;j++)
				{
					temp = m.read(k,j);
					m.write(k,j,m.read(k + 1,j));
					m.write(k + 1,j,temp);
				}
			}
		}
		
		//消元
		for (i = k + 1;i < m.m;i++)
		{
			//获得倍数
			b = m.read(i,k) / m.read(k,k);
			//行变换
			for (j = 0;j < m.n;j++)
			{
				temp = m.read(i,j) - b * m.read(k,j);
				m.write(i,j,temp);
			}
		}
	}
	//变换上三角
	for (k = m.m - 1;k > 0;k--)
	{
		//如果坐标为k,k的数为0,则行变换
		if (m.read(k,k) == 0)
		{
			for (i = k + 1;i < m.m;i++)
			{
				if (m.read(i,k) != 0)
				{
					break;
				}
			}
			if (i >= m.m)
			{
				return -1;
			}
			else
			{
				//交换行
				for (j = 0;j < m.n;j++)
				{
					temp = m.read(k,j);
					m.write(k,j,m.read(k + 1,j));
					m.write(k + 1,j,temp);
				}
			}
		}
		
		//消元
		for (i = k - 1;i >= 0;i--)
		{
			//获得倍数
			b = m.read(i,k) / m.read(k,k);
			//行变换
			for (j = 0;j < m.n;j++)
			{
				temp = m.read(i,j) - b * m.read(k,j);
				m.write(i,j,temp);
			}
		}
	}
	//将左边方阵化为单位矩阵
	for (i = 0;i < m.m;i++)
	{
		if (m.read(i,i) != 1)
		{
			//获得倍数
			b = 1 / m.read(i,i);
			//行变换
			for (j = 0;j < m.n;j++)
			{
				temp = m.read(i,j) * b;
				m.write(i,j,temp);
			}
		}
	}
	//求得逆矩阵
	for (i = 0;i < B->m;i++)
	{
		for (j = 0;j < B->m;j++)
		{
			B->write(i,j,m.read(i,j + m.m));
		}
	}
	//释放增广矩阵
	m.free_matrix();
	
	return 1;
}

//C = [A B]
//成功返回1，失败返回-1
int _Matrix:: linesExpand(_Matrix *A,_Matrix*B,_Matrix *C)
{
	int i = 0;
	int j = 0;
	int k = 0;
	int l = 0;
	if(A->m!=B->m)
		return -1;
	
	for(i=0;i<A->m;i++)
		for(j=0;j<A->n;j++)
			C->write(i,j,A->read(i,j));

	for(i=0;i<B->m;i++)
		for(j=0;j<B->n;j++)
			C->write(i,j+A->n,B->read(i,j));

	return 1;
}

//C = [A B]'
//成功返回1，失败返回-1
int _Matrix:: rowsExpand(_Matrix *A,_Matrix*B,_Matrix *C)
{
	int i = 0;
	int j = 0;
	int k = 0;
	int l = 0;
	if(A->n!=B->n)
		return -1;
	
	for(i=0;i<A->m;i++)
		for(j=0;j<A->n;j++)
			C->write(i,j,A->read(i,j));

	for(i=0;i<B->m;i++)
		for(j=0;j<B->n;j++)
			C->write(i+A->m,j,B->read(i,j));

	return 1;
}