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