简单的PID代码C++

class PID
{
public:
	double p_error;
	double i_error;
	double d_error;

	double Kp;
	double Ki;
	double Kd;

	PID()
		: p_error(0),
		i_error(0),
		d_error(0),
		Kp(0),
		Ki(0),
		Kd(0),
		has_prev_cte(0),
		prev_cte(0)
	{
	}

	void Init(double Kp, double Ki, double Kd)
	{
		Kp = Kp;
		Ki = Ki;
		Kd = Kd;

		p_error = 0;
		i_error = 0;
		d_error = 0;

		prev_cte = 0;
		has_prev_cte = false;
	}

	void UpdateError(double cte)
	{
		const int threshold = 1000;

		if (i_error > threshold) //if the accumulation is too large, clean it to 0
		{
			i_error = 0;
		}

		if (!has_prev_cte) //determine if there was an input last time
		{
			
			prev_cte = cte;
			has_prev_cte = true;
		}


		p_error = cte;
		i_error = i_error + cte;
		d_error = cte - prev_cte;

		prev_cte = cte;
	}

	double TotalError()
	{
		return -Kp * p_error - Ki * i_error - Kd * d_error;
	}

	bool has_prev_cte;
	double prev_cte;
};

需要注意的是，积分误差如果累计过大会对控制器的效果造成不利影响，因此要根据实际情况，对阈值进行设置，以及要注意pid控制器的输出频率，不应太大，0.0几秒比较好。

对PID的参数调整，一般来讲，先对比例系数Kp进行设置，其余为0，等到结果出现振荡再调节后两个。Ki调节系统的固有误差，一般值会小几个数量级，如果到稳态很慢，可以加。Kd用于防止超调，防止振荡，如果越过稳态时，应该加Kd。