//           File Jacob

fattore = (%pi)/180;

disp('Primo caso Jacobiano 2x2');
bb = [1; 1]
J = [-2, -2-sqrt(2); 0, -sqrt(2)]
JJ = J'*J;
deterJJ = det(JJ)
C = inv(JJ)
X = C * J' * bb
sol = J * X
err = bb - J * X


//disp('caso senza la pseudo-inversa');
//deterJ = det(J)
//invJ = inv(J)
//X= invJ*bb
//sol = J *X

disp('Primo caso Jacobiano 2x2 - fine');
pause;


disp('');
disp('Jacobiano con matrice 4x2');
bb = [1; 0; 1; 0]
J = [-2 (-2 -2*sin(45*fattore)); 0 -2*cos(45*fattore); 0 -2*sin(45*fattore); 0 2*cos(45*fattore)]
JJ = J'*J
C = inv(JJ)
X = C * J' * bb
sol = J * X
err = bb - J * X
errtot= norm(err)

disp('Jacobiano x2 - fine');
pause;


disp('');
disp('Jacobiano con matrice 4x2 e PESI');
bb = [1; 0; 1; 0]
J = [-2 (-2 -2*sin(45*fattore)); 0 -2*cos(45*fattore); 0 -2*sin(45*fattore); 0 2*cos(45*fattore)]
P = diag([10, 10, 1, 1])

JJ = J'*P*J
C = inv(JJ)
X = C * J' * P * bb
sol = J * X
err = bb - J * X
errtot= norm(err)

disp('Jacobiano 4x2 e PESI - fine');
pause;


// J * X = bb
// J = [U W V]
// U * X + W * X + V * X = bb;
// [U W V]'*[U W V]*X = [U W V]' * bb;
// V' * W * U' * U * W * V = V' * W * U' * bb;
// V' * W*W * V * X = V' * W * U' * bb
// W * W * V * X = V * V' * W * U' * bb;
// W * W * V * X = W * U' * bb;
// W * V * X = U' * bb;

bb = [10; 7];
J = [2 4 4; 2 3 2];
JJ = J' * J
deterJJ = det(JJ)
[U W V] = svd(JJ)
Wd = diag([1/W(1,1),1/W(2,2),0])
x =  V * Wd * U' * J' * bb

soluzione = J(1,1)*x(1) + J(1,2)*x(2) + J(1,3)*x(3)
soluzione2 = J(2,1)*x(1) + J(2,2) * x(2) + J(2,3) * x(3)
//% J* V(:,3)


disp('caso reale, sottodeterminato')
bb = [1; 0];

//% Sottodeterminato
l1 = 2; l0 = 2; alfar = 45/180*(%pi), betar = 45/180*(%pi)
J = [-l1*sin(alfar+betar), (-l1*sin(alfar+betar)-l0*sin(betar)), 1, 0; -l1*cos(alfar+betar), -l1*cos(alfar+betar)-l0*cos(betar), 0, 1]
//%J = [-2, (-2-2*sin(45*fattore)), 1, 0; 0, -2*cos(45*fattore), 0, 1]
JJ = J'*J

deterJJ = det(JJ)
[U W V] = svd(JJ)
Wd = diag([1/W(1,1),1/W(2,2),0,0])
xp =  V * Wd * U' * J' * bb

//%soluzione = J(1,1)*x(1) + J(1,2)*x(2) + J(1,3)*x(3) +  J(1,4)*x(4)
//%soluzione2 = J(2,1)*x(1) + J(2,2) * x(2) + J(2,3) * x(3) + J(2,4) * x(4)

//% x = [0.1372, -0.3933, -0.0686, 0.4437]';
V = J * xp
norma2 = norm(xp)
norma1 = norm(xp,1)

pause;
//%disp('testo');
//%xp = V * Wd * U' * J' * bb

disp('regolarizzatore');
//% Con regolarizzazione
I(4,4) = 1; I(3,3) = 1; I(2,2) = 1;
I(1,1) = 1
[Us, Ws, Vs] = svd(JJ+I)
Wsd = diag([1/Ws(1,1),1/Ws(2,2),1/Ws(3,3),1/Ws(4,4)])
xps =  Vs * Wsd * Us' * J' * bb
V = J * xps
norma2 = norm(xps)
norma1 = norm(xps,1)
diff = xps - xp

soluzione = inv(JJ+I)*J'*bb
pause;

disp('regolarizzatore piu spinto');
I(4,4) = 1; I(3,3) = 1; I(2,2) = 100;
I(1,1) = 100
detera = det(JJ+I)
inveresa = inv(JJ+I)
[Us2, Ws2, Vs2] = svd(JJ+I)
Wsd2 = diag([1/Ws2(1,1),1/Ws2(2,2),1/Ws2(3,3),1/Ws2(4,4)])
xps2 =  Vs2 * Wsd2 * Us2' * J' * bb
V = J * xps2
norma2 = norm(xps2)
norma1 = norm(xps2,1)
diff = xps2 - xp
pause;

disp('regolarizzatore piu spinto');
I(4,4) = 0.01; I(3,3) = 0.01; I(2,2) = 1;
I(1,1) = 1
detera = det(JJ+I)
inveresa = inv(JJ+I)
[Us2, Ws2, Vs2] = svd(JJ+I)
Wsd2 = diag([1/Ws2(1,1),1/Ws2(2,2),1/Ws2(3,3),1/Ws2(4,4)])
xps2 =  Vs2 * Wsd2 * Us2' * J' * bb
V = J * xps2
norma2 = norm(xps2)
norma1 = norm(xps2,1)
diff = xps2 - xp