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_OBJECTpublic: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 lineui.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 lineui.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 dotui.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 dotui.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;elseyd=-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;  }