function [x,E,r] = pTLS(A,b,p)
% A m-by-n, b m-by-1, 0 <=p <= n
% Assume that A has full rank and that the pTLS problem has a unique solution
% {x_pTLS, E_pTLS, r_pTLS.} Then x = x_pTLS, E = E_pTLS, and r = r_pTLS

[m,n] = size(A);
if p==n
% The ordinary LS situation
x = A\b;
E = zeros(m,n);
r = A*x - b;
elseif p==0
% The ordinary TLS situation
[x,E,r] = TLS(A,b);
else
% 1<=p<=n-1
% First transform to an equivalent form where the matrix is upper triangular...
[Q,R] = qr(A);
c = Q'*b;
% The new problem...
%
% R11 R12+F12 y c(1:p) + r1
% 0 R22+F22 z = c(p+1:n)+ r2
% 0 F32 c(n+1:m)+ r3
%
% Here, R11 = R(1:p,1:p).
% Note that for any choice of F22, F32, r2, r3 and z that satisfy
%
% 0 R22+F22 z = c(p+1:n)+ r2
% 0 F32 c(n+1:m)+ r3
%
% we can set F12 and r1 to zero and solve R11*y = c(1:p)- R12*z
% for y. The optimum choice for F22, F32, r2, and r3 is an unconstrained
% TLS problem
%
%
[z,F22F32,r2r3] = TLS(R(p+1:m,p+1:n),c(p+1:m));

% Now get x...
y = R(1:p,1:p)\(c(1:p)-R(1:p,p+1:n)*z);
x = [y;z];
% and use Q to get the optimum E and r for the original problem..
E = [zeros(m,p) Q*[zeros(p,n-p);F22F32]];
r = Q*[zeros(p,1);r2r3];
end

function [x,E,r] = TLS(A,b)
% b+r = (A+E)x and norm([b A],'fro') = min
[m,n] = size(A);
[U,S,V] = svd([A b]);
x = -V(1:n,n+1)/V(n+1,n+1);
delA = -S(n+1,n+1)*U(:,n+1)*V(:,n+1)';
E = delA(:,1:n);
r = delA(:,n+1);