erjie_rk4.cpp

PID仿真程序

#include "stdafx.h"
//#include "erjie_rk4.h"
extern float x[2];

float f(int i,float x1,float x2,float u)
{
	if(i=0) {return x2;}
	else	
	{return -2*x1-3*x2+u;}
}

float rk4(float hh,float mv)	//二阶对象
{
//	static float x[2]={0};
	float K[2][4];
	K[0][0]=f(0,x[0],x[1],mv);
	K[0][1]=f(0,x[0]+hh/2*K[0][0],x[1],mv);
	K[0][2]=f(0,x[0]+hh/2*K[0][1],x[1],mv);
	K[0][3]=f(0,x[0]+hh*K[0][2],x[1],mv);
	K[1][0]=f(1,x[0],x[1],mv);
	K[1][1]=f(1,x[0],x[1]+hh/2*K[1][0],mv);
	K[1][2]=f(1,x[0],x[1]+hh/2*K[1][1],mv);
	K[1][3]=f(1,x[0],x[1]+hh*K[1][2],mv);
	for(int i=0;i<2;i++)
	{
		x[i]=x[i]+(K[i][0]+2*K[i][1]+2*K[i][2]+K[i][3])*hh/6;
	}
	return 2*x[0];
}