A few corrections to CoSaMP and SP MATLAB

While running simulations last night I saw some warnings about ill-conditioned matrices.
I didn’t think much of it at the time, but I awoke this morning knowing from what this problem comes.
In my original version of CoSaMP, I have the following:

```Sest = zeros(size(Phi,2),1);
utrue = Sest;
v = u;
t = 1;
T2 = [];
while (t  tol)
y = abs(Phi'*v);
[vals,z] = sort(y,'descend');
Omega = find(y >= vals(2*K));
T = union(Omega,T2);
phit = Phi(:,T);
b = abs(pinv(phit)*u);
[vals,z] = sort(b,'descend');
Sest = zeros(length(utrue),1);
Sest(T(find(b >= vals(K)))) = b(find(b >= vals(K)));
[vals,z] = sort(Sest,'descend');
T2 = z(1:K);
phit = Phi(:,T2);
b = pinv(phit)*u;
Sest = zeros(length(utrue),1);
Sest(T2) = b;
v = u - Phi(:,T2)*b;
t = t+1;
end
```

The line in bold means we will always select 2*K elements each time (K is the sparsity).
This creates problems when there are projections with magnitudes that are effectively zero, yet are blindly included.
Just to be sure, I have made the following corrections:

```prevresen = norm(v);
t = 1;
numericalprecision = 1e-12;
T2 = [];
while (t  tol)
y = abs(Phi'*v);
[vals,z] = sort(y,'descend');
Omega = find(y >= vals(2*K) & y > numericalprecision);
T = union(Omega,T2);
phit = Phi(:,T);
b = abs(pinv(phit)*u);
[vals,z] = sort(b,'descend');
Sest = zeros(length(utrue),1);
Sest(T(find(b >= vals(K) & b > numericalprecision))) = ...
b(find(b >= vals(K) & b > numericalprecision));
[vals,z] = sort(Sest,'descend');
Told = T2;
T2 = z(1:K);
phit = Phi(:,T2);
b = pinv(phit)*u;
Sest = zeros(length(utrue),1);
Sest(T2) = b;
v = u - Phi(:,T2)*b;
newresen = norm(v);
if newresen > prevresen
T2 = Told;
phit = Phi(:,T2);
b = pinv(phit)*u;
Sest = zeros(length(utrue),1);
Sest(T2) = b;
v = u - Phi(:,T2)*b;
break;
end
prevresen = newresen;
t = t+1;
end
```

Now we can be sure that up to 2*K atoms are selected each step, but there may be fewer.
My codes are here:

20110816: I have further corrected it here