%% Complex Step Diff Checking
clc
format long g
F = @(x) exp(x)/((cos(x))^3 + (sin(x))^3)
G = symJac(F);
H = symHess(F);
x0 = pi/2;
true1d = G(x0)
true2d = H(x0)
I = sqrt(2)/2*(1i + 1);

k = 6;
cs1d = imag(F(x0 + 1i*10^-k)/(1*10^-k))
cs2d = imag((F(x0 + I*10^-k) + F(x0 - I*10^-k))/(10^-k)^2)

%% Multivariable 2nd der
clc
F = @(x) exp(x(1))/((cos(x(2)))^3 + (sin(x(2)))^3)
G = symJac(F);
H = symHess(F);
x0 = [2;2];
true1d = G(x0)
true2d = H(x0)

cstepHess(F,x0)

%% Multivariable 2nd der 4x4
clc
f = @(x) x(1)*x(4)*sum(x(1:3)) + x(3);
g = @(x) [ x(1)*x(4) + x(4)*sum(x(1:3))
                x(1)*x(4)
                x(1)*x(4) + 1
                x(1)*sum(x(1:3)) ];
c = @(x) [ prod(x); sum(x.^2) ];            
j = @(x) [ prod(x)./x 2*x ];
H = @(x,sigma,lambda) sigma*[ 2*x(4)             0      0   0;
                      x(4)               0      0   0;
                      x(4)               0      0   0;
                      2*x(1)+x(2)+x(3)  x(1)  x(1)  0 ] +....
          lambda(1)*[    0          0         0         0;
                              x(3)*x(4)     0         0         0;
                              x(2)*x(4) x(1)*x(4)     0         0;
                              x(2)*x(3) x(1)*x(3) x(1)*x(2)     0  ] +...
          lambda(2)*diag([2 2 2 2]);

      
x0 = randn(4,1);
sig = randn(1,1); lambda = randn(2,1);
% x0 = [0.1;0.2;0.3;0.4];

%Gradient
cstepJac(f,x0) - g(x0)'
%Jacobian
cstepJac(c,x0) - j(x0)'
%Hessian Of Objective
cstepHess(f,x0,true) - H(x0,1,[0;0])
%Hessian Of Lagrangian
cstepHessLag(f,c,x0,sig,lambda,true) - H(x0,sig,lambda)

