svd.c

00001 /*******************************************************************************
00002 * E.S.O. - VLT project
00003 *
00004 *
00005 *
00006 * who       when      what
00007 * --------  --------  ----------------------------------------------
00008 * schreib  16/04/03  created
00009 */
00010 
00011 #include "svd.h"
00012 
00013 void fpol(float x, float *p, int np)
00014 {
00015     int j ;
00016     
00017     p[1] = 1.0 ;
00018     for ( j = 2 ; j <= np ; j++ )
00019     {
00020         p[j] = p[j-1]*x ;
00021     }
00022 }
00023 
00024 void svb_kas(float **u, float w[], float **v, int m, int n, float b[],float x[])
00025 
00026 
00027 {
00028         int jj,j,i;
00029         float s,*tmp,*vector();
00030         void free_vector();
00031 
00032         tmp=vector(1,n);
00033         for (j=1;j<=n;j++) {
00034                 s=0.0;
00035                 if (w[j]) {
00036                         for (i=1;i<=m;i++) s += u[i][j]*b[i];
00037                         s /= w[j];
00038                 }
00039                 tmp[j]=s;
00040         }
00041         for (j=1;j<=n;j++) {
00042                 s=0.0;
00043                 for (jj=1;jj<=n;jj++) s += v[j][jj]*tmp[jj];
00044                 x[j]=s;
00045         }
00046         free_vector(tmp,1/*,n*/);
00047 }
00048 
00049 void svd_variance(float **v , int ma , float w[] , float **cvm)
00050 {
00051         int k,j,i;
00052         float sum,*wti,*vector();
00053         void free_vector();
00054 
00055         wti=vector(1,ma);
00056         for (i=1;i<=ma;i++) {
00057                 wti[i]=0.0;
00058                 if (w[i]) wti[i]=1.0/(w[i]*w[i]);
00059         }
00060         for (i=1;i<=ma;i++) {
00061                 for (j=1;j<=i;j++) {
00062                         for (sum=0.0,k=1;k<=ma;k++) sum += (v[i][k]*v[j][k]*wti[k]);
00063                         cvm[j][i]=cvm[i][j]=sum;
00064                 }
00065         }
00066         free_vector(wti,1/*,ma*/);
00067 }
00068 
00069 #define TOL 1.0e-5
00070 
00071 void svd_fitting ( float *x,
00072                    float *y,
00073                    float *sig,
00074                    int   ndata,
00075                    float *a,
00076                    int   ma,
00077                    float **u,
00078                    float **v,
00079                    float *w,
00080                    float **cvm,
00081                    float *chisq,
00082                    void (*funcs)(float,float *,int) )
00083 {
00084         int j,i;
00085         float /*sini,*/wmax,tmp,thresh,sum,*b,*afunc,*vector();
00086         void svdksb(float **u,float w[],float **v, int m, int n, float b[],float x[]);
00087         void svd_compare(float **a,int m,int n, float w[], float **v);
00088         void free_vector();
00089 
00090 
00091         b=vector(1,ndata);
00092         afunc=vector(1,ma);
00093         for (i=1;i<=ndata;i++) {
00094 
00095                 (*funcs)(x[i],afunc,ma);
00096                 tmp=1.0/sig[i];
00097                 for (j=1;j<=ma;j++) {
00098                         u[i][j]=afunc[j]*tmp;
00099                 }
00100                 b[i]=y[i]*tmp;
00101         }
00102         svd_compare(u,ndata,ma,w,v);
00103 
00104         wmax=0.0;
00105         for (j=1;j<=ma;j++)
00106                 if (w[j] > wmax) wmax=w[j];
00107         thresh=TOL*wmax;
00108         for (j=1;j<=ma;j++) {
00109                 if (w[j] < thresh) {
00110                         w[j]=0.0;
00111                         printf("WARNING : SVD_FITTING detected singular value in fit !");
00112                 }
00113         }
00114         svb_kas(u,w,v,ndata,ma,b,a);
00115         *chisq=0.0;
00116         for (i=1;i<=ndata;i++) {
00117                 (*funcs)(x[i],afunc,ma);
00118                 for (sum=0.0,j=1;j<=ma;j++) sum += a[j]*afunc[j];
00119                 *chisq += (tmp=(y[i]-sum)/sig[i],tmp*tmp);
00120         }
00121         free_vector(afunc,1/*,ma*/);
00122         free_vector(b,1/*,ndata*/);
00123         svd_variance(v,ma,w,cvm);
00124 
00125 }
00126 
00127 #undef TOL
00128 
00129 
00130 
00131 static float at,bt,ct;
00132 #define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? \
00133 (ct=bt/at,at*sqrt(1.0+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)): 0.0))
00134 
00135 
00136 static float maxarg1,maxarg2;
00137 #define MAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\
00138         (maxarg1) : (maxarg2))
00139 #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
00140 
00141 void svd_compare(float **a,int m,int n,float w[],float **v)
00142 {
00143         int flag,i,its,j,jj,k,l=0,nm=0;
00144         float c,f,h,s,x,y,z;
00145         float anorm=0.0,g=0.0,scale=0.0;
00146         float *rv1,*vector();
00147         void nerror(const char error_text[]);
00148         void free_vector();
00149 
00150         if (m < n) nerror("SVDCMP: You must augment A with extra zero rows");
00151         rv1=vector(1,n);
00152         for (i=1;i<=n;i++) {
00153                 l=i+1;
00154                 rv1[i]=scale*g;
00155                 g=s=scale=0.0;
00156                 if (i <= m) {
00157                         for (k=i;k<=m;k++) scale += fabs(a[k][i]);
00158                         if (scale) {
00159                                 for (k=i;k<=m;k++) {
00160                                         a[k][i] /= scale;
00161                                         s += a[k][i]*a[k][i];
00162                                 }
00163                                 f=a[i][i];
00164 
00165                                 g = -SIGN(sqrt(s),f);
00166                                 h=f*g-s;
00167                                 a[i][i]=f-g;
00168                                 if (i != n) {
00169                                         for (j=l;j<=n;j++) {
00170                                                 for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
00171                                                 f=s/h;
00172                                                 for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
00173                                         }
00174                                 }
00175                                 for (k=i;k<=m;k++) a[k][i] *= scale;
00176                         }
00177                 }
00178                 w[i]=scale*g;
00179                 g=s=scale=0.0;
00180                 if (i <= m && i != n) {
00181                         for (k=l;k<=n;k++) scale += fabs(a[i][k]);
00182                         if (scale) {
00183                                 for (k=l;k<=n;k++) {
00184                                         a[i][k] /= scale;
00185                                         s += a[i][k]*a[i][k];
00186                                 }
00187                                 f=a[i][l];
00188 
00189                                 g = -SIGN(sqrt(s),f);
00190                                 h=f*g-s;
00191                                 a[i][l]=f-g;
00192                                 for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
00193                                 if (i != m) {
00194                                         for (j=l;j<=m;j++) {
00195                                                 for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
00196                                                 for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
00197                                         }
00198                                 }
00199                                 for (k=l;k<=n;k++) a[i][k] *= scale;
00200                         }
00201                 }
00202                 anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
00203         }
00204 
00205         for (i=n;i>=1;i--) {
00206                 if (i < n) {
00207                         if (g) {
00208                                 for (j=l;j<=n;j++)
00209                                         v[j][i]=(a[i][j]/a[i][l])/g;
00210                                 for (j=l;j<=n;j++) {
00211                                         for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
00212                                         for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
00213                                 }
00214                         }
00215                         for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
00216                 }
00217                 v[i][i]=1.0;
00218                 g=rv1[i];
00219                 l=i;
00220         }
00221         for (i=n;i>=1;i--) {
00222                 l=i+1;
00223                 g=w[i];
00224                 if (i < n)
00225                         for (j=l;j<=n;j++) a[i][j]=0.0;
00226                 if (g) {
00227                         g=1.0/g;
00228                         if (i != n) {
00229                                 for (j=l;j<=n;j++) {
00230                                         for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
00231                                         f=(s/a[i][i])*g;
00232                                         for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
00233                                 }
00234                         }
00235                         for (j=i;j<=m;j++) a[j][i] *= g;
00236                 } else {
00237                         for (j=i;j<=m;j++) a[j][i]=0.0;
00238                 }
00239                 ++a[i][i];
00240         }
00241         for (k=n;k>=1;k--) {
00242                 for (its=1;its<=30;its++) {
00243                         flag=1;
00244                         for (l=k;l>=1;l--) {
00245                                 nm=l-1;
00246                                 if (fabs(rv1[l])+anorm == anorm) {
00247                                         flag=0;
00248                                         break;
00249                                 }
00250                                 if (fabs(w[nm])+anorm == anorm) break;
00251                         }
00252                         if (flag) {
00253                                 c=0.0;
00254                                 s=1.0;
00255                                 for (i=l;i<=k;i++) {
00256                                         f=s*rv1[i];
00257                                         if (fabs(f)+anorm != anorm) {
00258                                                 g=w[i];
00259 
00260                                                 h=PYTHAG(f,g);
00261                                                 w[i]=h;
00262                                                 h=1.0/h;
00263                                                 c=g*h;
00264                                                 s=(-f*h);
00265                                                 for (j=1;j<=m;j++) {
00266                                                         y=a[j][nm];
00267                                                         z=a[j][i];
00268                                                         a[j][nm]=y*c+z*s;
00269                                                         a[j][i]=z*c-y*s;
00270                                                 }
00271                                         }
00272                                 }
00273                         }
00274                         z=w[k];
00275                         if (l == k) {
00276                                 if (z < 0.0) {
00277                                         w[k] = -z;
00278                                         for (j=1;j<=n;j++) v[j][k]=(-v[j][k]);
00279                                 }
00280                                 break;
00281                         }
00282                         if (its == 30) nerror("No convergence in 30 SVDCMP iterations");
00283                         x=w[l];
00284                         nm=k-1;
00285                         y=w[nm];
00286                         g=rv1[nm];
00287                         h=rv1[k];
00288                         f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
00289 
00290                         g=PYTHAG(f,1.0);
00291                         f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
00292                         c=s=1.0;
00293                         for (j=l;j<=nm;j++) {
00294                                 i=j+1;
00295                                 g=rv1[i];
00296                                 y=w[i];
00297                                 h=s*g;
00298                                 g=c*g;
00299 
00300                                 z=PYTHAG(f,h);
00301                                 rv1[j]=z;
00302                                 c=f/z;
00303                                 s=h/z;
00304                                 f=x*c+g*s;
00305                                 g=g*c-x*s;
00306                                 h=y*s;
00307                                 y=y*c;
00308                                 for (jj=1;jj<=n;jj++) {
00309                                         x=v[jj][j];
00310                                         z=v[jj][i];
00311                                         v[jj][j]=x*c+z*s;
00312                                         v[jj][i]=z*c-x*s;
00313                                 }
00314 
00315                                 z=PYTHAG(f,h);
00316                                 w[j]=z;
00317                                 if (z) {
00318                                         z=1.0/z;
00319                                         c=f*z;
00320                                         s=h*z;
00321                                 }
00322                                 f=(c*g)+(s*y);
00323                                 x=(c*y)-(s*g);
00324                                 for (jj=1;jj<=m;jj++) {
00325                                         y=a[jj][j];
00326                                         z=a[jj][i];
00327                                         a[jj][j]=y*c+z*s;
00328                                         a[jj][i]=z*c-y*s;
00329                                 }
00330                         }
00331                         rv1[l]=0.0;
00332                         rv1[k]=f;
00333                         w[k]=x;
00334                 }
00335         }
00336         free_vector(rv1,1/*,n*/);
00337 }
00338 
00339 #undef SIGN
00340 #undef MAX
00341 #undef PYTHAG
00342 
00343 #define NR_END 1
00344 #define FREE_ARG char*
00345 
00346 void nerror(const char error_text[])
00347 {
00348         fprintf(stderr,"Runtime ERROR ...\n");
00349         fprintf(stderr,"%s\n",error_text);
00350         fprintf(stderr,"exiting system \n");
00351         exit(1);
00352 
00353 }
00354 
00355 float *vector(long nl, long nh)
00356 {
00357         float *v;
00358 
00359         v=(float *)cpl_malloc((size_t) ((nh-nl+1+NR_END)*sizeof(float)));
00360         if (!v) nerror("allocation failure in vector()");
00361         return v-nl+NR_END;
00362 
00363 }
00364 
00365 void free_vector(float *v, long nl/* , long nh*/)
00366 
00367 {
00368         cpl_free((FREE_ARG) (v+nl-NR_END));
00369 }
00370 
00371 float **matrix(long nrl, long nrh, long ncl, long nch)
00372 {
00373         long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
00374         float **m;
00375 
00376         m=(float **) cpl_malloc((size_t)((nrow+NR_END)*sizeof(float*)));
00377         if (!m) nerror("aloccation failure 1 in matrix()");
00378         m += NR_END;
00379         m -= nrl;
00380 
00381         m[nrl] = (float *) cpl_malloc((size_t)((nrow*ncol+NR_END)*sizeof(float)));
00382         if (!m[nrl]) nerror("allocation failure 2 in matrix()");
00383         m[nrl] += NR_END;
00384         m[nrl] -= ncl;
00385 
00386         for(i=nrl+1;i<=nrh;i++) m[i] = m[i-1]+ncol;
00387         return m;
00388 }
00389 
00390 void free_matrix(float **m,long nrl/*, long nrh*/, long ncl/*, long nch*/)
00391 {
00392         cpl_free((FREE_ARG)(m[nrl]+ncl-NR_END));
00393         cpl_free((FREE_ARG)(m+nrl-NR_END));
00394 }
00395 

Generated on Wed Oct 26 13:08:56 2005 for SINFONI Pipeline Reference Manual by doxygen1.2.13.1 written by Dimitri van Heesch, © 1997-2001