-
Notifications
You must be signed in to change notification settings - Fork 0
/
Jqwl_matrix_fun.m
57 lines (55 loc) · 2.41 KB
/
Jqwl_matrix_fun.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
function Jqwl = Jqwl_matrix_fun(CW1_1,CW1_2,CW1_3,CW1_4,CW2_1,CW2_2,CW2_3,CW2_4,d_w,delta1,delta2,delta3,delta4,rW1_1,rW1_2,rW1_3,rW1_4,rW2_1,rW2_2,rW2_3,rW2_4,s_w)
%JQWL_MATRIX_FUN
% JQWL = JQWL_MATRIX_FUN(CW1_1,CW1_2,CW1_3,CW1_4,CW2_1,CW2_2,CW2_3,CW2_4,D_W,DELTA1,DELTA2,DELTA3,DELTA4,RW1_1,RW1_2,RW1_3,RW1_4,RW2_1,RW2_2,RW2_3,RW2_4,S_W)
% This function was generated by the Symbolic Math Toolbox version 8.0.
% 22-Jan-2018 15:34:01
t2 = 1.0./d_w;
t3 = cos(delta1);
t4 = d_w.*t3;
t5 = sin(delta1);
t6 = s_w.*t5.*2.0;
t7 = cos(delta2);
t8 = d_w.*t7;
t9 = sin(delta2);
t10 = s_w.*t9.*2.0;
t11 = cos(delta3);
t12 = d_w.*t11;
t13 = sin(delta3);
t14 = s_w.*t13.*2.0;
t15 = cos(delta4);
t16 = d_w.*t15;
t17 = sin(delta4);
t18 = s_w.*t17.*2.0;
t19 = d_w.*t5;
t20 = d_w.*t9;
t21 = d_w.*t13;
t22 = d_w.*t17;
t23 = CW1_1.^2;
t24 = CW2_1.^2;
t25 = t23+t24;
t26 = 1.0./t25;
t27 = CW2_1.*d_w.*t3;
t28 = CW1_1.*s_w.*t3.*2.0;
t29 = CW2_1.*s_w.*t5.*2.0;
t30 = CW1_2.^2;
t31 = CW2_2.^2;
t32 = t30+t31;
t33 = 1.0./t32;
t34 = CW2_2.*d_w.*t7;
t35 = CW1_2.*s_w.*t7.*2.0;
t36 = CW2_2.*s_w.*t9.*2.0;
t37 = CW1_3.^2;
t38 = CW2_3.^2;
t39 = t37+t38;
t40 = 1.0./t39;
t41 = CW2_3.*d_w.*t11;
t42 = CW1_3.*s_w.*t11.*2.0;
t43 = CW2_3.*s_w.*t13.*2.0;
t44 = CW1_4.^2;
t45 = CW2_4.^2;
t46 = t44+t45;
t47 = 1.0./t46;
t48 = CW2_4.*d_w.*t15;
t49 = CW1_4.*s_w.*t15.*2.0;
t50 = CW2_4.*s_w.*t17.*2.0;
Jqwl = reshape([rW1_1.*t2.*(t4+t6).*(1.0./8.0),rW1_1.*t2.*(t19-s_w.*t3.*2.0).*(1.0./8.0),rW1_1.*t2.*t26.*(t27+t28+t29-CW1_1.*d_w.*t5).*(-1.0./8.0),rW2_1.*t2.*(t4-t6).*(1.0./8.0),rW2_1.*t2.*(t19+s_w.*t3.*2.0).*(1.0./8.0),rW2_1.*t2.*t26.*(-t27+t28+t29+CW1_1.*d_w.*t5).*(1.0./8.0),rW1_2.*t2.*(t8+t10).*(1.0./8.0),rW1_2.*t2.*(t20-s_w.*t7.*2.0).*(1.0./8.0),rW1_2.*t2.*t33.*(t34+t35+t36-CW1_2.*d_w.*t9).*(-1.0./8.0),rW2_2.*t2.*(t8-t10).*(1.0./8.0),rW2_2.*t2.*(t20+s_w.*t7.*2.0).*(1.0./8.0),rW2_2.*t2.*t33.*(-t34+t35+t36+CW1_2.*d_w.*t9).*(1.0./8.0),rW1_3.*t2.*(t12+t14).*(1.0./8.0),rW1_3.*t2.*(t21-s_w.*t11.*2.0).*(1.0./8.0),rW1_3.*t2.*t40.*(t41+t42+t43-CW1_3.*d_w.*t13).*(-1.0./8.0),rW2_3.*t2.*(t12-t14).*(1.0./8.0),rW2_3.*t2.*(t21+s_w.*t11.*2.0).*(1.0./8.0),rW2_3.*t2.*t40.*(-t41+t42+t43+CW1_3.*d_w.*t13).*(1.0./8.0),rW1_4.*t2.*(t16+t18).*(1.0./8.0),rW1_4.*t2.*(t22-s_w.*t15.*2.0).*(1.0./8.0),rW1_4.*t2.*t47.*(t48+t49+t50-CW1_4.*d_w.*t17).*(-1.0./8.0),rW2_4.*t2.*(t16-t18).*(1.0./8.0),rW2_4.*t2.*(t22+s_w.*t15.*2.0).*(1.0./8.0),rW2_4.*t2.*t47.*(-t48+t49+t50+CW1_4.*d_w.*t17).*(1.0./8.0)],[3,8]);