greypidcontrol.cpp

而灰色PID控制算法

// greyPIDcontrol.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"
#include "greyPIDcontrol.h"
#ifdef _DEBUG
#define new DEBUG_NEW
#endif


// 唯一的应用程序对象

CWinApp theApp;

using namespace std;

int _tmain(int argc, TCHAR* argv[], TCHAR* envp[])
{
	int nRetCode = 0;

	// 初始化 MFC 并在失败时显示错误
	if (!AfxWinInit(::GetModuleHandle(NULL), NULL, ::GetCommandLine(), 0))
	{
		// TODO: 更改错误代码以符合您的需要
		_tprintf(_T("致命错误: MFC 初始化失败\n"));
		nRetCode = 1;
	}
	else
	{
		// TODO: 在此处为应用程序的行为编写代码。
         
		double simTime=0.001;
	    greyPIDcontrol gpc(nNumRow,nNumColoumn);	
	    CMatrix data=gpc.initDataMatrix(simTime);
	    CMatrix x1=gpc.AGOmatrix(data);	
	    CMatrix B=gpc.dataMatrixB(x1);
	 //     for(int i=0;i<3;i++)
		//{	for(int j=0;j<3;j++)
		//	{
		//	  double b=data.GetElement(i,j);
	 //        	std::cout<<b<<"\t";
		//	}
		//	std::cout<<std::endl;
		//}
		gpc.data_Dk(data,simTime);
		gpc.AGO_D1k(gpc.D);
		gpc.paramaterVectorEstimation(B);
	   
	}

	return nRetCode;
}
//--------------灰色PID控制器构造函数------------------
//           函数名  ： greyPIDcontrol
//           入口参数： nRows   初始离散数列行数
//                      nCols   初始离散数列数
//           返回值  ： NULL
//-----------------------------------------------------
greyPIDcontrol::greyPIDcontrol(int nRows,int nCols)
{
   nNumRows=nRows;
   nNumCols=nCols;
   x[0]=0,x[1]=0;
   simStep=0;
}

 /////////////////////////////////////////////////////////////////////////////////////
 //        灰色估计器对不确定部分的模型参数建立GM (0,N)模型的具体算法的步骤         //
 /////////////////////////////////////////////////////////////////////////////////////

