Perhaps someone experienced and comfortable with the svdfit procedure can look this over? I'm still really stuck here. What I'm really trying to achieve here is a replacement for the procedure MultiLinReg which is apart of the SDL Component Suite from www.lohninger.com. The results of the two procedures differ by orders of magnitude.
In the following, SMatrix is typical of any matrix class, with the operator () overloaded for element lookup.
#include <math.h>
#include <algorithm>
#include <vector>
#include "SMatrix.h"
const float TOL = 1.0e-5f;
template <typename T>
inline T sqr(T x) {
return (x * x);
}
template <typename T>
inline T pythag(T a,T b) {
a = fabs(a);
b = fabs(b);
return (a > b ? (a * sqrt(1.0f + sqr(b / a))) :
(b == 0.0f ? 0.0f : b * sqrt(1.0f + sqr(a / b))));
}
template <typename T>
inline T sign(T a,T b) {
return (b >= 0.0) ? fabs(a) : -fabs(a);
}
template <typename T>
int svdcmp(SMatrix<T> &A,std::vector<T> &W,SMatrix<T> &V) {
int m = A.getRows();
int n = A.getCols();
T anorm, c, f, g, h, s, scale, x, y, z;
std::vector<T> tmp(n);
g = scale = anorm = 0.0;
for(int i=0; i<n; i++)
{
int l=i+1;
tmp[i]=scale*g;
g=0.0;
scale=0.0;
if(i<m)
{
for(int k=i; k<m; k++)
scale+=fabs(A(k,i));
if(scale)
{
T sum=0.0;
for(int k=i; k<m; k++)
{
A(k,i)/=scale;
sum += A(k,i) * A(k,i);
}
f=A(i,i);
g= -sign(sqrt(sum),f);
h=f*g-sum;
A(i,i)=f-g;
for(int j=l; j<n; j++)
{
T sum=0.0;
for(int k=i; k<m; k++)
sum += A(k,i) * A(k,j);
f=sum/h;
for(int k=i; k<m; k++)
A(k,j) += f * A(k,i);
}
for(int k=i; k<m; k++)
A(k,i) *= scale;
}
}
W[i]=scale*g;
g=0.0;
scale=0.0;
if(i<m && i!=n)
{
for(int k=l; k<n; k++)
scale += fabs(A(i,k));
if(scale)
{
T sum=0.0;
for(int k=l; k<n; k++)
{
A(i,k) /= scale;
sum += A(i,k) * A(i,k);
}
f=A(i,l);
g= -sign(sqrt(sum), f);
h=f*g-sum;
A(i,l)=f-g;
for(int k=l; k<n; k++)
tmp[k] = A(i,k) / h;
for(int j=l; j<m; j++)
{
T sum=0.0;
for(int k=l; k<n; k++)
sum += A(j,k) * A(i,k);
for(int k=l; k<n; k++)
A(j,k) += sum * tmp[k];
}
for(int k=l; k<n; k++)
A(i,k) *= scale;
}
}
anorm=std::max( anorm, fabs(W[i])+fabs(tmp[i]) );
}
for(int i=n-1,l=n; i>=0; i--)
{
if(i<n-1)
{
if(g)
{
for(int j=l; j<n; j++)
V(j,i) = (A(i,j) / A(i,l)) / g;
for(int j=l; j<n; j++)
{
T sum=0.0;
for(int k=l; k<n; k++)
sum += A(i,k) * V(k,j);
for(int k=l; k<n; k++)
V(k,j) += sum * V(k,i);
}
}
for(int j=l; j<n; j++)
V(i,j) = V(j,i) = 0.0;
}
V(i,i)=1.0;
g=tmp[i];
l=i;
}
for(int i=std::min(m,n)-1; i>=0; i--)
{
int l=i+1;
g=W[i];
for(int j=l; j<n; j++)
A(i,j) = 0.0f;
if(g)
{
g=1.0f/g;
for(int j=l; j<n; j++)
{
T sum=0.0;
for(int k=l; k<m; k++)
sum += A(k,i) * A(k,j);
f=sum/A(i,i)*g;
for(int k=i; k<m; k++)
A(k,j)+=f*A(k,i);
}
for(int j=i; j<m; j++)
A(j,i) *= g;
}
else {
for(int j=i; j<m; j++) {
A(j,i)=0.0;
}
}
A(i,i) += 1.0f;
}
for(int k=n-1; k>=0; k--)
{
for(int its=1,l; its<=30; its++)
{
int nm, flag = 1;
for(l=k; l>=0; l--)
{
nm=l-1;
// Theoretically, tmp[1] must be 0...
if( fabs(tmp[l]) + anorm == anorm ) {
flag = 0;
break;
}
if( fabs(W[nm]) + anorm == anorm ) break;
}
if(flag) {
c=0.0;
s=1.0;
for(int i=l; i<=k; i++) /* whats up here ?? */
{
f=s*tmp[i];
tmp[i]=c*tmp[i];
if( fabs(f) + anorm == anorm) break;
g=W[i];
h=pythag(f,g);
W[i]=h;
h=1.0f/h;
c=g*h;
s= -f*h;
for(int j=0; j<m; j++)
{
y=A(j,nm);
z=A(j,i);
A(j,nm) = y*c + z*s;
A(j,i) = z*c - y*s;
}
}
}
z=W[k];
if(l==k)
{
if(z<0.0)
{
// Make sure the singular value is >=0.
W[k]=-z;
for(int j=0; j<n; j++)
V(j,k)= -V(j,k);
}
break;
}
if(its==60)
return 1;
x=W[l];
nm=k-1;
y=W[nm];
g=tmp[nm];
h=tmp[k];
f=((y-z)*(y+z) + (g-h)*(g+h)) / (2.0f*h*y);
g=pythag(f,1.0f);
f=((x-z)*(x+z) + h*((y / (f + sign(g,f))) - h)) / x;
c=s=1.0;
// Basically a QR transformation:
for(int j=l; j<=nm; j++)
{
int i=j+1;
g=tmp[i];
y=W[i];
h=s*g;
g=c*g;
z=pythag(f,h);
tmp[j]=z;
c=f/z;
s=h/z;
f=x*c + g*s;
g=g*c - x*s;
h=y*s;
y*=c;
for(int ii=0; ii<n; ii++)
{
x=V(ii,j);
z=V(ii,i);
V(ii,j)=x*c + z*s;
V(ii,i)=z*c - x*s;
}
z=pythag(f,h);
W[j]=z;
if(z)
{
z=1.0f/z;
c=f*z;
s=h*z;
}
f=c*g + s*y;
x=c*y - s*g;
for(int ii=0; ii<m; ii++)
{
y=A(ii,j);
z=A(ii,i);
A(ii,j)=y*c + z*s;
A(ii,i)=z*c - y*s;
}
}
tmp[l]=0.0;
tmp[k]=f;
W[k]=x;
}
}
return 0;
}
template <typename T>
void svbksb(const SMatrix<T> &U,const std::vector<T> &W,
const SMatrix<T> &V,const std::vector<T> &b,std::vector<T> &x) {
int m = U.getRows();
int n = U.getCols();
std::vector<T> tmp(n);
for(int j=0;j<n;j++) {
T s=0.0;
if(W[j]!=0.0f) {
for(int i=0;i<m;i++)
s += U(i,j)*b[i];
s /= W[j];
}
tmp[j]=s;
}
for(int j=0;j<n;j++) {
T s=0.0;
for(int jj=0;jj<n;jj++)
s += V(j,jj)*tmp[jj];
x[j]=s;
}
}
template <typename T>
int multilinreg(const SMatrix<T> &A,const std::vector<T> &b,std::vector<T> &x) {
int m = A.getRows();
int n = A.getCols();
SMatrix<T> TMP = A;
SMatrix<T> V(n,n);
std::vector<T> W(n);
T wmax, thresh;
int r = svdcmp(TMP,W,V);
if(r != 0) return r;
wmax = 0.0;
for(int j=0;j<n;j++) {
if(W[j]>wmax) wmax=W[j];
}
thresh=TOL*wmax;
for(int j=0;j<n;j++) {
if(W[j] < thresh) W[j]=0.0;
}
svbksb(TMP,W,V,b,x);
}
return 0;
}