clear all;
clc;
y=[20-50i -10+20i -10+30i;-10+20i 26-52i -16+32i;-10+30i -16+32i 26-62i;];
Y=abs(y)
th=angle(y)
p2s=-4;
q2s=-2.50;
p3s=2;
v1=1.05;
v2=1;
v3=1.04;
del1=0;
del2=0;
del3=0;
p2=abs(v2)*abs(v1)*Y(2,1)*cos(th(2,1)- del2+del1)+abs(v2^2)*Y(2,2)* cos(th(2,2))+abs(v2)* abs(v3)*Y(2,3)*cos(th(2,3)- del2+del3);
p3=abs(v3)*abs(v1)*Y(3,1)*cos(th(3,1)-del3+del1)+abs(v3^2)* Y(3,3)*cos(th(3,3))+abs(v3)* abs(v2)*Y(3,2)*cos(th(3,2)-del3+del2);
q2=-(abs(v2)*abs(v1)*Y(2,1)*sin(th(2,1)-del2+del1))-(abs(v2^2)*Y(2,2)*sin(th(2,2)))-(abs(v2)*abs(v3)*Y(2,3)*sin(th(2,3)-del2+del3));
Dp2=p2s-p2;
Dp3=p3s-p3;
Dq2=q2s-q2;
j11=(abs(v2)*abs(v1)*Y(2,1)*sin(th(2,1)-del2+del1))+(abs(v2)*abs(v3)*Y(2,3)*sin(th(2,3)-del2+del3));
j12=-(abs(v2)*abs(v3)*Y(2,3)*sin(th(2,3)-del2+del3));
j13=(abs(v1)*Y(2,1)*cos(th(2,1)-del2+del1))+(2*abs(v2)*Y(2,2)*cos(th(2,2)))+(abs(v3)*Y(2,3)*cos(th(2,3)-del2+del3));
j21=-(abs(v3)*abs(v2)*Y(3,2)*sin(th(3,2)-del3+del2));
j22=(abs(v3)*abs(v1)*Y(3,1)*sin(th(3,1)-del3+del1))+(abs(v3)*abs(v2)*Y(3,2)*sin(th(3,2)-del3+del2));
j23=(abs(v3)*Y(3,2)*cos(th(3,2)-del3+del2));
j31=(abs(v2)*abs(v1)*Y(2,1)*cos(th(2,1)-del2+del1))+(abs(v2)*abs(v3)*Y(2,3)*cos(th(2,3)-del2+del3));
j32=-(abs(v2)*abs(v3)*Y(2,3)*cos(th(2,3)-del2+del3));
j33=-(abs(v1)*Y(2,1)*sin(th(2,1)-del2+del1))-(2*abs(v2)*Y(2,2)*sin(th(2,2)))-(abs(v3)* Y(2,3) *sin(th(2,3)-del2+del3));
J=[j11 j12 j13;j21 j22 j23;j31 j32 j33]
PQ=[Dp2;Dp3;Dq2]
DV=inv(J)*PQ
del2=DV(1,1)+del2
del3=DV(2,1)+del3
v2=DV(3,1)+v2
p1=(abs(v1^2)*Y(1,1)*cos(th(1,1)))+(abs(v1)*abs(v2)*Y(1,2)*cos(th(1,2)-del1+del2))+ (abs(v1)* abs(v3)*Y(1,3)*cos(th(1,3)-del1+del3))
q1=-(abs(v1^2)*Y(1,1)*sin(th(1,1)))-(abs(v1)*abs(v2)*Y(1,2)*sin(th(1,2)-del1+del2))-(abs(v1)* abs(v3)*Y(1,3)*sin(th(1,3)-del1+del3))
q3=-(abs(v3)*abs(v1)*Y(3,1)*sin(th(3,1)-del3+del1))-(abs(v3)*abs(v2)*Y(3,2)*sin(th(3,2)-del3+del2)) -(abs(v3^2)*Y(3,3)*sin(th(3,3)))