//--------------------------建立原始离散序列-----------------------------step 1
//         函数名  : initDataMatrix
//         入口参数: ts 采样时间
//         返回值  : CMatrix(数据矩阵)
//------------------------------------------------------------------------
CMatrix greyPIDcontrol::initDataMatrix(double ts)
{
	double value[nSize];
	int count=0;
	int k=2;
    for(int i=0;i<nNumCols;i++)
	{
	  rungeKutta4(x,k,ts);
	  value[count]=x[0],value[count+nNumCols]=x[1];
	  count++;
	}
	CMatrix data(nNumRows,nNumCols,value);
   return data;
}
//-----------------计算一次累加生成(1-AGO)离散序列x1(k)------------------step 2
//         函数名  : AGOmatrix
//         入口参数: mat 输入矩阵
//         返回值  : CMatrix(输出矩阵)
//------------------------------------------------------------------------            
CMatrix greyPIDcontrol::AGOmatrix(CMatrix mat)
{
	double a[nSize];
	double *pData=new double[nSize];
    for(int i=0;i<nNumRows;i++)
		for(int j=0;j<nNumCols;j++)
		{
			 a[j+i*nNumCols]=mat.GetElement(i,j);
		}
	for(int i=0;i<nNumRows;i++)
	{   double sum=0;
		for(int j=0;j<nNumCols;j++)
		{
          sum+=a[j+i*nNumCols];
		  pData[j+i*nNumCols]=sum;
		}
	}
	    CMatrix data1(nNumRows,nNumCols,pData); 
	delete[] pData;
	return data1;
}
//---------------------------计算数据矩阵B-------------------------------step 3
//         函数名  : dataMatrixB
//         入口参数: mat 输入矩阵
//         返回值  : CMatrix(输出矩阵)
//------------------------------------------------------------------------
CMatrix greyPIDcontrol::dataMatrixB(CMatrix mat)
{
	double pData[NSize];
	int count=0;
	double N=1;
	CMatrix matT(nNumCols,nNumRows);
		matT=mat.Transpose();
	for(int i=0;i<nNumCols;i++)
	{	
		for(int j=0;j<nNumRows;j++)
		{
			pData[count++]=matT.GetElement(i,j);
		}
		    pData[count++]=N++;	
	}
	CMatrix data2(nNumCols,nNumCols,pData);
	return data2;
}
//----------------------计算离散序列D(k)---------------------------------step 4
//         函数名  : data_Dk
//         入口参数: mat 输入矩阵
//                   ts  采样时间
//         返回值  : NULL
//------------------------------------------------------------------------
void greyPIDcontrol::data_Dk(CMatrix mat,double ts)
{
	double a=30,b=150;
	double dx2[nNumColoumn];
	 dx2[0]=(mat.GetElement(1,0)-0)/ts;
	 for(int k=1;k<nNumCols;k++)
	  {
		  dx2[k]=(mat.GetElement(1,k)-mat.GetElement(1,k-1))/ts;
	  }
	  for(int k=0;k<nNumCols;k++)
	  {
		  double u=0.5*sin(ts*k);
		  D[k]=(dx2[k]+a*mat.GetElement(1,k))/b-u;
		  //  std::cout<<D[k]<<std::endl;
	  }
}
//-------------------计算(1-AGO)离散序列D1(k)----------------------------step 5
//         函数名  : AGO_D1k
//         入口参数: value[] 离散系列Dk
//         返回值  : NULL
//------------------------------------------------------------------------
void greyPIDcontrol::AGO_D1k(double value[])
{
  D1[0]=value[0];
  double sum=0;
  for(int k=0;k<nNumCols;k++)
  {
     sum+=value[k];
	 D1[k]=sum;
  }

}
//----------------估算不确定部分bD(x,t)的灰色模型的参数向量V-------------
//         函数名  : paramaterVectorEstimation
//         入口参数: mat 输入矩阵
//         返回值  : NULL
//------------------------------------------------------------------------
void greyPIDcontrol::paramaterVectorEstimation(CMatrix mat)
{
	CMatrix BT(nNumRows+1,nNumCols);
	CMatrix BTB(nNumRows+1,nNumCols);
	CMatrix BBB(nNumRows+1,nNumCols);
	BT=mat.Transpose();

	BTB=BT*mat;
	BTB.InvertGaussJordan();
    BBB=BTB*BT;
    for(int i=0;i<nNumRows+1;i++)
	{   double sum=0;
		for(int j=0;j<nNumCols;j++)
		{
			sum+=BBB.GetElement(i,j)*D1[j];
		}
		Ve[i]=sum;
		std::cout<<Ve[i]<<std::endl;
	}
}
//--------------------------被控对象的状态方程形式------------------------
//         函数名  : paramaterVectorEstimation
//         入口参数: dx[] 方程组导数项
//                    x[] 方程组常数项
//                    h   采样时间
//         返回值  : NULL
//------------------------------------------------------------------------
void greyPIDcontrol::dynamicModel(double dx[],double x[],double h)
{
	double a=30,b=150;
    int M=4;
    double V[2],f;
 	if(M==1)
	     { V[0]=0.1,V[1]=0.1, f=0.1;}
	else if(M==2)
		{ V[0]=0.3,V[1]=0.3, f=0.5;}
	else if(M==3)
		 { V[0]=0.5,V[1]=0.5, f=1.5;}
	else if(M==4)
		 { V[0]=1.5,V[1]=1.5, f=5;}
	 double DD=V[0]*x[0]+V[1]*x[1]+f;
	 double u=0.5*sin(h*simStep++);
	 dx[0]=x[1];
	 dx[1]=-a*x[1]+b*(u+DD);	
}
//---------------------四阶龙格库塔解微分方程-----------------------------
//         函数名  : paramaterVectorEstimation
//         入口参数:  x[] 方程组常数项
//                    k   方程组个数
//                    h   采样时间
//         返回值  : NULL
//------------------------------------------------------------------------
void greyPIDcontrol::rungeKutta4(double x[],int k,double h)
{
 //   double a[4], *b, *f;
	//b = new double[k];
	//f = new double[k];
	double a[4],b[2]={0},f[2]={0};
	a[0] = h/2.0; a[1] = a[0]; a[2] = h; a[3] = h;
	//dynamicModel(f, x,h);
	for (int i = 0; i <=k-1; i++)  b[i] = x[i];
	for (int j = 0; j <=2; j++)
	{
		for (i = 0; i <=k-1; i++)
		{
			x[i] += a[j]*f[i];
			b[i] += a[j+1]*f[i]/3.0;
		}
	dynamicModel(f, x,h);
	}
	for (i = 0; i <=k-1; i++) x[i] = b[i]+h*f[i]/6.0;//解出微分方程组
	//delete(b);  delete(f);   //
}