卡尔曼滤波器的PID控制

%Discrete Kalman filter for PID control
%Reference kalman_2rank.m
%x=Ax+B（u+w（k））;
%y=Cx+D+v（k）
clear all;
close all;

ts=0.001;
%Continuous Plant
a=25;b=133;
sys=tf（b[1a0]）;
dsys=c2d（systs‘z‘）;
[numden]=tfdata（dsys‘v‘）;

A1=[0 1;0 -a];
B1=[0;b];
C1=[1 0];
D1=[0];
[ABCD]=c2dm（A1B1C1D1ts‘z‘）;

Q=1;               %Covariances of w
R=1;               %Covariances of v

P=B*Q*B‘;          %Initial error covariance
x=zeros（21）;      %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:1:1000
time（k）=k*ts;

rin（k）=1;
kp=8.0;ki=0.80;kd=0.20;

w（k）=0.002*rands（1）;   %Process noise on u
v（k）=0.002*rands（1）;   %Measurement noise