RBF_PID_CPP

#include <iostream>
#include <vector>
#include <math.h>
using namespace std;

double eta=0.5;//学习率
double alfa=0.05;//动量因子
double x[3]={0};//输入向量x
double ci[6][3]={0};//中心矢量C
double bi[6];//基宽向量B
double w[6];//权值向量W
double h[6]={0};//径向基向量h
double ci_1[6][3],ci_2[6][3],ci_3[6][3];
double bi_1[6],bi_2[6],bi_3[6];
double w_1[6],w_2[6],w_3[6];
double u,u_1,y,y_1;
double xc[3]={0};//增量式PID的输入
double error,error_1,error_2;
double kp0=0.01,ki0=0.01,kd0=0.01;//初始P、I、D
double kp_1=kp0,ki_1=ki0,kd_1=kd0;
double etaP=0.15,etaI=0.15,etaD=0.15;//P,I,D系数调整速度

double kp,ki,kd;

//vector<double> yd;



double caculateNorm2(double * tx,int count)//计算矩阵2-范数
{
	double norm2=0;
	for(int i=0;i<count;i++)
	{
		norm2+=pow(tx[i],2);
	}
	return norm2;
}
double caculateHj(double* tx,double * tc,int count,double tbij)//计算h[j]
{
	double norm2=0,hj=0;
	for(int i=0;i<count;i++)
	{
		norm2+=pow(tx[i]-tc[i],2);
	}
	//hj=exp(-norm2/(2*pow(tbj,2)));
	return exp(-norm2/(2*pow(tbij,2)));
}

double caculateYpY(double * tx,double* ty,int count)//辨识器网络输出
{
	double re=0;
	for (int i=0;i<count;i++)
	{
		re+=tx[i]*ty[i];
	}
	return re;
}
void initPara()//初始化参数
{
	for (int i=0;i<6;i++)
	{
		bi[i]=10;
		w[i]=0.1;
	}
	memcpy(ci_1,ci,18*sizeof(double));
	memcpy(ci_2,ci,18*sizeof(double));
	memcpy(ci_3,ci,18*sizeof(double));
	memcpy(w_1,w,6*sizeof(double));
	memcpy(w_2,w,6*sizeof(double));
	memcpy(w_3,w,6*sizeof(double));
	memcpy(bi_1,bi,6*sizeof(double));
	memcpy(bi_2,bi,6*sizeof(double));
	memcpy(bi_3,bi,6*sizeof(double));
	u_1=0;
	y_1=0;

}
void updatePID(double yd,double ty)//更新PID
{
	//double ts=0.01;
	//yd.push_back(sin(time*ts)*cos(2*time*ts)); 
	//double yd=sin(time*ts)*cos(2*time*ts);
	//double ty=(-0.1*y_1+u_1)/(1+y_1*y_1); 
	for(int j=0;j<6;j++)
	{
		h[j]=caculateHj(x,ci[j],3,bi[j]);
	}
	double ym=caculateYpY(w,h,6);

	double d_w[6]={0};
	for (int j=0;j<6;j++)
	{
		d_w[j]=eta*(ty-ym)*h[j];
	}
	for (int i=0;i<6;i++)
	{
		w[i]=w_1[i]+d_w[i]+alfa*(w_1-w_2);
	}

	double d_bi[6]={0};
	for (int j=0;j<6;j++)
	{
		double x_cij[3];
		for(int i=0;i<3;i++)
		{
			x_cij[i]=x[i]-ci[j][i];
		}
		d_bi[j]=eta*(ty-ym)*w[j]*h[j]*pow(bi[j],-3)*caculateNorm2(x_cij,3);
	}
	for (int i=0;i<6;i++)
	{
		bi[i]=bi_1[i]+d_bi[i]+alfa*(bi_1[i]-bi_2[i]);
	}

	double d_ci[6][3]={0};
	for (int j=0;j<6;j++)
	{
		for (int i=0;i<3;i++)
		{
			d_ci[j][i]=eta*(y-ym)*w[j]*h[j]*(x[i]-ci[j][i])*pow(bi[j],-2);
		}
	}
	for (int i=0;i<3;i++)
	{
		for (int j=0;j<6;j++)
		{
			ci[j][i]=ci_1[j][i]+d_ci[j][i]+alfa*(ci_1[j][i]-ci_2[j][i]);
		}
	}

	double yu=0;
	for (int j=0;j<6;j++)
	{
		yu+=w[j]*h[j]*(ci[j][1]-x[1])/pow(bi[j],2);
	}
	double dyu=yu;
	error=yd-ty;
	kp=kp_1+etaP*error*dyu*xc[1];
	kd=kd_1+etaD*error*dyu*xc[2];
	kp=ki_1+etaI*error*dyu*xc[3];
	if (kp<0)
	{
		kp=0;
	}
	if (ki<0)
	{
		ki=0;
	}
	if (kd<0)
	{
		kd=0;
	}
//	double du=kp*xc[1]+kd*xc[2]+ki*xc[3];

}

void main()
{
	initPara();
	double ts=0.01;
	for(int i=0;i<2000;i++)
	{
		double time=i*ts;
		double yd=sin(time)*cos(2*time);
		y=(-0.1*y_1+u_1)/(1+y_1*y_1);
		updatePID(yd,y);
		cout<<"第"<<i<<"次迭代误差："<<error<<endl;
		double du=kp*xc[1]+kd*xc[2]+ki*xc[3];
		u=u_1+du;
		if (i==300)
		{
			u=u+0.5;
		}
		x[1]=du;
		x[2]=y;
		x[3]=y_1;

		u_1=u;
		y_1=y;

		for (int j=0;j<6;j++)
		{
			for(int i=0;i<3;i++)
			{
				ci_2[j][i]=ci_1[j][i];
				ci_1[j][i]=ci[j][i];
			}

			bi_2[j]=bi_1[j];
			bi_1[j]=bi[j];

			w_2[j]=w_1[j];
			w_1[j]=w[j];
		}

		xc[1]=error-error_1;
		xc[2]=error-2*error_1+error_2;
		xc[3]=error;

		error_2=error_1;
		error_1=error;

		kp_1=kp;
		kd_1=kd;
		ki_1=ki;
		

	}
	cin.get();
}