-
Notifications
You must be signed in to change notification settings - Fork 0
/
HinfinityTest_Mat.m
51 lines (43 loc) · 1.38 KB
/
HinfinityTest_Mat.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
% Plot the results
close all; % Close all open figures
g = 0.01;
duration = 20;
dt = 0.1;
SteadyState = 0;
[pos,vel,poshat, velhat, poshatinf, velhatinf, HinfGains, KalmanGains] = Hinfinity(g, duration, dt, SteadyState);
fprintf('Interpretation from MATLAB');
t = 0 : dt : duration; % Create a time array
% Plot the position estimation error
% (Kalman filter = red line, H-infinity filter = green line)
figure;
plot(t,pos-poshat,'r', t,pos-poshatinf,'b--');
set(gca,'FontSize',12); set(gcf,'Color','White');
grid;
xlabel('Time (sec)');
ylabel('Position Error (feet)');
title('Position Estimation Error');
legend('Kalman filter', 'H_{\infty} filter');
% Plot the velocity estimation error
% (Kalman filter = red line, H-infinity filter = green line)
figure;
plot(t,vel-velhat,'r', t,vel-velhatinf,'b--');
set(gca,'FontSize',12); set(gcf,'Color','White');
grid;
xlabel('Time (sec)');
ylabel('Velocity Error (feet)');
title('Velocity Estimation Error');
legend('Kalman filter', 'H_{\infty} filter');
% Plot the Kalman filter gain matrix
figure;
plot(t,KalmanGains(1,:),'r', t,KalmanGains(2,:),'b--');
set(gca,'FontSize',12); set(gcf,'Color','White');
grid;
xlabel('Time (sec)');
title('Kalman Gains');
% Plot the H-infinity filter gain matrix
figure;
plot(t,HinfGains(1,:),'r', t,HinfGains(2,:),'b--');
set(gca,'FontSize',12); set(gcf,'Color','White');
grid;
xlabel('Time (sec)');
title('H-Infinity Gains');