Kp = [0.5;0.6;0.7]
Kd = [5;6;7]
J = [0.034504068510081;0.034441772751836;0.034460446423664]
A = [Kp(1)*Kp(1) Kp(1) 1;Kp(2)*Kp(2) Kp(2) 1;Kp(3)*Kp(3) Kp(3) 1]
Ainv = inv(A)
[a] = Ainv*J
Kpmin = -a(2)/(2*a(1))
Jmin = a(1)*Kpmin*Kpmin + a(2)*Kpmin + a(3)