-
Notifications
You must be signed in to change notification settings - Fork 2
/
gt3.m
55 lines (49 loc) · 1.86 KB
/
gt3.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
function [p,v,a,eul,eul_rate,t,freq] = gt3()
freq = 100; % Groundtruth simulation frequency in Hz.
sim_step = 1/freq;
start_time = 0;
% First: static case at start up, 0-5s
end_time = 5;
t = (start_time+sim_step: sim_step: end_time)';
p = repmat([0 0 0],length(t),1);
v = repmat([0 0 0],length(t),1);
a = repmat([0 0 0],length(t),1);
eul = repmat([0 0 0],length(t),1);
eul_rate = repmat([0 0 0],length(t),1);
% Second: no acceleration, orientation change with time, 5~60s
end_time = 50;
t_i = (start_time+sim_step : sim_step : end_time)';
p_i = [1.2*t_i, t_i 0.1*t_i];
v_i = repmat([1.2, 1, 0.1], length(t_i),1);
a_i = repmat([0, 0, 0], length(t_i),1);
eul_i = [pi/8*cos(0.2*t_i)-pi/8*cos(0.2*t_i(1)), ...
pi/12*cos(0.2*t_i)-pi/12*cos(0.2*t_i(1)), ...
pi/6*cos(0.1*t_i)-pi/6*cos(0.1*t_i(1))];
eul_rate_i = [-0.2*pi/8*sin(0.2*t_i), ...
-0.2*pi/12*sin(0.2*t_i), ...
-0.1*pi/6*sin(0.1*t_i)];
t = [t; t(end,:) + t_i];
p = [p; p(end,:) + (v(end,:) + a(end,:).*t_i).*t_i + p_i];
v = [v; v(end,:) + a(end,:).*t_i + v_i];
a = [a; a(end,:) + a_i];
eul = [eul; eul(end,:) + eul_rate(end,:).*t_i + eul_i];
eul_rate = [eul_rate; eul_rate(end,:) + eul_rate_i];
% Third: with acceleration
end_time = 10;
t_i = (start_time+sim_step : sim_step : end_time)';
p_i = [0.002, 0.04, -0.05].*t_i.*t_i.*t_i;
v_i = [0.006, 0.12, -0.15].*t_i.*t_i;
a_i = [0.012, 0.24, -0.3].*t_i;
eul_i = [pi/8*cos(0.2*t_i)-pi/8*cos(0.2*t_i(1)), ...
pi/12*cos(0.2*t_i)-pi/12*cos(0.2*t_i(1)), ...
pi/6*cos(0.1*t_i)-pi/6*cos(0.1*t_i(1))];
eul_rate_i = [-0.2*pi/8*sin(0.2*t_i), ...
-0.2*pi/12*sin(0.2*t_i), ...
-0.1*pi/6*sin(0.1*t_i)];
t = [t; t(end,:) + t_i];
p = [p; p(end,:) + (v(end,:) + a(end,:).*t_i).*t_i + p_i];
v = [v; v(end,:) + a(end,:).*t_i + v_i];
a = [a; a(end,:) + a_i];
eul = [eul; eul(end,:) + eul_rate(end,:).*t_i + eul_i];
eul_rate = [eul_rate; eul_rate(end,:) + eul_rate_i];
end