RBF_PID_Qt&plot

头文件

//testrbfplot.h
#ifndef TESTRBFPLOT_H
#define TESTRBFPLOT_H

#include <QtGui/QMainWindow>
#include "ui_testrbfplot.h"

#include <QTimer>
#include <QDateTime>


#include <iostream>  
#include <math.h>  

#include <fstream>
using namespace std;  



class testRBFPlot : public QMainWindow
{
	Q_OBJECT

public:
	testRBFPlot(QWidget *parent = 0, Qt::WFlags flags = 0);
	~testRBFPlot();

private:
	Ui::testRBFPlotClass ui;
	QTimer t1;


	double caculateNorm2(double * tx,int count);
	double caculateHj(double* tx,double * tc,int count,double tbij);
	double caculateYpY(double * tx,double* ty,int count);
	void initPara();
	void updatePID(double yd,double ty);

	void caculateData();
	unsigned int timeI;
	bool isPloting;
	ofstream outf;

	double yd,outy;
	double timt;

private slots:
	void plotData();
	void startPlot();
};

#endif // TESTRBFPLOT_H

cpp文件

//testrbfplot.h
#include "testrbfplot.h"

#include <QDebug>

double eta=0.5;//学习率  
double alfa=0.05;//动量因子  
double inx[3];//输入向量x  
double ci[6][3];//中心矢量C  
double bi[6];//基宽向量B  
double w[6];//权值向量W  
double h[6];//径向基向量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_1;  
double xc[3];//增量式PID的输入  
double error,error_1,error_2;  
double kp0=0.01,ki0=0.01,kd0=0.01;//初始P、I、D  
double kp_1,ki_1,kd_1;  
double etaP=0.15,etaI=0.15,etaD=0.15;//P,I,D系数调整速度  

double kp,ki,kd;  

testRBFPlot::testRBFPlot(QWidget *parent, Qt::WFlags flags)
	: QMainWindow(parent, flags)
{
	ui.setupUi(this);

	isPloting=false;
	connect(&t1,SIGNAL(timeout()),this,SLOT(plotData()));
	connect(ui.startBtn,SIGNAL(clicked()),this,SLOT(startPlot()));

	/*ui.customPlot->xAxis->setTickLabelType(QCPAxis::ltDateTime);
	ui.customPlot->xAxis->setDateTimeFormat("hh:mm:ss");
	ui.customPlot->addGraph();
	ui.customPlot->addGraph();*/
	ui.customPlot->setInteractions(QCP::iRangeZoom);
	ui.customPlot->axisRect(0)->setRangeZoom(Qt::Vertical);

	ui.customPlot->addGraph(); // blue line
	ui.customPlot->graph(0)->setPen(QPen(Qt::blue));
	//ui.customPlot->graph(0)->setBrush(QBrush(QColor(240, 255, 200)));
	ui.customPlot->graph(0)->setAntialiasedFill(false);
	ui.customPlot->addGraph(); // red line
	ui.customPlot->graph(1)->setPen(QPen(Qt::red));
	//ui.customPlot->graph(0)->setChannelFillGraph(ui.customPlot->graph(1));
	ui.customPlot->graph(0)->setName("target");
	ui.customPlot->graph(1)->setName("real output");

	ui.customPlot->addGraph(); // blue dot
	ui.customPlot->graph(2)->setPen(QPen(Qt::blue));
	ui.customPlot->graph(2)->setLineStyle(QCPGraph::lsNone);
	ui.customPlot->graph(2)->setScatterStyle(QCPScatterStyle::ssDisc);
	ui.customPlot->addGraph(); // red dot
	ui.customPlot->graph(3)->setPen(QPen(Qt::red));
	ui.customPlot->graph(3)->setLineStyle(QCPGraph::lsNone);
	ui.customPlot->graph(3)->setScatterStyle(QCPScatterStyle::ssDisc);

	ui.customPlot->xAxis->setTickLabelType(QCPAxis::ltDateTime);
	ui.customPlot->xAxis->setDateTimeFormat("hh:mm:ss");
	ui.customPlot->xAxis->setAutoTickStep(false);
	ui.customPlot->xAxis->setTickStep(2);
	ui.customPlot->axisRect()->setupFullAxesBox();
	connect(ui.customPlot->xAxis, SIGNAL(rangeChanged(QCPRange)), ui.customPlot->xAxis2, SLOT(setRange(QCPRange)));
	connect(ui.customPlot->yAxis, SIGNAL(rangeChanged(QCPRange)), ui.customPlot->yAxis2, SLOT(setRange(QCPRange)));
	ui.customPlot->yAxis->setRange(-1.5, 1.5);
	ui.customPlot->legend->setVisible(true);
	ui.customPlot->axisRect()->insetLayout()->setInsetAlignment(0,Qt::AlignTop|Qt::AlignLeft);
	ui.customPlot->graph(2)->removeFromLegend();
	ui.customPlot->graph(3)->removeFromLegend();
	ui.customPlot->yAxis->grid()->setVisible(false);
	ui.customPlot->xAxis->grid()->setVisible(false);


	ui.customPlot->legend->setBrush(QColor(255,255,255,0));
	/*ui.customPlot->legend->setBrush(QColor(29,32,41));
	ui.customPlot->legend->setTextColor(Qt::white);

	QBrush qBrush(QColor(29,32,41));
	ui.customPlot->setBackground(qBrush);*/
	//ui.customPlot->setBackground(QColor(255,255,41));
	/*ui.customPlot->legend->setBrush(QColor(29,32,41));*/
	/*ui.customPlot->legend->setBrush(QColor(29,32,41));
	ui.customPlot->legend->setTextColor(Qt::white);*/

}

