clear all;close all;clc;

target_speed = [5*ones(300,1);ones(300,1);ones(300,1)*10;ones(300,1)*2];
real_speed = zeros(length(target_speed),1);

Kp = 0.2;Ki = 0.1;Kd = 0.1;

intergral = 0;
pre_err = 0;
for i=2:length(target_speed)
    
    err = target_speed(i) - real_speed(i-1);
    intergral = intergral + err;
    u = Kp*err + Ki*intergral + Kd*(err - pre_err);
    pre_err = err;
    
    real_speed(i) = u;　　　　%简化一下，控制量直接转为状态量了
end

plot(target_speed,'b')
hold on;
plot(real_speed,'r')
legend('目标','实际')