-
Notifications
You must be signed in to change notification settings - Fork 3
/
main.m
124 lines (106 loc) · 3.27 KB
/
main.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
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
%% Toy example link : https://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
clear
clc
%% Initialization
Ts = 0.1; % Sampling Time
time=0:Ts:100;
N = length(time)-1;
% For Plant
x0 = [0;0]; % Position, Velocity
x(:,1) = x0;
% For Kalman Filter
sigma_a = 0.1;
sigma_y = 0.01;
mu=0;
x_hat(:,1) = x0;
P_k{1} = 0*eye(2,2);
P_k_norm(1) = norm(P_k{1});
% Velocity Profile
Velocity(1:20) = 0; % m/s
Velocity(20:100) = 0.5;
Velocity(100:300) = 1.2;
Velocity(300:500) = 1;
Velocity(500:700) = 0;
Velocity(700:800) = 0.7;
Velocity(800:1001) = 0;
%% Plant Model
F = [1 Ts; 0 1];
G = [Ts^2/2; Ts];
H = [1 0]; % we have only position's measurement
% Check System Controllability
if(rank(ctrb(F,G))==2)
disp('System is controllable !')
end
% Check System Observability
if(rank(obsv(F,H))==2)
disp('System is observable !')
end
%% PID Controller
Kp = 0.8;
Ki = 1e-3;
[Kp,Ki]= mydialog;
%% Velocity Estimate using Kalman Filter
Q = G*G'*sigma_a^2;
R = sigma_y^2;
noise = sigma_y*randn(1,N+1)+mu; % Generate White Noise
Error = zeros(1,length(N+1));
Prop = zeros(1,length(N+1));
Integral = zeros(1,length(N+1));
for t=1:N
% PID Controller
Error(t+1) = Velocity(t)-x_hat(2,t);
Prop(t+1) = Error(t+1);
Int(t+1)=Error(t+1)*Ts;
Integral(t+1)= sum(Int);
a(t+1) = Kp*Prop(t+1)+ Ki*Integral(t+1);
% Plant Open Loop Simulation
x(:,t+1) = F*x(:,t) + G*a(t+1);
y(t+1) = H*x(:,t+1)+ noise(t+1); % Add noise to the measurement
% Estimate the Velocity using Kalman Filter
[x_up, P_k{t+1}] = Kalman_Filter(x_hat(:,t), P_k{t}, y(t), F, H, Q, R);
x_hat(:,t+1) = x_up;
end
%% Plot Data
maxfig
subplot(2,1,1)
plot(time,x(1,:),'-',time,x_hat(1,:),'--',time, y,'--','Linewidth',2)
xlabel('Time (s)','fontweight','bold')
ylabel('Distance (m)','fontweight','bold')
grid on
legend('True Position','Estimated Position','Measurement','Location','northwest')
title('Estimated vs Real Position', 'FontSize', 14)
axes('position',[.68 .64 .2 .2])
box on
your_index = 58<time & time<65;
plot(time(your_index),x(1,your_index),time(your_index),x_hat(1,your_index),time(your_index),y(your_index),'Linewidth',2)
axis tight
subplot(2,1,2)
plot(time,x(1,:)-x_hat(1,:),time,x_hat(1,:)-y,'Linewidth',1.5)
legend('True Position','Measurement','Location','northwest')
xlabel('Time (s)','fontweight','bold')
ylabel('Distance (m)','fontweight','bold')
grid on
title('Error', 'FontSize', 14)
saveas(gcf,'Position.png')
maxfig
subplot(2,1,1)
plot(time,Velocity,time,x(2,:),time,x_hat(2,:),'Linewidth',2)
xlabel('Time (s)','fontweight','bold')
ylabel('Velocity (m/s)','fontweight','bold')
legend('Velocity Profile','True Velocity','Estimated Velocity','Location','northeast')
grid on
title('Estimated vs Real Velocity ', 'FontSize', 14)
subplot(2,1,2)
plot(time,Velocity-x(2,:),'Linewidth',1.5)
xlabel('Time (s)','fontweight','bold')
ylabel('Velocity (m/s)','fontweight','bold')
grid on
title('Error', 'FontSize', 14)
saveas(gcf,'Velocity.png')
maxfig
plot(time,a,'Linewidth',2)
xlabel('Time (s)','fontweight','bold')
ylabel('Acceleration (m/s^2)','fontweight','bold')
grid on
title('Acceleration - Controller Output', 'FontSize', 14)
saveas(gcf,'Acceleration.png')