%% Speed Test
clc
La = 0.6604; Lh = 0.1778; m_b = 0.575; m_f = 0.575; Lw = 0.4699; m_w = 1.87; g = 9.81; Kf = 0.1188; u = [1;1];
F = @(x) (0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2 + 0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f + 0.6e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh * Lw ^ 2 * m_b * m_f * m_w + 0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 4 * Lh * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh * Lw ^ 2 * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * Lh * Lw ^ 4 * m_b * m_w ^ 2 + 0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 4 * Lh * m_b ^ 2 * m_f + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh * Lw ^ 2 * m_b ^ 2 * m_w + cos(x(2)) ^ 3 * cos(x(1)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.2e1 * cos(x(2)) * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w * cos(x(1)) ^ 2 + cos(x(2)) * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w * cos(x(1)) ^ 2 + cos(x(2)) * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w * cos(x(1)) ^ 2 + 0.4e1 * cos(x(2)) ^ 3 * cos(x(1)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * Lh * Lw ^ 4 * m_f * m_w ^ 2 - cos(x(2)) ^ 3 * cos(x(1)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - 0.4e1 * cos(x(2)) ^ 3 * cos(x(1)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w) * x(4) ^ 2 + ((0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f + 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2 + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w + 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * La ^ 4 * Lh * m_b ^ 2 * m_f + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * Lh * Lw ^ 4 * m_f * m_w ^ 2 + 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * La ^ 4 * Lh * m_b * m_f ^ 2 + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * Lh * Lw ^ 4 * m_b * m_w ^ 2 - 0.2e1 * cos(x(1)) * La ^ 2 * Lh * Lw ^ 2 * m_f ^ 2 * m_w - 0.2e1 * cos(x(1)) * La ^ 2 * Lh * Lw ^ 2 * m_b ^ 2 * m_w - 0.4e1 * cos(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w - 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 + 0.12e2 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * La ^ 2 * Lh * Lw ^ 2 * m_b * m_f * m_w + 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * cos(x(2)) ^ 2 * cos(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * La ^ 2 * Lh * Lw ^ 2 * m_b ^ 2 * m_w + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * La ^ 2 * Lh * Lw ^ 2 * m_f ^ 2 * m_w - 0.12e2 * cos(x(1)) * La ^ 2 * Lh * Lw ^ 2 * m_b * m_f * m_w - 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - 0.2e1 * cos(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w - 0.2e1 * cos(x(1)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w - 0.8e1 * cos(x(1)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f - 0.8e1 * cos(x(1)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2 - 0.8e1 * cos(x(1)) * La ^ 4 * Lh * m_b ^ 2 * m_f - 0.8e1 * cos(x(1)) * La ^ 4 * Lh * m_b * m_f ^ 2 - 0.2e1 * cos(x(1)) * Lh * Lw ^ 4 * m_b * m_w ^ 2 - 0.2e1 * cos(x(1)) * Lh * Lw ^ 4 * m_f * m_w ^ 2) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w) * x(6) + (0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) * sin(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) * sin(x(1)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w + 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) * sin(x(1)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f + 0.4e1 * cos(x(2)) ^ 2 * cos(x(1)) * sin(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w + 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) * sin(x(1)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w) * x(5)) * x(4) + (0.2e1 * cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w - cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 2 * Lh * Lw ^ 2 * m_b ^ 2 * m_w + cos(x(2)) ^ 3 * cos(x(1)) ^ 3 * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) ^ 3 * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + 0.2e1 * cos(x(2)) * cos(x(1)) ^ 3 * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w - cos(x(2)) ^ 3 * cos(x(1)) ^ 3 * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w - cos(x(2)) * cos(x(1)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w - cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 2 * Lh * Lw ^ 2 * m_f ^ 2 * m_w - 0.4e1 * cos(x(2)) * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w * cos(x(1)) ^ 2 - 0.8e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2 - 0.8e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f - 0.2e1 * cos(x(2)) * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w * cos(x(1)) ^ 2 - 0.2e1 * cos(x(2)) * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w * cos(x(1)) ^ 2 - cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * Lh * Lw ^ 4 * m_f * m_w ^ 2 + 0.4e1 * cos(x(2)) ^ 3 * cos(x(1)) ^ 3 * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f - 0.4e1 * cos(x(2)) ^ 3 * cos(x(1)) ^ 3 * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 - 0.4e1 * cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 4 * Lh * m_b ^ 2 * m_f - 0.4e1 * cos(x(2)) * cos(x(1)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 + 0.4e1 * cos(x(2)) * cos(x(1)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f - 0.8e1 * cos(x(2)) * cos(x(1)) ^ 3 * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f + cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w + 0.8e1 * cos(x(2)) * cos(x(1)) ^ 3 * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 - 0.4e1 * cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 4 * Lh * m_b * m_f ^ 2 + 0.4e1 * cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f + 0.4e1 * cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2 - cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * Lh * Lw ^ 4 * m_b * m_w ^ 2 - 0.6e1 * cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 2 * Lh * Lw ^ 2 * m_b * m_f * m_w) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w) * x(6) ^ 2 + (-0.2e1 * cos(x(2)) * cos(x(1)) ^ 3 * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w - 0.8e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f + 0.2e1 * cos(x(2)) * cos(x(1)) ^ 3 * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w - 0.8e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2 - 0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w - 0.8e1 * cos(x(2)) * cos(x(1)) ^ 3 * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f + 0.2e1 * cos(x(2)) * cos(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.8e1 * cos(x(2)) * cos(x(1)) ^ 3 * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 - 0.8e1 * cos(x(2)) * cos(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 + 0.8e1 * cos(x(2)) * cos(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w) * x(5) * x(6) + (-0.4e1 * cos(x(2)) * cos(x(1)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 - cos(x(2)) * cos(x(1)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + 0.4e1 * cos(x(2)) * cos(x(1)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w) * x(5) ^ 2 + (-0.3e1 * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b ^ 2 * m_f + sin(x(2)) * sin(x(1)) * g * Lh * Lw ^ 3 * m_b * m_w ^ 2 + sin(x(2)) * sin(x(1)) * g * Lh * Lw ^ 3 * m_f * m_w ^ 2 - cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 3 * Lw * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 3 * Lw * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f - cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w - cos(x(2)) * cos(x(1)) ^ 2 * g * La * Lw ^ 3 * m_b * m_w ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * g * La * Lw ^ 3 * m_f * m_w ^ 2 + cos(x(2)) * cos(x(1)) * g * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) * g * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b ^ 3 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_f ^ 3 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 3 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_f ^ 3 + cos(x(2)) * cos(x(1)) ^ 3 * g * La ^ 3 * Lw * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) ^ 3 * g * La ^ 3 * Lw * m_f ^ 2 * m_w - 0.4e1 * cos(x(2)) * cos(x(1)) ^ 3 * g * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f - 0.3e1 * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b * m_f ^ 2 - 0.3e1 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 2 * m_f - 0.3e1 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b * m_f ^ 2 + sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b ^ 2 * m_w + sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_f ^ 2 * m_w + 0.4e1 * cos(x(2)) * cos(x(1)) ^ 3 * g * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 3 * g * La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) ^ 3 * g * La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 3 * g * La * Lw ^ 3 * m_b * m_w ^ 2 - cos(x(2)) * cos(x(1)) ^ 3 * g * La * Lw ^ 3 * m_f * m_w ^ 2 + 0.2e1 * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_f * m_w - 0.2e1 * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_b * m_w - 0.2e1 * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_f * m_w - sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_f ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_f ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_f ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b ^ 2 - cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b ^ 2 - cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_f ^ 2 + 0.2e1 * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_b * m_w + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_b * m_f - 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_b * m_f - cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_b * m_w - cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_b * m_w + 0.2e1 * cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b * m_f - 0.2e1 * cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b * m_f + cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_f * m_w + cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_f * m_w + cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b ^ 2 * cos(x(2)) ^ 2 + cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b ^ 2 * cos(x(2)) ^ 2 + cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b ^ 2 * cos(x(1)) ^ 2 + cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_f ^ 2 * cos(x(1)) ^ 2 - cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_f ^ 2 * cos(x(2)) ^ 2 - cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_f ^ 2 * cos(x(2)) ^ 2 - cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b ^ 2 * cos(x(1)) ^ 2 - cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_f ^ 2 * cos(x(1)) ^ 2 - cos(x(1)) * g * La * Lw ^ 3 * m_b * m_w ^ 2 * cos(x(2)) ^ 2 - 0.4e1 * cos(x(2)) * cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 - cos(x(1)) * g * La * Lh ^ 2 * Lw * m_f ^ 2 * m_w + 0.4e1 * cos(x(2)) * cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + cos(x(1)) * g * La * Lh ^ 2 * Lw * m_b ^ 2 * m_w + cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f * cos(x(2)) ^ 2 + cos(x(1)) * g * La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w * cos(x(2)) ^ 2 + cos(x(1)) * g * La ^ 3 * Lw * m_f ^ 2 * m_w * cos(x(2)) ^ 2 + cos(x(1)) * g * La * Lw ^ 3 * m_f * m_w ^ 2 * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 3 * Lw * m_b ^ 2 * m_w * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w * cos(x(2)) ^ 2 + 0.2e1 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b * m_f - 0.2e1 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b * m_f + Kf * Lh ^ 2 * Lw ^ 2 * u(1) * m_b * m_w + Kf * Lh ^ 2 * Lw ^ 2 * u(1) * m_f * m_w - Kf * Lh ^ 2 * Lw ^ 2 * u(2) * m_b * m_w - Kf * Lh ^ 2 * Lw ^ 2 * u(2) * m_f * m_w + cos(x(2)) * cos(x(1)) ^ 3 * g * Lw ^ 4 * m_b * m_w ^ 2 - cos(x(2)) * cos(x(1)) ^ 3 * g * Lw ^ 4 * m_f * m_w ^ 2 - sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b ^ 3 - sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_f ^ 3 - sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 3 - sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_f ^ 3 + cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 4 * m_b ^ 2 * m_f - cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 4 * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 2 * Lh ^ 2 * m_b ^ 3 - cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 2 * Lh ^ 2 * m_f ^ 3 - cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_b ^ 2 - cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_f ^ 2 - 0.2e1 * cos(x(2)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_b * m_f + 0.2e1 * cos(x(2)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_b * m_f + 0.2e1 * cos(x(1)) ^ 2 * Kf * La ^ 4 * u(1) * m_b * m_f - 0.2e1 * cos(x(1)) ^ 2 * Kf * La ^ 4 * u(2) * m_b * m_f - 0.2e1 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_b * m_f + 0.2e1 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_b * m_f + cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_b ^ 2 + cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 4 * u(1) * m_b ^ 2 - cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 4 * u(1) * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 4 * u(2) * m_b ^ 2 - cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 4 * u(2) * m_f ^ 2 - cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b ^ 2 - cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b ^ 2 - cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b ^ 2 - cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_f ^ 2 + cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_f ^ 2 + cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_f ^ 2 + cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b ^ 2 + cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_f ^ 2 - cos(x(1)) * Kf * La ^ 4 * u(2) * m_f ^ 2 * cos(x(2)) ^ 2 + cos(x(1)) * Kf * La ^ 4 * u(1) * m_b ^ 2 * cos(x(2)) ^ 2 + cos(x(1)) * Kf * La ^ 4 * u(2) * m_b ^ 2 * cos(x(2)) ^ 2 - cos(x(1)) * Kf * La ^ 4 * u(1) * m_f ^ 2 * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - cos(x(1)) * g * La ^ 3 * Lw * m_f ^ 2 * m_w - cos(x(1)) * g * La * Lw ^ 3 * m_f * m_w ^ 2 + cos(x(1)) * g * La ^ 3 * Lw * m_b ^ 2 * m_w + cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + cos(x(1)) * g * La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + cos(x(1)) * g * La * Lw ^ 3 * m_b * m_w ^ 2 + cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b ^ 3 * cos(x(2)) ^ 2 + cos(x(1)) * g * La ^ 4 * m_b ^ 2 * m_f * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 4 * m_b * m_f ^ 2 * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_f ^ 3 * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b ^ 2 * m_w + sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_f ^ 2 * m_w - sin(x(2)) * sin(x(1)) * g * La * Lh * Lw ^ 2 * m_b ^ 2 * m_w - sin(x(2)) * sin(x(1)) * g * La * Lh * Lw ^ 2 * m_f ^ 2 * m_w + 0.2e1 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b * m_f * m_w - cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b ^ 2 * m_w - cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 3 * g * La * Lh ^ 2 * Lw * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) ^ 3 * g * La * Lh ^ 2 * Lw * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b ^ 3 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_f ^ 3 - cos(x(2)) * cos(x(1)) ^ 2 * g * La * Lh ^ 2 * Lw * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * g * La * Lh ^ 2 * Lw * m_f ^ 2 * m_w - cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b ^ 2 * m_f - cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b * m_f ^ 2 + 0.3e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 2 * m_f + 0.3e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_f ^ 3 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 3 - 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_f ^ 2 - 0.2e1 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b * m_f + 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_f ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_f ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_f ^ 2 - 0.2e1 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b * m_f + 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b ^ 2 - 0.2e1 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b * m_f + 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_f ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b ^ 2 + 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b ^ 2 - 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b ^ 2 - 0.2e1 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b * m_f + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_f ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_b * m_w - cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_f * m_w + cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_b * m_w - cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_f * m_w - 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_f ^ 2 - 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b ^ 2 + cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_b * m_w * cos(x(2)) ^ 2 + cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_b * m_w * cos(x(2)) ^ 2 - 0.2e1 * cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b * m_f * cos(x(1)) ^ 2 + 0.2e1 * cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b * m_f * cos(x(1)) ^ 2 - cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_f * m_w * cos(x(2)) ^ 2 - cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_f * m_w * cos(x(2)) ^ 2 + cos(x(1)) * g * La * Lh ^ 2 * Lw * m_f ^ 2 * m_w * cos(x(2)) ^ 2 - cos(x(1)) * g * La * Lh ^ 2 * Lw * m_b ^ 2 * m_w * cos(x(2)) ^ 2 + Kf * Lh ^ 4 * u(1) * m_b ^ 2 + Kf * Lh ^ 4 * u(1) * m_f ^ 2 - Kf * Lh ^ 4 * u(2) * m_b ^ 2 - Kf * Lh ^ 4 * u(2) * m_f ^ 2 + cos(x(1)) * g * La ^ 4 * m_f ^ 3 - cos(x(1)) * g * La ^ 4 * m_b ^ 3 + 0.2e1 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b * m_f * m_w - 0.2e1 * sin(x(2)) * sin(x(1)) * g * La * Lh * Lw ^ 2 * m_b * m_f * m_w - cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b ^ 2 * m_w - cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_f ^ 2 * m_w - 0.2e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b * m_f * m_w - cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b ^ 2 * m_f - cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b * m_f ^ 2 + 0.3e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 2 * m_f + 0.3e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b * m_f ^ 2 - cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_f ^ 2 * m_w - 0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 2 * m_f - 0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_f ^ 2 * m_w - sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(1) * m_b * m_w - sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(1) * m_f * m_w - sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(2) * m_b * m_w - sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(2) * m_f * m_w - 0.2e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b * m_f - 0.2e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b * m_f + 0.2e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b * m_f + 0.2e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b * m_f + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_f ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_f ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_f ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_f ^ 2 - Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b ^ 2 - Kf * La ^ 2 * Lh ^ 2 * u(2) * m_f ^ 2 + 0.2e1 * Kf * Lh ^ 4 * u(1) * m_b * m_f - 0.2e1 * Kf * Lh ^ 4 * u(2) * m_b * m_f + Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b ^ 2 + Kf * La ^ 2 * Lh ^ 2 * u(1) * m_f ^ 2 - cos(x(1)) * g * La ^ 4 * m_f ^ 3 * cos(x(2)) ^ 2 + cos(x(1)) * g * La ^ 4 * m_b ^ 3 * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b ^ 3 - cos(x(1)) * g * La ^ 4 * m_b ^ 2 * m_f + cos(x(1)) * g * La ^ 4 * m_b * m_f ^ 2 + cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_f ^ 3 + cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 4 * m_b ^ 3 - cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 4 * m_f ^ 3 - cos(x(1)) * Kf * La ^ 4 * u(1) * m_b ^ 2 - cos(x(1)) * Kf * La ^ 4 * u(2) * m_b ^ 2 + cos(x(1)) * Kf * La ^ 4 * u(1) * m_f ^ 2 + cos(x(1)) * Kf * La ^ 4 * u(2) * m_f ^ 2 - cos(x(2)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_b ^ 2 - cos(x(2)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_f ^ 2 + cos(x(2)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_b ^ 2 + cos(x(2)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_f ^ 2 + cos(x(1)) ^ 2 * Kf * La ^ 4 * u(1) * m_b ^ 2 + cos(x(1)) ^ 2 * Kf * La ^ 4 * u(1) * m_f ^ 2 - cos(x(1)) ^ 2 * Kf * La ^ 4 * u(2) * m_b ^ 2 - cos(x(1)) ^ 2 * Kf * La ^ 4 * u(2) * m_f ^ 2 - cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_b ^ 2 - cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_f ^ 2 + cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_b ^ 2 + cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_f ^ 2 + cos(x(1)) ^ 2 * Kf * Lw ^ 4 * u(1) * m_w ^ 2 - cos(x(1)) ^ 2 * Kf * Lw ^ 4 * u(2) * m_w ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh * Lw ^ 2 * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh * Lw ^ 2 * m_f ^ 2 * m_w + 0.2e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b * m_f * m_w + 0.2e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b * m_f * m_w - cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_f ^ 2 * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b * m_f * m_w + 0.2e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b * m_f + 0.2e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b * m_f + 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(1) * m_b * m_w - 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(1) * m_f * m_w - 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(2) * m_b * m_w + 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(2) * m_f * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b * m_f - 0.2e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b * m_f - 0.2e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b * m_f * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh * Lw ^ 2 * m_b * m_f * m_w + 0.2e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b * m_f * m_w) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w);
x0 = [1;1;1;1;1;1];

tic
mj = mklJac(F,x0);
toc
tic
aj = autoJac(F,x0);
toc
tic
cj = cstepJac(F,x0');
toc

aj-cj

%% Vector Fun Test
clc
%Function
i = (1:50)';
fun = @(x) x(1)*exp(-x(2)*i) + x(3);
x0 = ones(3,1);

tic
mj = mklJac(fun,x0);
toc
tic
aj = autoJac(fun,x0);
toc
tic
cj = cstepJac(fun,x0);
toc