forked from zarathustr/ARRB
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathproposed.m
82 lines (70 loc) · 3.34 KB
/
proposed.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
% Computationally Efficient Robust Algorithm for
% Generalized Sensor Calibration Problem AR=RB
%
% Author: Jin Wu and Ming Liu
% e-mail: [email protected]
%
% Reference: Wu, J., Liu, M. (2018) Computationally Efficient Robust
% Algorithm for Generalized Sensor Calibration
% Problem AR=RB. Submitted to IEEE Sensors J.
%
% (c) 2018 Jin Wu
function R = proposed(A, B)
K = zeros(4, 4);
len = length(A);
for j = 1 : len
Ki = Ks(A(:, :, j), B(:, :, j));
K = K + 1 / len * Ki;
end
[V, D] = eig(K);
q_proposed = V(:, 1);
R = quat2dcm(q_proposed');
end
function Ki = Ks(A, B)
K11 = [
A(1, 1) - B(1, 1), 0, A(1, 3) + B(3, 1), - A(1, 2) - B(2, 1);
0, A(1, 1) - B(1, 1), A(1, 2) - B(2, 1), A(1, 3) - B(3, 1);
A(1, 3) + B(3, 1), A(1, 2) - B(2, 1), B(1, 1) - A(1, 1), 0;
- A(1, 2) - B(2, 1), A(1, 3) - B(3, 1), 0, B(1, 1) - A(1, 1)];
K22 = [
A(2, 2) - B(2, 2), - A(2, 3) - B(3, 2), 0, A(2, 1) + B(1, 2);
- A(2, 3) - B(3, 2), B(2, 2) - A(2, 2), A(2, 1) - B(1, 2), 0;
0, A(2, 1) - B(1, 2), A(2, 2) - B(2, 2), A(2, 3) - B(3, 2);
A(2, 1) + B(1, 2), 0, A(2, 3) - B(3, 2), B(2, 2) - A(2, 2)];
K33 = [
A(3, 3) - B(3, 3), A(3, 2) + B(2, 3), - A(3, 1) - B(1, 3), 0;
A(3, 2) + B(2, 3), B(3, 3) - A(3, 3), 0, A(3, 1) - B(1, 3);
- A(3, 1) - B(1, 3), 0, B(3, 3) - A(3, 3), A(3, 2) - B(2, 3);
0, A(3, 1) - B(1, 3), A(3, 2) - B(2, 3), A(3, 3) - B(3, 3)];
K12 = [
A(2, 1) - B(2, 1), -B(3, 1), A(2, 3), B(1, 1) - A(2, 2);
-B(3, 1), A(2, 1) + B(2, 1), A(2, 2) - B(1, 1), A(2, 3);
A(2, 3), A(2, 2) - B(1, 1), - A(2, 1) - B(2, 1), -B(3, 1);
B(1, 1) - A(2, 2), A(2, 3), -B(3, 1), B(2, 1) - A(2, 1)];
K13 = [
A(3, 1) - B(3, 1), B(2, 1), A(3, 3) - B(1, 1), -A(3, 2);
B(2, 1), A(3, 1) + B(3, 1), A(3, 2), A(3, 3) - B(1, 1);
A(3, 3) - B(1, 1), A(3, 2), B(3, 1) - A(3, 1), -B(2, 1);
-A(3, 2), A(3, 3) - B(1, 1), -B(2, 1), - A(3, 1) - B(3, 1)];
K21 = [
A(1, 2) - B(1, 2), -A(1, 3), B(3, 2), A(1, 1) - B(2, 2);
-A(1, 3), - A(1, 2) - B(1, 2), A(1, 1) - B(2, 2), -B(3, 2);
B(3, 2), A(1, 1) - B(2, 2), A(1, 2) + B(1, 2), A(1, 3);
A(1, 1) - B(2, 2), -B(3, 2), A(1, 3), B(1, 2) - A(1, 2)];
K23 = [
A(3, 2) - B(3, 2), B(2, 2) - A(3, 3), -B(1, 2), A(3, 1);
B(2, 2) - A(3, 3), B(3, 2) - A(3, 2), A(3, 1), -B(1, 2);
-B(1, 2), A(3, 1), A(3, 2) + B(3, 2), A(3, 3) - B(2, 2);
A(3, 1), -B(1, 2), A(3, 3) - B(2, 2), - A(3, 2) - B(3, 2)];
K31 = [
A(1, 3) - B(1, 3), A(1, 2), B(3, 3) - A(1, 1), -B(2, 3);
A(1, 2), - A(1, 3) - B(1, 3), -B(2, 3), A(1, 1) - B(3, 3);
B(3, 3) - A(1, 1), -B(2, 3), B(1, 3) - A(1, 3), A(1, 2);
-B(2, 3), A(1, 1) - B(3, 3), A(1, 2), A(1, 3) + B(1, 3)];
K32 = [
A(2, 3) - B(2, 3), A(2, 2) - B(3, 3), -A(2, 1), B(1, 3);
A(2, 2) - B(3, 3), B(2, 3) - A(2, 3), -B(1, 3), A(2, 1);
-A(2, 1), -B(1, 3), - A(2, 3) - B(2, 3), A(2, 2) - B(3, 3);
B(1, 3), A(2, 1), A(2, 2) - B(3, 3), A(2, 3) + B(2, 3)];
Ki = K11^2 + K12^2 + K13^2 + K21^2 + K22^2 + K23^2 + K31^2 + K32^2 + K33^2;
end