-
Notifications
You must be signed in to change notification settings - Fork 2
/
gt4.m
93 lines (83 loc) · 3.08 KB
/
gt4.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
function [p,v,a,eul,eul_rate,t,freq] = gt4()
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_i = (start_time+sim_step: sim_step: end_time)';
p_i = repmat([0 0 0],length(t_i),1);
v_i = repmat([0 0 0],length(t_i),1);
a_i = repmat([0 0 0],length(t_i),1);
eul_i = repmat([0 0 0],length(t_i),1);
eul_rate_i = repmat([0 0 0],length(t_i),1);
t = t_i;
p = p_i;
v = v_i;
a = a_i;
eul = eul_i;
eul_rate = eul_rate_i;
% 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,:).*t_i + 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,:).*t_i + 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];
% Fourth: turning into zero acceleration and zero euler_rate
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 = -eul_rate(end,:)/end_time.*t_i;
eul_rate_i = repmat(-eul_rate(end,:)/end_time,length(t_i),1);
t = [t; t(end,:) + t_i];
p = [p; p(end,:) + v(end,:).*t_i + 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];
% 5th: static
end_time = 20;
t_i = (start_time+sim_step: sim_step: end_time)';
p_i = repmat([0 0 0],length(t_i),1);
v_i = repmat([0 0 0],length(t_i),1);
a_i = repmat([0 0 0],length(t_i),1);
eul_i = repmat([0 0 0],length(t_i),1);
eul_rate_i = repmat([0 0 0],length(t_i),1);
t = [t; t(end,:) + t_i];
p = [p; p(end,:) + v(end,:).*t_i + 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