-
Notifications
You must be signed in to change notification settings - Fork 0
/
localization.m
111 lines (88 loc) · 3.49 KB
/
localization.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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
function result = localization()
% Step 0: Data load
clear;
rate = 100; % Sampling rate = 100 Hz
deltaT = 1/100;
filename = 'sensor_data.xls';
% A reference vector used for showing orientations
ref.v = [0, 1, 0];
ref.q = quaternion(0, ref.v(1), ref.v(2), ref.v(3));
% Read a gyro and accel data from an excel file
gyro = xlsread(filename, 'Gyroscope');
gyro(:, 1) = []; % The first column in the data is a timestamp
accel = xlsread(filename, 'Linear Accelerometer');
accel(:, 1) = [];
% Step 1: Orientation relative to a previous body frame
orientation = struct();
for i = 1:length(gyro)
% Estimate a rotation vector from the gyroscrope reading
theta = deltaT * norm(gyro(i, :));
v = gyro(i, :) / norm(gyro(i, :));
% Estimate a rotation quaternion from the rotation vector
v = sin(theta/2) * v;
orientation(i).q = quaternion(cos(theta/2), v(1), v(2), v(3));
end
% Rotate the reference vector to the previous body frame
rotV = zeros(length(orientation), 3);
for i = 1:length(orientation)
q = orientation(i).q * ref.q * quatinv(orientation(i).q);
v = q.compact;
rotV(i, :) = v(2:4);
end
%plot3(rotV(:, 1), rotV(:, 2), rotV(:, 3), 'o')
%Step 2: Orienation relative to an initial body frame
for i = 1:length(orientation)
if i == 1
orientation(i).q2i = orientation(i).q;
else
% Integrate quaternions
orientation(i).q2i = orientation(i - 1).q2i * orientation(i).q;
end
end
% Rotate the reference vector to the initial body frame
rotV = zeros(length(orientation), 3);
for i = 1:length(orientation)
q = orientation(i).q2i * ref.q * quatinv(orientation(i).q2i);
v = q.compact;
rotV(i, :) = v(2:4);
% rotV(i, :) = quatrotate(quatinv(orientation(i).q2i), ref.v);
end
%plot3(rotV(:, 1), rotV(:, 2), rotV(:, 3), 'o')
%Step 3: Gesture tracking using accel
velocity = zeros(length(accel), 3);
position = zeros(size(velocity));
for i = 1:length(accel)
if i == 1
velocity(i, :) = accel(i, :) * deltaT;
position(i, :) = 1 / 2 * accel(i, :) * deltaT^2;
else
velocity(i, :) = velocity(i - 1, :) + accel(i, :) * deltaT;
position(i, :) = position(i - 1, :) + velocity(i - 1, :) * deltaT + 1 / 2 * accel(i, :) * deltaT^2;
end
end
%plot3(position(:, 1), position(:, 2), position(:, 3))
%Step 4: Gesture tracking using accel & gyro
velocity = zeros(length(accel), 3);
position = zeros(size(velocity));
for i = 1:length(accel)
if i == 1
velocity(i, :) = accel(i, :) * deltaT;
position(i, :) = 1 / 2 * accel(i, :) * deltaT^2;
else
q = quaternion(0, accel(i, 1), accel(i, 2), accel(i, 3));
rotA = orientation(i - 1).q2i * q * quatinv(orientation(i - 1).q2i);
rotA = rotA.compact;
rotA = rotA(2:4);
velocity(i, :) = velocity(i - 1, :) + rotA * deltaT;
position(i, :) = position(i - 1, :) + velocity(i - 1, :) * deltaT + 1 / 2 * rotA * deltaT^2;
end
end
%plot3(position(:, 1), position(:, 2), position(:, 3))
resultX = position(i, 1) - position(1, 1);
resultY = position(i, 2) - position(1, 2);
resultZ = position(i, 3) - position(1, 3);
result = [resultX, resultY, resultZ];
%disp(resultX)
%disp(resultY)
%disp(resultZ)
end