-
Notifications
You must be signed in to change notification settings - Fork 93
/
plotekf.m
53 lines (47 loc) · 1.97 KB
/
plotekf.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
load /tmp/odom_file.txt;
%load vo_file.txt;
load /tmp/corr_file.txt;
load /tmp/gps_file.txt;
load /tmp/imu_file.txt;
figure;
hold on;
axis equal;
plot(odom_file(:,2), odom_file(:,3),'b');
%plot(vo_file(:,2),vo_file(:,3),'g');
plot(corr_file(:,2), corr_file(:,3),'r');
plot(gps_file(:,2), gps_file(:,3),'k');
legend('Wheel Odometry', 'Filter output', 'GPS Measurements');
%legend('Wheel Odometry','Visual Odometry', 'Filter output', 'GPS Measurements');
%legend('Wheel Odometry','Visual Odometry', 'Filter output');
hold off;
figure;
subplot(3,1,1)
hold on;
plot(odom_file(:,1),odom_file(:,2),'b');
%plot(vo_file(:,1),vo_file(:,2),'g');
plot(corr_file(:,1),corr_file(:,2),'r');
plot(gps_file(:,1), gps_file(:,2), 'k');
legend('Wheel Odometry', 'Filter output', 'GPS Measurements');
%legend('Wheel Odometry','Visual Odometry', 'Filter output', 'GPS Measurements');
%legend('Wheel Odometry','Visual Odometry', 'Filter output');
subplot(3,1,2)
hold on;
plot(odom_file(:,1),odom_file(:,3),'b');
%plot(vo_file(:,1),vo_file(:,3),'g');
plot(corr_file(:,1),corr_file(:,3),'r');
plot(gps_file(:,1), gps_file(:,3), 'k');
legend('Wheel Odometry', 'Filter output', 'GPS Measurements');
%legend('Wheel Odometry','Visual Odometry', 'Filter output', 'GPS Measurements');
%legend('Wheel Odometry','Visual Odometry', 'Filter output');
subplot(3,1,3)
hold on;
plot(odom_file(:,1),odom_file(:,4),'b');
%plot(vo_file(:,1),vo_file(:,7),'g');
plot(corr_file(:,1),corr_file(:,7),'r');
plot(imu_file(:,1), imu_file(:,2), 'k');
legend('Wheel Odometry', 'Filter output', 'IMU Measurements');
%legend('Wheel Odometry','Visual Odometry', 'Filter output', 'IMU Measurements');
error_odom = sqrt( (odom_file(1,2)-odom_file(end,2))^2 + (odom_file(1,3)-odom_file(end,3))^2 )
%error_vo = sqrt( (vo_file(1,2)-vo_file(end,2))^2 + (vo_file(1,3)-vo_file(end,3))^2 )
error_corr = sqrt( (corr_file(1,2)-corr_file(end,2))^2 + (corr_file(1,3)-corr_file(end,3))^2 )
error_gps = sqrt( (gps_file(1,2)-gps_file(end,2))^2 + (gps_file(1,3)-gps_file(end,3))^2 )