testRBFPlot::~testRBFPlot()
{

}

void testRBFPlot::plotData()
{
	caculateData();
	ui.customPlot->graph(0)->addData(timt,yd);
	ui.customPlot->graph(1)->addData(timt,outy);

	ui.customPlot->graph(2)->clearData();
	ui.customPlot->graph(2)->addData(timt,yd);
	ui.customPlot->graph(3)->clearData();
	ui.customPlot->graph(3)->addData(timt,outy);

	
	if (ui.customPlot->graph(0)->data()->count()>800)
	{
		ui.customPlot->graph(0)->removeData(ui.customPlot->graph(0)->data()->begin().key());
		ui.customPlot->graph(1)->removeData(ui.customPlot->graph(1)->data()->begin().key());
	}
	/*ui.customPlot->graph(0)->removeDataBefore(timt-21);
	ui.customPlot->graph(1)->removeDataBefore(timt-21);*/

	//ui.customPlot->graph(0)->rescaleValueAxis();
	//ui.customPlot->graph(1)->rescaleValueAxis(true);
	/*ui.customPlot->xAxis->setRange(timt+0.25, 8, Qt::AlignRight);*/
	ui.customPlot->xAxis->setRange(ui.customPlot->graph(0)->data()->begin().key(),timt+0.2);
	ui.customPlot->replot();
}

void testRBFPlot::startPlot()
{
	if(isPloting)
	{
		outf.close();
		t1.stop();
		for(int i=0;i<4;i++)
			ui.customPlot->graph(i)->clearData();
		ui.customPlot->replot();
		isPloting=false;
		ui.startBtn->setText(tr("start"));
	}
	else
	{
		initPara();
		outf.open("result.txt");
		t1.start(10);
		isPloting=true;
		ui.startBtn->setText(tr("stop"));
	}
}

double testRBFPlot::caculateNorm2( double * tx,int count )
{
	double norm2=0;  
	for(int i=0;i<count;i++)  
	{  
		norm2+=pow(tx[i],2);  
	}  
	return norm2;  
}

double testRBFPlot::caculateHj( double* tx,double * tc,int count,double tbij )
{
	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 testRBFPlot::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 testRBFPlot::initPara()
{
	timeI=0;
	 
	error=error_1=error_2=y_1=0;  
	kp_1=kp0,ki_1=ki0,kd_1=kd0;  

	for (int i=0;i<6;i++)  
	{  
		bi[i]=10;  
		w[i]=0.1;
		h[i]=0;
		for (int j=0;j<3;j++)
		{
			ci[i][j]=0;
		}
	}
	for (int j=0;j<3;j++)
	{
		inx[j]=0;
		xc[j]=0;
	}

	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; 
	error_1=0;
	error_2=0;

}

void testRBFPlot::updatePID( double yd,double ty )
{
	for(int j=0;j<6;j++)  
	{  
		h[j]=caculateHj(inx,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]=inx[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*(outy-ym)*w[j]*h[j]*(inx[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][0]-inx[0])/pow(bi[j],2);  
	}  
	double dyu=yu;  
	error=yd-ty;  
	kp=kp_1+etaP*error*dyu*xc[0];  
	kd=kd_1+etaD*error*dyu*xc[1];  
	ki=ki_1+etaI*error*dyu*xc[2];  
	if (kp<0)  
	{  
		kp=0;  
	}  
	if (ki<0)  
	{  
		ki=0;  
	}  
	if (kd<0)  
	{  
		kd=0;  
	}  
}

void testRBFPlot::caculateData()
{
	timt= QDateTime::currentDateTime().toMSecsSinceEpoch()/1000.0;
	yd=sin(timt)*cos(2*timt); 
	//yd=sin(timt)*cos(3*timt);
	/*if (sin(timt)>0)
		yd=1;
	else
		yd=-1;*/
	//yd=sin(timt);
	//outy=(-0.1*y_1+u_1)/(1+y_1*y_1); 
	outy=(-0.1*y_1+u_1*3)/(1+y_1*y_1);//p0.0087224,i0.335238,d0.043908 
	updatePID(yd,outy);  

	/*kp=0.008724;
	ki=0.335238;
	kd=0.043908;*/

	qDebug()<<"times "<<timeI<<": "<<error<<",P:"<<kp<<",I:"<<ki<<",D:"<<kd<<endl;  

	outf<<"第"<<timeI<<"次迭代误差："<<error<<", P:"<<kp<<", I:"<<ki<<", D:"<<kd<<endl;  
	timeI++;

	//ui.statusBar->showMessage(QString("tmies :%1, P:%2 ,I:%3 ,D:%4").arg(error).arg(kp).arg(ki).arg(kd));
	ui.statusBar->showMessage(tr("tmies: ")+QString::number(timeI)+tr("er: ")+QString::number(error)+", P:"+QString::number(kp)+", I:"+QString::number(ki)+", D:"+QString::number(kd));


	double du=kp*xc[0]+kd*xc[1]+ki*xc[2];  
	u=u_1+du;  
	/*if (i==300)  
	{  
		u=u+0.5;  
	} */ 
	inx[0]=du;  
	inx[1]=outy;  
	inx[2]=y_1;  

	u_1=u;  
	y_1=outy;  

	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[0]=error-error_1;  
	xc[1]=error-2*error_1+error_2;  
	xc[2]=error;  

	error_2=error_1;  
	error_1=error;  

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