%**************************************************************************
%*** function : Walcott-Zak 鲁棒观测器
%**************************************************************************
clear
clc
x=[0.5;0.8;-1]; %初始状态
x_e=[0.2;-0.3;0.5];%初始状态
A=[-2 -1 0;
0 -3 -1;
0 0 -4];
B=[0 0;
3 0;
0 5];
C=[-1 0 1;
0 1 0];
n=1;
t=0;
Dt=0.001;
for i=1:5000
u=zeros(2,1);
Dx=A*x+B*u+B*[2*sin(2*pi*t);2*cos(2*pi*t)];%非线性系统
x=x+Dx*Dt;
%观测器设计
G=zeros(3,2);
F=[0 6;5 0];
e=x_e-x;
F*C*e
if norm(F*C*e)~=0
v=-2*F*C*e/norm(F*C*e);
else
v=0;
end
Dx_e=A*x_e+B*u-G*(C*x_e-C*x)+B*v;%鲁棒观测器
x_e=x_e+Dx_e*Dt;
x_store(:,n)=[x;x_e];
e_store(:,n)=e;
v_store(:,n)=v;
t=t+Dt;
n=n+1;
end
figure(1)
plot((1:n-1)*Dt,x_store(1,:),(1:n-1)*Dt,x_store(4,:))
legend('x1','x1 的估计值')
xlabel('time/s')
ylabel('x1')
title('观测器x1')
figure(2)
plot((1:n-1)*Dt,x_store(2,:),(1:n-1)*Dt,x_store(5,:))
legend('x2','x2 的估计值')
xlabel('time/s')
ylabel('x2')
title('观测器x2')
figure(3)
plot((1:n-1)*Dt,x_store(3,:),(1:n-1)*Dt,x_store(6,:))
legend('x3','x3 的估计值')
xlabel('time/s')
ylabel('x3')
title('观测器x3')
figure(4)
plot((1:n-1)*Dt,e_store(1,:),(1:n-1)*Dt,e_store(2,:),(1:n-1)*Dt,e_store(3,:))
legend('e1','e2','e3')
xlabel('time/s')
ylabel('e')
title('观测器误差e')
figure(5)
plot((1:n-1)*Dt,v_store(1,:),(1:n-1)*Dt,v_store(2,:))
legend('v1','v2')
xlabel('time/s')
ylabel('v')
title('观测器输入v')
评论1