-
Notifications
You must be signed in to change notification settings - Fork 10
/
NonlinearDO.m
103 lines (72 loc) · 1.5 KB
/
NonlinearDO.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
clear
clc
close all
Ts=1e-3; % Sampling time 1ms
Tsim=20; % Simulation time 20s
t=0:Ts:Tsim-1*Ts;
N=Tsim/Ts;
% All model are continuous model
% Plant matrix without nonlinear element tanh()
A=[0 1
-50 -4.0];
B=[0
50];
C=[1 0];
D=0;
% Disturbance input matrix
Bd=[0
50];
% Initialize of plant state variable
dotx(1:2,1:N)=0;
x(1:2,1:N)=0;
% Observer matrix without nonlinear element tanh()
Aob=[0 1
-50 -4.0]
Bob=[0
50]
Cob=[1 0]
g1=Bob;
g2=Bob;
% Initialize of observer state variable
dotz(1:1,1:N)=0;
z(1:1,1:N)=0;
% Feedback gain L
Lob=[0 5]*0.02
% Estimated disturbance
dhat(1:1,1:N)=0;
% Difinition of plant input at 5s
u(1:N/4) = 0;
u(N/4+1:N) = 1;
% Difinition of disturbance input at 10s
d(1:N/2) = 0;
d(N/2+1:N) = 1*1;
% Simulation
for i=1:N-1
% Caluculation of observer and estimation of disturbance
fx=A*x(:,i)+[0;-5*tanh(2*x(2,i))];
px=Lob*x(:,i);
dotz(1,i+1)=-Lob*g2*z(:,i)-Lob*(g2*px+fx+g1*u(i));
z(:,i+1)=z(:,i)+dotz(:,i+1)*Ts;
dhat(:,i+1) = z(:,i+1)+px;
% Caluculation of plant
dotx(:,i+1)=A*x(:,i)+[0;-5*tanh(2*x(2,i))]+B*u(i)+Bd*d(i);
x(:,i+1)=x(:,i)+dotx(:,i+1)*Ts;
end
% Plot data
figure
hold on
grid on
plot(t,x(1,:)) % position
xlabel('Time [s])')
ylabel('position')
title('Position');
figure
hold on
grid on
plot(t,d) % Actual disturbance
plot(t,dhat) % Estimated disturbance
xlabel('Time [s])')
ylabel('input')
title('Disturbance estimation');
legend('Actual disturbance', 'Estimated disturbance')
ylim([-0.2 1.2])