-
Notifications
You must be signed in to change notification settings - Fork 0
/
HandIdentificationOLD.m
179 lines (131 loc) · 140 KB
/
HandIdentificationOLD.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
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
clear
close all
clc
tic
%mainCalibration
directory = 'datasets/';
file_id = 'kio2'; %Index & Ring Calib Data
dat_name = [directory file_id];
generic_rigid_body_visar %rotate all
bestgeopar = load('HandPar.mat');
bestgeopar = bestgeopar.bestgeopar;
frame = load(dat_name);
frame = frame.frame;
measData = frame(:,2:61)';
NTOT = size(measData,2);
timeEst = frame(:,1);
% Declare a matrix that will contain all estimed angles
stateEstall = [];
% Declare a matrix that will contain all marker components obtained by estimated angles
mdtio = [];
% Estimate angles of each finger
for d=1:5
% Initialize joint angles vector
q = zeros(4,1);
% Define the measurement covariance
R_OBS = 0.0005*eye(9);
% Initialize the innovation vector
innovation = zeros(9,1);
% Initialize the innovation variance vector (69x69)
varInnov = zeros(9,9);
% Define P
P = 0.001*eye(4);
processNoise = eye(4);
% Declare a matrix that will contain all estimed angles
stateEst = [];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mi2x = bestgeopar(d,1);
mi2y = bestgeopar(d,2);
mi2z = bestgeopar(d,3);
mi3x = bestgeopar(d,4);
mi3y = bestgeopar(d,5);
mi3z = bestgeopar(d,6);
mi4x = bestgeopar(d,7);
mi4y = bestgeopar(d,8);
mi4z = bestgeopar(d,9);
a2 = bestgeopar(d,10);
a3 = bestgeopar(d,11);
a4 = 0.027; % not influent for the result
ofx = bestgeopar(d,12);
ofy = bestgeopar(d,13);
ofz = bestgeopar(d,14);
x = zeros(4,1);
for i = 1:NTOT
% Add the process noise (Pk = I*Pk-1*I + Q)
for j = 1:4
P(j,j) = P(j,j) + processNoise(j,j);
end
mi1x=measData(1+12*(d-1),i);
mi1y=measData(2+12*(d-1),i);
mi1z=measData(3+12*(d-1),i);
% Thumb
if d==1
h =[
mi1x + ofx + (conj(mi2y) + (4967757600021511*conj(mi2z))/40564819207303340847894502572032)*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096) - ((4967757600021511*conj(mi2y))/40564819207303340847894502572032 - conj(mi2z))*((4967757600021511*sin(x(1)))/81129638414606681695789005144064 + cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) + conj(mi2x)*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)
mi1y + ofy + ((4967757600021511*conj(mi2y))/40564819207303340847894502572032 - conj(mi2z))*((4967757600021511*cos(x(1)))/81129638414606681695789005144064 + (4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + (conj(mi2y) + (4967757600021511*conj(mi2z))/40564819207303340847894502572032)*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + conj(mi2x)*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))
mi1z + ofz - conj(mi2x)*sin(x(2)) - (cos(x(2)) - 24678615572571482867467662723121/6582018229284824168619876730229402019930943462534319453394436096)*((4967757600021511*conj(mi2y))/40564819207303340847894502572032 - conj(mi2z)) + (conj(mi2y) + (4967757600021511*conj(mi2z))/40564819207303340847894502572032)*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)
mi1x + ofx - conj(mi3x)*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) - ((4967757600021511*conj(mi3y))/40564819207303340847894502572032 - conj(mi3z))*((4967757600021511*sin(x(1)))/81129638414606681695789005144064 + cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) + (conj(mi3y) + (4967757600021511*conj(mi3z))/40564819207303340847894502572032)*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096)) + a2*cos(x(1))*cos(x(2)) - (4967757600021511*a2*sin(x(1))*sin(x(2)))/81129638414606681695789005144064
mi1y + ofy + (sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + cos(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064))*(conj(mi3y) + (4967757600021511*conj(mi3z))/40564819207303340847894502572032) + ((4967757600021511*conj(mi3y))/40564819207303340847894502572032 - conj(mi3z))*((4967757600021511*cos(x(1)))/81129638414606681695789005144064 + (4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) - conj(mi3x)*(sin(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) + (4967757600021511*a2*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + a2*cos(x(2))*sin(x(1))
mi1z + ofz - a2*sin(x(2)) - (conj(mi3y) + (4967757600021511*conj(mi3z))/40564819207303340847894502572032)*(sin(x(2))*sin(x(3)) - cos(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)) - (cos(x(2)) - 24678615572571482867467662723121/6582018229284824168619876730229402019930943462534319453394436096)*((4967757600021511*conj(mi3y))/40564819207303340847894502572032 - conj(mi3z)) - conj(mi3x)*(cos(x(3))*sin(x(2)) + sin(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064))
mi1x + ofx - ((4967757600021511*conj(mi4y))/40564819207303340847894502572032 - conj(mi4z))*((4967757600021511*sin(x(1)))/81129638414606681695789005144064 + cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) + (conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032)*(cos(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096)) - sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064))) - conj(mi4x)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) + sin(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096))) + a3*cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + a2*cos(x(1))*cos(x(2)) - (4967757600021511*a2*sin(x(1))*sin(x(2)))/81129638414606681695789005144064 - a3*sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096)
mi1y + ofy - conj(mi4x)*(cos(x(4))*(sin(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) + sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + cos(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064))) + ((4967757600021511*conj(mi4y))/40564819207303340847894502572032 - conj(mi4z))*((4967757600021511*cos(x(1)))/81129638414606681695789005144064 + (4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + (conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + cos(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) - sin(x(4))*(sin(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))))) + a3*cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + (4967757600021511*a2*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + a2*cos(x(2))*sin(x(1)) - a3*sin(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)
mi1z + ofz - (cos(x(4))*(sin(x(2))*sin(x(3)) - cos(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)) + sin(x(4))*(cos(x(3))*sin(x(2)) + sin(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)))*(conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032) - conj(mi4x)*(cos(x(4))*(cos(x(3))*sin(x(2)) + sin(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)) - sin(x(4))*(sin(x(2))*sin(x(3)) - cos(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064))) - a2*sin(x(2)) - (cos(x(2)) - 24678615572571482867467662723121/6582018229284824168619876730229402019930943462534319453394436096)*((4967757600021511*conj(mi4y))/40564819207303340847894502572032 - conj(mi4z)) - a3*cos(x(3))*sin(x(2)) - a3*sin(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)];
H =[
- ((4967757600021511*conj(mi2y))/40564819207303340847894502572032 - conj(mi2z))*((4967757600021511*cos(x(1)))/81129638414606681695789005144064 + (4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) - (conj(mi2y) + (4967757600021511*conj(mi2z))/40564819207303340847894502572032)*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - conj(mi2x)*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))), ((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - (24678615572571482867467662723121*sin(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096)*(conj(mi2y) + (4967757600021511*conj(mi2z))/40564819207303340847894502572032) - (cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)*((4967757600021511*conj(mi2y))/40564819207303340847894502572032 - conj(mi2z)) - conj(mi2x)*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064), 0, 0
(conj(mi2y) + (4967757600021511*conj(mi2z))/40564819207303340847894502572032)*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096) - ((4967757600021511*conj(mi2y))/40564819207303340847894502572032 - conj(mi2z))*((4967757600021511*sin(x(1)))/81129638414606681695789005144064 + cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) + conj(mi2x)*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064), ((24678615572571482867467662723121*cos(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)*(conj(mi2y) + (4967757600021511*conj(mi2z))/40564819207303340847894502572032) - ((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))*((4967757600021511*conj(mi2y))/40564819207303340847894502572032 - conj(mi2z)) + conj(mi2x)*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))), 0, 0
0, sin(x(2))*((4967757600021511*conj(mi2y))/40564819207303340847894502572032 - conj(mi2z)) - conj(mi2x)*cos(x(2)) - (4967757600021511*sin(x(2))*(conj(mi2y) + (4967757600021511*conj(mi2z))/40564819207303340847894502572032))/81129638414606681695789005144064, 0, 0
conj(mi3x)*(sin(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) - ((4967757600021511*conj(mi3y))/40564819207303340847894502572032 - conj(mi3z))*((4967757600021511*cos(x(1)))/81129638414606681695789005144064 + (4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) - (sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + cos(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064))*(conj(mi3y) + (4967757600021511*conj(mi3z))/40564819207303340847894502572032) - (4967757600021511*a2*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - a2*cos(x(2))*sin(x(1)), - (cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)*((4967757600021511*conj(mi3y))/40564819207303340847894502572032 - conj(mi3z)) - (conj(mi3y) + (4967757600021511*conj(mi3z))/40564819207303340847894502572032)*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - (24678615572571482867467662723121*sin(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096)) - conj(mi3x)*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - (24678615572571482867467662723121*sin(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)) - a2*cos(x(1))*sin(x(2)) - (4967757600021511*a2*cos(x(2))*sin(x(1)))/81129638414606681695789005144064, - conj(mi3x)*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096)) - (conj(mi3y) + (4967757600021511*conj(mi3z))/40564819207303340847894502572032)*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)), 0
(conj(mi3y) + (4967757600021511*conj(mi3z))/40564819207303340847894502572032)*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096)) - ((4967757600021511*conj(mi3y))/40564819207303340847894502572032 - conj(mi3z))*((4967757600021511*sin(x(1)))/81129638414606681695789005144064 + cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - conj(mi3x)*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) + a2*cos(x(1))*cos(x(2)) - (4967757600021511*a2*sin(x(1))*sin(x(2)))/81129638414606681695789005144064, (conj(mi3y) + (4967757600021511*conj(mi3z))/40564819207303340847894502572032)*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((24678615572571482867467662723121*cos(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)) - ((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))*((4967757600021511*conj(mi3y))/40564819207303340847894502572032 - conj(mi3z)) - conj(mi3x)*(sin(x(3))*((24678615572571482867467662723121*cos(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2)))) + (4967757600021511*a2*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - a2*sin(x(1))*sin(x(2)), - (sin(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))))*(conj(mi3y) + (4967757600021511*conj(mi3z))/40564819207303340847894502572032) - conj(mi3x)*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + cos(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)), 0
0, sin(x(2))*((4967757600021511*conj(mi3y))/40564819207303340847894502572032 - conj(mi3z)) - a2*cos(x(2)) - (conj(mi3y) + (4967757600021511*conj(mi3z))/40564819207303340847894502572032)*(cos(x(2))*sin(x(3)) + (4967757600021511*cos(x(3))*sin(x(2)))/81129638414606681695789005144064) - conj(mi3x)*(cos(x(2))*cos(x(3)) - (4967757600021511*sin(x(2))*sin(x(3)))/81129638414606681695789005144064), conj(mi3x)*(sin(x(2))*sin(x(3)) - cos(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)) - (conj(mi3y) + (4967757600021511*conj(mi3z))/40564819207303340847894502572032)*(cos(x(3))*sin(x(2)) + sin(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)), 0
conj(mi4x)*(cos(x(4))*(sin(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) + sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + cos(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064))) - ((4967757600021511*conj(mi4y))/40564819207303340847894502572032 - conj(mi4z))*((4967757600021511*cos(x(1)))/81129638414606681695789005144064 + (4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) - (conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + cos(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) - sin(x(4))*(sin(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))))) - a3*cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - (4967757600021511*a2*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - a2*cos(x(2))*sin(x(1)) + a3*sin(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064), - (cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)*((4967757600021511*conj(mi4y))/40564819207303340847894502572032 - conj(mi4z)) - (cos(x(4))*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - (24678615572571482867467662723121*sin(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096)) + sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - (24678615572571482867467662723121*sin(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)))*(conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032) - conj(mi4x)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - (24678615572571482867467662723121*sin(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)) - sin(x(4))*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - (24678615572571482867467662723121*sin(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096))) - a3*cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - a2*cos(x(1))*sin(x(2)) - (4967757600021511*a2*cos(x(2))*sin(x(1)))/81129638414606681695789005144064 - a3*sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - (24678615572571482867467662723121*sin(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096), - (conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) + sin(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096))) - conj(mi4x)*(cos(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096)) - sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064))) - a3*sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - a3*cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096), - (conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) + sin(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096))) - conj(mi4x)*(cos(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096)) - sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)))
(conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032)*(cos(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096)) - sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064))) - ((4967757600021511*conj(mi4y))/40564819207303340847894502572032 - conj(mi4z))*((4967757600021511*sin(x(1)))/81129638414606681695789005144064 + cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - conj(mi4x)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) + sin(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096))) + a3*cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + a2*cos(x(1))*cos(x(2)) - (4967757600021511*a2*sin(x(1))*sin(x(2)))/81129638414606681695789005144064 - a3*sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - sin(x(1)) + (24678615572571482867467662723121*cos(x(2))*sin(x(1)))/6582018229284824168619876730229402019930943462534319453394436096), (cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((24678615572571482867467662723121*cos(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)) - sin(x(4))*(sin(x(3))*((24678615572571482867467662723121*cos(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2)))))*(conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032) - ((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))*((4967757600021511*conj(mi4y))/40564819207303340847894502572032 - conj(mi4z)) - conj(mi4x)*(cos(x(4))*(sin(x(3))*((24678615572571482867467662723121*cos(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2)))) + sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((24678615572571482867467662723121*cos(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064))) + a3*cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + (4967757600021511*a2*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - a2*sin(x(1))*sin(x(2)) - a3*sin(x(3))*((24678615572571482867467662723121*cos(x(1))*sin(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064), - conj(mi4x)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + cos(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) - sin(x(4))*(sin(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))))) - (conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032)*(cos(x(4))*(sin(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) + sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + cos(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064))) - a3*sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - a3*cos(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064), - conj(mi4x)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + cos(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) - sin(x(4))*(sin(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))))) - (conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032)*(cos(x(4))*(sin(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) + sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + cos(x(3))*(cos(x(1)) - (24678615572571482867467662723121*cos(x(1))*cos(x(2)))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)))
0, sin(x(2))*((4967757600021511*conj(mi4y))/40564819207303340847894502572032 - conj(mi4z)) - (sin(x(4))*(cos(x(2))*cos(x(3)) - (4967757600021511*sin(x(2))*sin(x(3)))/81129638414606681695789005144064) + cos(x(4))*(cos(x(2))*sin(x(3)) + (4967757600021511*cos(x(3))*sin(x(2)))/81129638414606681695789005144064))*(conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032) + conj(mi4x)*(sin(x(4))*(cos(x(2))*sin(x(3)) + (4967757600021511*cos(x(3))*sin(x(2)))/81129638414606681695789005144064) - cos(x(4))*(cos(x(2))*cos(x(3)) - (4967757600021511*sin(x(2))*sin(x(3)))/81129638414606681695789005144064)) - a2*cos(x(2)) - a3*cos(x(2))*cos(x(3)) + (4967757600021511*a3*sin(x(2))*sin(x(3)))/81129638414606681695789005144064, conj(mi4x)*(cos(x(4))*(sin(x(2))*sin(x(3)) - cos(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)) + sin(x(4))*(cos(x(3))*sin(x(2)) + sin(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064))) - (cos(x(4))*(cos(x(3))*sin(x(2)) + sin(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)) - sin(x(4))*(sin(x(2))*sin(x(3)) - cos(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)))*(conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032) + a3*sin(x(2))*sin(x(3)) - a3*cos(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064), conj(mi4x)*(cos(x(4))*(sin(x(2))*sin(x(3)) - cos(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)) + sin(x(4))*(cos(x(3))*sin(x(2)) + sin(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064))) - (cos(x(4))*(cos(x(3))*sin(x(2)) + sin(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)) - sin(x(4))*(sin(x(2))*sin(x(3)) - cos(x(3))*((4967757600021511*cos(x(2)))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)))*(conj(mi4y) + (4967757600021511*conj(mi4z))/40564819207303340847894502572032)];
% Other fingers
else
h =[
mi1x+ofx - conj(mi2y)*sin(x(1)) + conj(mi2x)*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + conj(mi2z)*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)
mi1y+ofy + conj(mi2y)*cos(x(1)) + conj(mi2x)*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - conj(mi2z)*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2)))
mi1z+ofz + (4967757600021511*conj(mi2y))/81129638414606681695789005144064 + conj(mi2z)*cos(x(2)) - conj(mi2x)*sin(x(2))
mi1x+ofx - conj(mi3y)*sin(x(1)) - conj(mi3x)*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) + conj(mi3z)*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)) + a2*cos(x(1))*cos(x(2)) - (4967757600021511*a2*sin(x(1))*sin(x(2)))/81129638414606681695789005144064
mi1y+ofy + conj(mi3y)*cos(x(1)) + conj(mi3x)*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) + conj(mi3z)*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2)))) + (4967757600021511*a2*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + a2*cos(x(2))*sin(x(1))
mi1z+ofz + (4967757600021511*conj(mi3y))/81129638414606681695789005144064 - a2*sin(x(2)) - conj(mi3x)*(cos(x(2))*sin(x(3)) + cos(x(3))*sin(x(2))) + conj(mi3z)*(cos(x(2))*cos(x(3)) - sin(x(2))*sin(x(3)))
mi1x+ofx - conj(mi4y)*sin(x(1)) - conj(mi4x)*(cos(x(4))*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) + sin(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064))) + conj(mi4z)*(cos(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)) - sin(x(4))*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064))) + a3*cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - a3*sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) + a2*cos(x(1))*cos(x(2)) - (4967757600021511*a2*sin(x(1))*sin(x(2)))/81129638414606681695789005144064
mi1y+ofy + conj(mi4y)*cos(x(1)) + conj(mi4x)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) - sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))))) + conj(mi4z)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2)))) + sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))))) + a3*cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + a3*sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + (4967757600021511*a2*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + a2*cos(x(2))*sin(x(1))
mi1z+ofz + (4967757600021511*conj(mi4y))/81129638414606681695789005144064 - conj(mi4x)*(cos(x(4))*(cos(x(2))*sin(x(3)) + cos(x(3))*sin(x(2))) + sin(x(4))*(cos(x(2))*cos(x(3)) - sin(x(2))*sin(x(3)))) + conj(mi4z)*(cos(x(4))*(cos(x(2))*cos(x(3)) - sin(x(2))*sin(x(3))) - sin(x(4))*(cos(x(2))*sin(x(3)) + cos(x(3))*sin(x(2)))) - a2*sin(x(2)) - a3*cos(x(2))*sin(x(3)) - a3*cos(x(3))*sin(x(2))];
% Define the output Jacobian Matrix
H =[
conj(mi2z)*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) - conj(mi2x)*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - conj(mi2y)*cos(x(1)), conj(mi2z)*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - conj(mi2x)*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064), 0, 0;
conj(mi2x)*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - conj(mi2y)*sin(x(1)) + conj(mi2z)*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064), conj(mi2x)*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + conj(mi2z)*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))), 0, 0;
0, - conj(mi2x)*cos(x(2)) - conj(mi2z)*sin(x(2)), 0, 0;
- conj(mi3y)*cos(x(1)) - conj(mi3x)*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) - conj(mi3z)*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2)))) - (4967757600021511*a2*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - a2*cos(x(2))*sin(x(1)), - conj(mi3x)*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)) - conj(mi3z)*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) - a2*cos(x(1))*sin(x(2)) - (4967757600021511*a2*cos(x(2))*sin(x(1)))/81129638414606681695789005144064, - conj(mi3x)*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)) - conj(mi3z)*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)), 0;
conj(mi3z)*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)) - conj(mi3x)*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) - conj(mi3y)*sin(x(1)) + a2*cos(x(1))*cos(x(2)) - (4967757600021511*a2*sin(x(1))*sin(x(2)))/81129638414606681695789005144064, conj(mi3z)*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) - conj(mi3x)*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2)))) + (4967757600021511*a2*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - a2*sin(x(1))*sin(x(2)), conj(mi3z)*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) - conj(mi3x)*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2)))), 0;
0, - a2*cos(x(2)) - conj(mi3x)*(cos(x(2))*cos(x(3)) - sin(x(2))*sin(x(3))) - conj(mi3z)*(cos(x(2))*sin(x(3)) + cos(x(3))*sin(x(2))), - conj(mi3x)*(cos(x(2))*cos(x(3)) - sin(x(2))*sin(x(3))) - conj(mi3z)*(cos(x(2))*sin(x(3)) + cos(x(3))*sin(x(2))), 0;
- conj(mi4y)*cos(x(1)) - conj(mi4x)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) - sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))))) - conj(mi4z)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2)))) + sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))))) - a3*cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - a3*sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) - (4967757600021511*a2*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 - a2*cos(x(2))*sin(x(1)), - conj(mi4x)*(cos(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)) - sin(x(4))*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064))) - conj(mi4z)*(cos(x(4))*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) + sin(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064))) - a3*cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - a3*sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - a2*cos(x(1))*sin(x(2)) - (4967757600021511*a2*cos(x(2))*sin(x(1)))/81129638414606681695789005144064, - conj(mi4x)*(cos(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)) - sin(x(4))*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064))) - conj(mi4z)*(cos(x(4))*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) + sin(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064))) - a3*cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - a3*sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064), - conj(mi4x)*(cos(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)) - sin(x(4))*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064))) - conj(mi4z)*(cos(x(4))*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) + sin(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)));
conj(mi4z)*(cos(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064)) - sin(x(4))*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064))) - conj(mi4x)*(cos(x(4))*(sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) - cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064)) + sin(x(4))*(sin(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) + cos(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064))) - conj(mi4y)*sin(x(1)) + a3*cos(x(3))*(cos(x(1))*cos(x(2)) - (4967757600021511*sin(x(1))*sin(x(2)))/81129638414606681695789005144064) - a3*sin(x(3))*(cos(x(1))*sin(x(2)) + (4967757600021511*cos(x(2))*sin(x(1)))/81129638414606681695789005144064) + a2*cos(x(1))*cos(x(2)) - (4967757600021511*a2*sin(x(1))*sin(x(2)))/81129638414606681695789005144064, conj(mi4z)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) - sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))))) - conj(mi4x)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2)))) + sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))))) + a3*cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) - a3*sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) + (4967757600021511*a2*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - a2*sin(x(1))*sin(x(2)), conj(mi4z)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) - sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))))) - conj(mi4x)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2)))) + sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))))) + a3*cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) - a3*sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))), conj(mi4z)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))) - sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))))) - conj(mi4x)*(cos(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1))) - cos(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2)))) + sin(x(4))*(sin(x(3))*((4967757600021511*cos(x(1))*cos(x(2)))/81129638414606681695789005144064 - sin(x(1))*sin(x(2))) + cos(x(3))*((4967757600021511*cos(x(1))*sin(x(2)))/81129638414606681695789005144064 + cos(x(2))*sin(x(1)))));
0, a3*sin(x(2))*sin(x(3)) - conj(mi4z)*(cos(x(4))*(cos(x(2))*sin(x(3)) + cos(x(3))*sin(x(2))) + sin(x(4))*(cos(x(2))*cos(x(3)) - sin(x(2))*sin(x(3)))) - a2*cos(x(2)) - a3*cos(x(2))*cos(x(3)) - conj(mi4x)*(cos(x(4))*(cos(x(2))*cos(x(3)) - sin(x(2))*sin(x(3))) - sin(x(4))*(cos(x(2))*sin(x(3)) + cos(x(3))*sin(x(2)))), a3*sin(x(2))*sin(x(3)) - conj(mi4z)*(cos(x(4))*(cos(x(2))*sin(x(3)) + cos(x(3))*sin(x(2))) + sin(x(4))*(cos(x(2))*cos(x(3)) - sin(x(2))*sin(x(3)))) - a3*cos(x(2))*cos(x(3)) - conj(mi4x)*(cos(x(4))*(cos(x(2))*cos(x(3)) - sin(x(2))*sin(x(3))) - sin(x(4))*(cos(x(2))*sin(x(3)) + cos(x(3))*sin(x(2)))), - conj(mi4x)*(cos(x(4))*(cos(x(2))*cos(x(3)) - sin(x(2))*sin(x(3))) - sin(x(4))*(cos(x(2))*sin(x(3)) + cos(x(3))*sin(x(2)))) - conj(mi4z)*(cos(x(4))*(cos(x(2))*sin(x(3)) + cos(x(3))*sin(x(2))) + sin(x(4))*(cos(x(2))*cos(x(3)) - sin(x(2))*sin(x(3))))];
end
% Define the observation vector
observation = measData(4+12*(d-1):12+12*(d-1),i);
innovation = observation - h ;
varInnov = H*P*H' + R_OBS; % (9x9)
% Kalman Gain
K = P*H'*varInnov^-1;
% Calculate state corrections
x = x + K * innovation;
% Update the covariance
P = P - K*H*P;
%save estimated angles (in columns)
stateEst = [stateEst; x'];
end
display('GenEstMeas...')
GenEstMeas;
% Marker components obtained by estimated angles
mdtio=[mdtio [mdt1io mdt2io mdt3io mdt4io]];
display('GenEstMeas Done')
EKFPlots;
% Save all estimated satates
stateEstall = [stateEstall stateEst];
end
estname = ['EstimatedQHand' file_id '.mat'];
save(estname, 'EstimatedQ')
toc