00001
00002
00003
00004
00005
00006
00007
00008
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);
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);
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 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);
00122 free_vector(b,1);
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);
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)
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 ncl)
00391 {
00392 cpl_free((FREE_ARG)(m[nrl]+ncl-NR_END));
00393 cpl_free((FREE_ARG)(m+nrl-NR_END));
00394 }
00395