function  [z,x]=ConstrainedTLS(C,p);
% CONSTRAINEDTLS computes constrained total least squares solution
%   [z,x]=ConstrainedTLS(C,p); computes the constrained total least
%   squares solution with C=[A_1 A_2 b], where A1=C(:,1:p) is without
%   errors. One therefore solves [A_1 A_2]x ~b by modifying A2 and b
%   such that [A_1 A_2 b]z=0 or [A_1 A_2] x=b

[m,n]=size(C); 
[Q,R]=qr(C);
R11=R(1:p,1:p); R12=R(1:p,p+1:n);
R22=R(p+1:n,p+1:n);
[m1,n1]=size(R22);
[x,v]=TLS(R22(:,1:n1-1),R22(:,n1));
u=-R11\R12*v;
z=[u;v]; x=-z(1:n-1)/z(n);