使用递推最小二乘法（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      endsubplot(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;//正确//更新xitafor (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");}

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

#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);//设置mvoid set_m(int mm);//设置nvoid set_n(int nn);//初始化void init_matrix();//释放void free_matrix();//读取i,j坐标的数据//失败返回-31415,成功返回值double read(int i,int j);//写入i,j坐标的数据//失败返回-1,成功返回1int write(int i,int j,double val);//C = A + B//成功返回1,失败返回-1int add(_Matrix *A,_Matrix *B,_Matrix *C);//C = A - B//成功返回1,失败返回-1int subtract(_Matrix *A,_Matrix *B,_Matrix *C);//C = A * B//成功返回1,失败返回-1int multiply(_Matrix *A,_Matrix *B,_Matrix *C);//行列式的值,只能计算2 * 2,3 * 3//失败返回-31415,成功返回值double det(_Matrix *A);//求转置矩阵,B = AT//成功返回1,失败返回-1int transpos(_Matrix *A,_Matrix *B);//求逆矩阵,B = A^(-1)//成功返回1,失败返回-1int inverse(_Matrix *A,_Matrix *B);//矩阵行扩展，C = [A B]//成功返回1，失败返回-1int linesExpand(_Matrix *A,_Matrix*B,_Matrix *C);//矩阵列扩展，C = [A B]'//成功返回1，失败返回-1int 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;}//设置mvoid _Matrix::set_m(int mm){m = mm;}//设置nvoid _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,成功返回1int _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,失败返回-1int _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,失败返回-1int _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,失败返回-1int _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,失败返回-1int _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,失败返回-1int _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，失败返回-1int _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，失败返回-1int _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;}