1.算法描述
PID控制器，即控制器的控制方式为P比例调整，I积分调整以及D微分调整三个部分构成，PID控制器是目前为止应用最为广泛的控制方式。PID控制器具有结构简单，性能稳定，参数设置简单等优势。PID控制器适用于各种控制对象无法进行测量获得系统参数的情况，其根据控制对象的输出和参考控制变量的输入差进行实时的调整实现对未知参数控制对象的有效控制。PID控制器由比例调整模块，积分调整模块以及微分调整模块三个部分构成，那么其输入的误差信号e(t)与输出u(t)的关系为公式6：
公式中，参数kp表示的是比例调整模块的系数，其作用是将系统的反馈误差e(t)根据参数 kp进行调节，使得调整后的控制对象的反馈误差减小。当比例系数kp取较大值的时候，那么控制器的调整速度较快，但是其抖动也较大，从而导致系统了不稳定性。当比例系数kp取较小值的时候，那么控制器调整速度较慢，但调整过程较为稳定。
参数ki表示的是积分调整模块的系数，其作用将消除系统中存在的稳态误差，当存在稳态误差的时候，通过积分调节模块进行调节，直到完全消除稳态误差为止，之后积分调节功能停止工作，积分调节模块输出一个固定值。当积分调制系数ki较小的时候，积分调节作用越强，反之，积分调节作用较弱，系统调整速度较慢。
参数kd表示的是微分调整模块的系数，其主要是对系统反馈误差的变化率进行调整，其具有超前调整功能，可以预测系统反馈误差的变化率，因此可以在反馈误差产生之间将误差消除，因此通过设置微分调整参数，可以降低PID控制器的超调量，并加快系统的调整速度。————————————————
卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
数据滤波是去除噪声还原真实数据的一种数据处理技术，Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。
状态估计
状态估计是卡尔曼滤波的重要组成部分。一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功能。比如对飞行器状态估计。状态估计对于了解和控制一个系统具有重要意义，所应用的方法属于统计学中的估计理论。最常用的是最小二乘估计，线性最小方差估计、最小方差估计、递推最小二乘估计等。其他如风险准则的贝叶斯估计、最大似然估计、随机逼近等方法也都有应用。状态量
受噪声干扰的状态量是个随机量，不可能测得精确值，但可对它进行一系列观测，并依据一组观测值，按某种统计观点对它进行估计。使估计值尽可能准确地接近真实值，这就是最优估计。真实值与估计值之差称为估计误差。若估计值的数学期望与真实值相等，这种估计称为无偏估计。卡尔曼提出的递推最优估计理论，采用状态空间描述法，在算法采用递推形式，卡尔曼滤波能处理多维和非平稳的随机过程。理论
卡尔曼滤波理论的提出，克服了威纳滤波理论的局限性使其在工程上得到了广泛的应用，尤其在控制、制导、导航、通讯等现代工程方面。
2.仿真效果预览
matlab2022a仿真结果如下：
第一个图不加滤波器
第二个图为加滤波器
3.MATLAB核心程序
B1=[0;b];
C1=[1 0];
D1=[0];
[A,B,C,D]=c2dm(A1,B1,C1,D1,ts,'z');
Q=1; %Covariances of w
R=1; %Covariances of v
P=B*Q*B'; %Initial error covariance
x=zeros(2,1); %Initial condition on the state
u_1=0;u_2=0;
y_1=0;y_2=0;
ei=0;
error_1=0;
for k=1:N
time(k)=k*ts;
rin(k)=1;
kp=8.0;ki=0.80;kd=0.20;
......................................................................
x=A*x+Mn*(yv(k)-C*A*x);
ye(k)=C*x+D; %Filtered value
if M==1 %Not using filter
yout(k)=yv(k);
end
if M==2 %Using filter
yout(k)=ye(k);
end
error(k)=rin(k)-yout(k);
ei=ei+error(k)*ts;
u(k)=kp*error(k)+ki*ei+kd*(error(k)-error_1)/ts; %PID
u(k)=u(k)+w(k);