-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathObjHand.m
More file actions
47 lines (39 loc) · 34.3 KB
/
Copy pathObjHand.m
File metadata and controls
47 lines (39 loc) · 34.3 KB
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
function F = ObjHand(x,mt1,mt2,mt3,mt4,d)
mi1x=mt1(:,1);
mi1y=mt1(:,2);
mi1z=mt1(:,3);
global Nframe
F=[];
for i=1:Nframe
if d==1
% thumb objective function
h =[
mi1x(i) + x(12) + (conj(x(2)) + (4967757600021511*conj(x(3)))/40564819207303340847894502572032)*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1))) + (24678615572571482867467662723121*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096) - ((4967757600021511*conj(x(2)))/40564819207303340847894502572032 - conj(x(3)))*((4967757600021511*sin(x(15+4*(i-1))))/81129638414606681695789005144064 + cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))) + (4967757600021511*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/81129638414606681695789005144064) + conj(x(1))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064)
mi1y(i) + x(13) + ((4967757600021511*conj(x(2)))/40564819207303340847894502572032 - conj(x(3)))*((4967757600021511*cos(x(15+4*(i-1))))/81129638414606681695789005144064 + (4967757600021511*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1)))*sin(x(16+4*(i-1)))) + (conj(x(2)) + (4967757600021511*conj(x(3)))/40564819207303340847894502572032)*(cos(x(15+4*(i-1))) - (24678615572571482867467662723121*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064) + conj(x(1))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))
mi1z(i) + x(14) - conj(x(1))*sin(x(16+4*(i-1))) - (cos(x(16+4*(i-1))) - 24678615572571482867467662723121/6582018229284824168619876730229402019930943462534319453394436096)*((4967757600021511*conj(x(2)))/40564819207303340847894502572032 - conj(x(3))) + (conj(x(2)) + (4967757600021511*conj(x(3)))/40564819207303340847894502572032)*((4967757600021511*cos(x(16+4*(i-1))))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)
mi1x(i) + x(12) - conj(x(4))*(sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1))) + (24678615572571482867467662723121*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064)) - ((4967757600021511*conj(x(5)))/40564819207303340847894502572032 - conj(x(6)))*((4967757600021511*sin(x(15+4*(i-1))))/81129638414606681695789005144064 + cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))) + (4967757600021511*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/81129638414606681695789005144064) + (conj(x(5)) + (4967757600021511*conj(x(6)))/40564819207303340847894502572032)*(sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064) + cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1))) + (24678615572571482867467662723121*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096)) + x(10)*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*x(10)*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064
mi1y(i) + x(13) + (sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))) + cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1))) - (24678615572571482867467662723121*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064))*(conj(x(5)) + (4967757600021511*conj(x(6)))/40564819207303340847894502572032) + ((4967757600021511*conj(x(5)))/40564819207303340847894502572032 - conj(x(6)))*((4967757600021511*cos(x(15+4*(i-1))))/81129638414606681695789005144064 + (4967757600021511*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1)))*sin(x(16+4*(i-1)))) - conj(x(4))*(sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1))) - (24678615572571482867467662723121*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064) - cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))) + (4967757600021511*x(10)*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + x(10)*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))
mi1z(i) + x(14) - x(10)*sin(x(16+4*(i-1))) - (conj(x(5)) + (4967757600021511*conj(x(6)))/40564819207303340847894502572032)*(sin(x(16+4*(i-1)))*sin(x(17+4*(i-1))) - cos(x(17+4*(i-1)))*((4967757600021511*cos(x(16+4*(i-1))))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)) - (cos(x(16+4*(i-1))) - 24678615572571482867467662723121/6582018229284824168619876730229402019930943462534319453394436096)*((4967757600021511*conj(x(5)))/40564819207303340847894502572032 - conj(x(6))) - conj(x(4))*(cos(x(17+4*(i-1)))*sin(x(16+4*(i-1))) + sin(x(17+4*(i-1)))*((4967757600021511*cos(x(16+4*(i-1))))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064))
mi1x(i) + x(12) - ((4967757600021511*conj(x(8)))/40564819207303340847894502572032 - conj(x(9)))*((4967757600021511*sin(x(15+4*(i-1))))/81129638414606681695789005144064 + cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))) + (4967757600021511*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/81129638414606681695789005144064) + (conj(x(8)) + (4967757600021511*conj(x(9)))/40564819207303340847894502572032)*(cos(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064) + cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1))) + (24678615572571482867467662723121*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096)) - sin(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1))) + (24678615572571482867467662723121*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064))) - conj(x(7))*(cos(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1))) + (24678615572571482867467662723121*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096) - cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064)) + sin(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064) + cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1))) + (24678615572571482867467662723121*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096))) + x(11)*cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064) + x(10)*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*x(10)*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 - x(11)*sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1))) + (24678615572571482867467662723121*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096)
mi1y(i) + x(13) - conj(x(7))*(cos(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1))) - (24678615572571482867467662723121*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064) - cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))) + sin(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))) + cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1))) - (24678615572571482867467662723121*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064))) + ((4967757600021511*conj(x(8)))/40564819207303340847894502572032 - conj(x(9)))*((4967757600021511*cos(x(15+4*(i-1))))/81129638414606681695789005144064 + (4967757600021511*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1)))*sin(x(16+4*(i-1)))) + (conj(x(8)) + (4967757600021511*conj(x(9)))/40564819207303340847894502572032)*(cos(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))) + cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1))) - (24678615572571482867467662723121*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064)) - sin(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1))) - (24678615572571482867467662723121*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064) - cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))))) + x(11)*cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))) + (4967757600021511*x(10)*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + x(10)*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))) - x(11)*sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1))) - (24678615572571482867467662723121*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/6582018229284824168619876730229402019930943462534319453394436096 + (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064)
mi1z(i) + x(14) - (cos(x(18+4*(i-1)))*(sin(x(16+4*(i-1)))*sin(x(17+4*(i-1))) - cos(x(17+4*(i-1)))*((4967757600021511*cos(x(16+4*(i-1))))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)) + sin(x(18+4*(i-1)))*(cos(x(17+4*(i-1)))*sin(x(16+4*(i-1))) + sin(x(17+4*(i-1)))*((4967757600021511*cos(x(16+4*(i-1))))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)))*(conj(x(8)) + (4967757600021511*conj(x(9)))/40564819207303340847894502572032) - conj(x(7))*(cos(x(18+4*(i-1)))*(cos(x(17+4*(i-1)))*sin(x(16+4*(i-1))) + sin(x(17+4*(i-1)))*((4967757600021511*cos(x(16+4*(i-1))))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)) - sin(x(18+4*(i-1)))*(sin(x(16+4*(i-1)))*sin(x(17+4*(i-1))) - cos(x(17+4*(i-1)))*((4967757600021511*cos(x(16+4*(i-1))))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064))) - x(10)*sin(x(16+4*(i-1))) - (cos(x(16+4*(i-1))) - 24678615572571482867467662723121/6582018229284824168619876730229402019930943462534319453394436096)*((4967757600021511*conj(x(8)))/40564819207303340847894502572032 - conj(x(9))) - x(11)*cos(x(17+4*(i-1)))*sin(x(16+4*(i-1))) - x(11)*sin(x(17+4*(i-1)))*((4967757600021511*cos(x(16+4*(i-1))))/81129638414606681695789005144064 + 4967757600021511/81129638414606681695789005144064)];
else
% index, middle, ring and little finger have the same function
h =[
mi1x(i)+x(12) - conj(x(2))*sin(x(15+4*(i-1))) + conj(x(1))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064) + conj(x(3))*(cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))) + (4967757600021511*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/81129638414606681695789005144064)
mi1y(i)+x(13) + conj(x(2))*cos(x(15+4*(i-1))) + conj(x(1))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))) - conj(x(3))*((4967757600021511*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))
mi1z(i)+x(14) + (4967757600021511*conj(x(2)))/81129638414606681695789005144064 + conj(x(3))*cos(x(16+4*(i-1))) - conj(x(1))*sin(x(16+4*(i-1)))
mi1x(i)+x(12) - conj(x(5))*sin(x(15+4*(i-1))) - conj(x(4))*(sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))) + (4967757600021511*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/81129638414606681695789005144064) - cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064)) + conj(x(6))*(sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064) + cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))) + (4967757600021511*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/81129638414606681695789005144064)) + x(10)*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*x(10)*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064
mi1y(i)+x(13) + conj(x(5))*cos(x(15+4*(i-1))) + conj(x(4))*(sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1)))*sin(x(16+4*(i-1)))) + cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))) + conj(x(6))*(sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))) - cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))) + (4967757600021511*x(10)*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + x(10)*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))
mi1z(i)+x(14) + (4967757600021511*conj(x(5)))/81129638414606681695789005144064 - x(10)*sin(x(16+4*(i-1))) - conj(x(4))*(cos(x(16+4*(i-1)))*sin(x(17+4*(i-1))) + cos(x(17+4*(i-1)))*sin(x(16+4*(i-1)))) + conj(x(6))*(cos(x(16+4*(i-1)))*cos(x(17+4*(i-1))) - sin(x(16+4*(i-1)))*sin(x(17+4*(i-1))))
mi1x(i)+x(12) - conj(x(8))*sin(x(15+4*(i-1))) - conj(x(7))*(cos(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))) + (4967757600021511*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/81129638414606681695789005144064) - cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064)) + sin(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064) + cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))) + (4967757600021511*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/81129638414606681695789005144064))) + conj(x(9))*(cos(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064) + cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))) + (4967757600021511*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/81129638414606681695789005144064)) - sin(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))) + (4967757600021511*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/81129638414606681695789005144064) - cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064))) + x(11)*cos(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064) - x(11)*sin(x(17+4*(i-1)))*(cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))) + (4967757600021511*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))/81129638414606681695789005144064) + x(10)*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))) - (4967757600021511*x(10)*sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064
mi1y(i)+x(13) + conj(x(8))*cos(x(15+4*(i-1))) + conj(x(7))*(cos(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1)))*sin(x(16+4*(i-1)))) + cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1))))) - sin(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))) - cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1)))*sin(x(16+4*(i-1)))))) + conj(x(9))*(cos(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))) - cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1)))*sin(x(16+4*(i-1))))) + sin(x(18+4*(i-1)))*(sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1)))*sin(x(16+4*(i-1)))) + cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))))) + x(11)*cos(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))) + x(11)*sin(x(17+4*(i-1)))*((4967757600021511*cos(x(15+4*(i-1)))*cos(x(16+4*(i-1))))/81129638414606681695789005144064 - sin(x(15+4*(i-1)))*sin(x(16+4*(i-1)))) + (4967757600021511*x(10)*cos(x(15+4*(i-1)))*sin(x(16+4*(i-1))))/81129638414606681695789005144064 + x(10)*cos(x(16+4*(i-1)))*sin(x(15+4*(i-1)))
mi1z(i)+x(14) + (4967757600021511*conj(x(8)))/81129638414606681695789005144064 - conj(x(7))*(cos(x(18+4*(i-1)))*(cos(x(16+4*(i-1)))*sin(x(17+4*(i-1))) + cos(x(17+4*(i-1)))*sin(x(16+4*(i-1)))) + sin(x(18+4*(i-1)))*(cos(x(16+4*(i-1)))*cos(x(17+4*(i-1))) - sin(x(16+4*(i-1)))*sin(x(17+4*(i-1))))) + conj(x(9))*(cos(x(18+4*(i-1)))*(cos(x(16+4*(i-1)))*cos(x(17+4*(i-1))) - sin(x(16+4*(i-1)))*sin(x(17+4*(i-1)))) - sin(x(18+4*(i-1)))*(cos(x(16+4*(i-1)))*sin(x(17+4*(i-1))) + cos(x(17+4*(i-1)))*sin(x(16+4*(i-1))))) - x(10)*sin(x(16+4*(i-1))) - x(11)*cos(x(16+4*(i-1)))*sin(x(17+4*(i-1))) - x(11)*cos(x(17+4*(i-1)))*sin(x(16+4*(i-1)))];
end
y = [mt2(i,:) mt3(i,:) mt4(i,:)]';
r = y-h;
F = [F;r];
end
%F=norm(1000*F);
F=sum(abs(F))/Nframe;
end