-
Notifications
You must be signed in to change notification settings - Fork 0
/
sensor_test.m
62 lines (50 loc) · 1.55 KB
/
sensor_test.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
clear all;
addpath ./sequences
addpath ./sensor_model
seq = readtable('./sequences/seq1.txt', 'ReadVariableNames', true, 'HeaderLines',0);
imu = timeseries([seq.wx seq.wy seq.wz seq.ax seq.ay seq.az seq.qw seq.qx seq.qy seq.qz], seq.time);
gps = timeseries([seq.px seq.py seq.pz seq.vx seq.vy seq.vz seq.qw seq.qx seq.qy seq.qz], seq.time);
quat = timeseries([seq.qw seq.qx seq.wy seq.qz], seq.time);
lidar= timeseries([seq.px seq.py seq.pz seq.qw seq.qx seq.qy seq.qz], seq.time);
sensor_model;
imu_sampled = sensor_imu(imu, sensor_config.imu);
gps_sampled = sensor_gps(gps, sensor_config.gps);
compass_sampled = sensor_compass(quat, sensor_config.compass);
lidar_sampled = sensor_lidar(lidar, sensor_config.lidar);
figure('Name', 'imu');
subplot(1,2,1);
title('wx');
hold on;
grid on;
plot(imu.Time, imu.Data(:,1), 'g');
plot(imu_sampled.Time, imu_sampled.Data(:,1), 'r');
subplot(1,2,2);
title('ax');
hold on;
grid on;
plot(imu.Time, imu.Data(:,4), 'g');
plot(imu_sampled.Time, imu_sampled.Data(:,4), 'r');
figure('Name', 'gps');
subplot(1,2,1);
title('px');
hold on;
grid on;
plot(gps.Time, gps.Data(:,1), 'g');
plot(gps_sampled.Time, gps_sampled.Data(:,1), 'r');
subplot(1,2,2);
title('vx');
hold on;
grid on;
plot(gps.Time, gps.Data(:,4), 'g');
plot(gps_sampled.Time, gps_sampled.Data(:,4), 'r');
figure('Name', 'compass');
title('magx');
hold on;
grid on;
plot(compass_sampled.Time, compass_sampled.Data(:,1), 'r');
figure('Name', 'lidar');
title('quat.w');
hold on;
grid on;
plot(lidar.Time, lidar.Data(:,1), 'g');
plot(lidar_sampled.Time, lidar_sampled.Data(:,1), 'r');