C语言实现PID与卡尔曼滤波

位置式PID实现

代码如下：

typedef struct
{
	float Kp;
	float Ki;
	float Kd;
	float err0;
	float err1;
	float err2;
	float last_err;
	float longlast_err;
	float pid_num;
	float last_pid_num;
	
	float LastNumber;
	float NewNumber;	
}pidNum_T;

pidNum_T pid;

float pidFunction(float err)  
{
	pid.err0 = err;
	pid.err1 = pid.err0 - pid.last_err;
	pid.pid_num = pid.Kp*pid.err0 + pid.Kd*pid.err1 + pid.Ki*pid.last_pid_num;
	
	pid.last_pid_num += pid.err0;
	pid.last_err = pid.err0;
	
	return pid.pid_num;
}

---

卡尔曼滤波

代码如下：

double KalmanFilter(double Data, double Q, double R)
{
	static double x_last = 0;
	static double p_last = 0;
	double x_mid;
	double x_now;
	double p_mid;
	double p_now;
	double kg;

	//x_last=x(k-1|k-1),x_mid=x(k|k-1)预测结果
	x_mid = x_last; //对应下图公式(1)
	//p_mid=p(k|k-1)预测误差,p_last=p(k-1|k-1),Q=过程噪声
	p_mid = p_last + Q; //对应下图公式(2)
	//kg为kalman 增益，R为测量噪声
	kg = p_mid / (p_mid + R); //对应下图公式(3)
	//估计出的最优值
	x_now = x_mid + kg * (Data - x_mid);//对应下图公式(4)
	//最优值对应的均方误差  
	p_now = (1 - kg) * p_mid;//对应下图公式(5)

	p_last = p_now; //更新均方误差
	x_last = x_now; //更新系统状态值

	return x_now;
}

---

卡尔曼公式如下：