00001 #include <iostream>
00002 #include "tnt_array1d.h"
00003 #include "tnt_array2d.h"
00004 #include "linalg.h"
00005 #include "lsqrinvert.h"
00006 #include "jama_qr.h"
00007
00008 using namespace TNT;
00009 using namespace JAMA;
00010 using namespace std;
00011
00012 #define _EPSILON 1e-8
00013
00014
00016
00017 int LeastSquaresInvert(
00018 const Array1D<double>& data,
00019 const Array1D<double>& err,
00020 const Array2D<double> &K,
00021 Array1D<double> &m,
00022 Array2D<double> &Dm)
00023 {
00024 if (!compare_dim(data,err,K)) exit(-1);
00025
00026
00027 Array2D<double> KT(transpose(K));
00028
00029
00030 Array2D<double> B(data.dim(),data.dim());
00031 B=0.0;
00032 for (int i=0;i<data.dim();i++){
00033 B[i][i]=1.0/(err[i])/(err[i]);
00034 }
00035
00036
00037 KT=matmult(KT,B);
00038 Dm=svdinvert(matmult(KT,K));
00039 m=Dm*(KT*data);
00040
00041 return 1;
00042 }
00043
00044
00046
00047 int LeastSquaresInvert(
00048 const Array1D<double>& data,
00049 const Array2D<double>& covmtx,
00050 const Array2D<double> &K,
00051 Array1D<double> &m,
00052 Array2D<double> &Dm)
00053 {
00054
00055 if (!compare_dim(data,covmtx,K)) exit(-1);
00056
00057
00058 Array2D<double> KT(transpose(K));
00059
00060
00061 Array2D<double> B(data.dim(),data.dim());
00062 B=svdinvert(covmtx);
00063
00064
00065 KT=matmult(KT,B);
00066 Dm=svdinvert(matmult(KT,K));
00067 m=Dm*(KT*data);
00068
00069 return 1;
00070 }
00071
00072
00075
00076 int SVDConstrainedLeastSquaresInvert(
00077 const Array1D<double>& data,
00078 const Array1D<double>& err,
00079 const Array2D<double>& K,
00080 const Array2D<double> &C,
00081 const Array1D<double> &c,
00082 Array1D<double> &m,
00083 Array2D<double> &Dm){
00084
00085
00086 Array2D<double> B(data.dim(),data.dim());
00087 B=0.0;
00088 for (int i=0;i<data.dim();i++){
00089 B[i][i]=(err[i])*(err[i]);
00090 }
00091
00092 return SVDConstrainedLeastSquaresInvert(data,B,K,C,c,m,Dm);
00093
00094 }
00095
00096
00099
00100 int SVDConstrainedLeastSquaresInvert(
00101 const Array1D<double>& data,
00102 const Array2D<double>& covmtx,
00103 const Array2D<double>& K,
00104 const Array2D<double>& C,
00105 const Array1D<double>& c,
00106 Array1D<double>& m,
00107 Array2D<double>& Dm
00108 ){
00109
00110 if (!compare_dim(data,covmtx,K,C,c)) exit(-1);
00111
00112
00113 Array2D<double> KT(transpose(K));
00114
00115
00116 Array2D<double> B(data.dim(),data.dim());
00117 B=svdinvert(covmtx);
00118
00119
00120 KT=matmult(KT,B);
00121 Array2D<double> Dminv(matmult(KT,K));
00122 Dm=svdinvert(Dminv);
00123 Array1D<double> x(KT*data);
00124 m=Dm*x;
00125
00126
00127
00128 double chi2_data=0.0, chi2_cons=0.0;
00129 {
00130 Array1D<double> dum1(K*m-data), dum2(B*dum1);
00131 chi2_data=inner_prod(dum1,dum2);
00132 }
00133 {
00134 Array1D<double> dum1(C*m-c);
00135 chi2_cons=inner_prod(dum1,dum1);
00136 }
00137
00138
00139
00140 double trade;
00141
00142 if ((chi2_data>_EPSILON)&&(chi2_cons>_EPSILON))
00143 trade=1e2*std::max(chi2_data/chi2_cons,chi2_data);
00144 else trade=1e2*std::max(static_cast<double>(data.dim()),chi2_data);
00145
00146
00147
00148
00149 Dm=svdinvert(Dminv+trade*matmult(transpose(C),C));
00150 m=Dm*(x+trade*(transpose(C)*c));
00151
00152
00153 return 1;
00154 }
00155
00156
00159
00160 int QRConstrainedLeastSquaresInvert(
00161 const Array1D<double>& data,
00162 const Array1D<double>& err,
00163 const Array2D<double>& K,
00164 const Array2D<double> &C,
00165 const Array1D<double> &c,
00166 Array1D<double> &m,
00167 Array2D<double> &Dm){
00168
00169
00170 Array2D<double> B(data.dim(),data.dim());
00171 B=0.0;
00172 for (int i=0;i<data.dim();i++){
00173 B[i][i]=(err[i])*(err[i]);
00174 }
00175
00176 return QRConstrainedLeastSquaresInvert(data,B,K,C,c,m,Dm);
00177
00178 }
00179
00180
00183
00184 int QRConstrainedLeastSquaresInvert(
00185 const Array1D<double>& data,
00186 const Array2D<double>& covmtx,
00187 const Array2D<double>& K,
00188 const Array2D<double>& C,
00189 const Array1D<double>& c,
00190 Array1D<double>& m,
00191 Array2D<double>& Dm
00192 ){
00193
00194 if (!compare_dim(data,covmtx,K,C,c)) exit(-1);
00195
00196
00197 Array2D<double> KT(transpose(K));
00198
00199
00200 Array2D<double> B(data.dim(),data.dim());
00201 B=svdinvert(covmtx);
00202
00203
00204 KT=matmult(KT,B);
00205 Array2D<double> Dminv(matmult(KT,K));
00206
00207
00208 Array1D<double> y(uplow_block(KT*data,c));
00209 Array2D<double> A(uplow_block(Dminv,C));
00210
00211
00212 QR<double> decomp(A);
00213 Array2D<double> QT(transpose(decomp.getQ()));
00214 Array2D<double> R(decomp.getR());
00215
00216
00217 m=decomp.solve(y);
00218
00219
00220 Dm=svdinvert(A);
00221
00222
00223 return !decomp.isFullRank();
00224 }
00225
00226
00227
00228
00229
00230
00231
00232 bool compare_dim(
00233 const Array1D<double>& data,
00234 const Array1D<double>& err,
00235 const Array2D<double>& K){
00236
00237 if (data.dim()!=K.dim1())
00238 {
00239 cerr<< "data and kernel have different dimensions!"<<endl;
00240 return 0;
00241 }
00242 if (data.dim()!=err.dim())
00243 {
00244 cerr<< "data vector and error vector have different dimensions!"<<endl;
00245 return 0;
00246 }
00247
00248 return 1;
00249 }
00250
00251 bool compare_dim(
00252 const Array1D<double>& data,
00253 const Array1D<double>& err,
00254 const Array2D<double>& K,
00255 const Array2D<double>& C,
00256 const Array1D<double>& c
00257 ){
00258
00259 if (!compare_dim(data,err,K)) return 0;
00260
00261
00262 if (c.dim()!=C.dim1())
00263 {
00264 cerr<< "constraint vector and constraint matrix have different dimensions!"<<endl;
00265 return 0;
00266 }
00267
00268 return 1;
00269 }
00270
00271 bool compare_dim(
00272 const Array1D<double>& data,
00273 const Array2D<double>& covmtx,
00274 const Array2D<double>& K
00275 ){
00276
00277 if (data.dim()!=K.dim1())
00278 {
00279 cerr<< "data and kernel have different dimensions!"<<endl;
00280 return 0;
00281 }
00282 if (data.dim()!=covmtx.dim1())
00283 {
00284 cerr<< "data vector and data covarience matrix have different dimensions!"<<endl;
00285 return 0;
00286 }
00287 if (covmtx.dim2()!=covmtx.dim1())
00288 {
00289 cerr<< "ddata covarience matrix is not square!"<<endl;
00290 return 0;
00291 }
00292
00293 return 1;
00294 }
00295 bool compare_dim(
00296 const Array1D<double>& data,
00297 const Array2D<double>& covmtx,
00298 const Array2D<double>& K,
00299 const Array2D<double>& C,
00300 const Array1D<double>& c
00301 ){
00302 if (!compare_dim(data,covmtx,K)) return 0;
00303
00304
00305 if (c.dim()!=C.dim1())
00306 {
00307 cerr<< "constraint vector and constraint matrix have different dimensions!"<<endl;
00308 return 0;
00309 }
00310
00311 return 1;
00312 }
00313
00314
00315
00316
00317
00318 bool CLSqrInvert::solve(Array1D<double> data){
00319 Array1D<double> fakerr(data.dim1(),1.);
00320 return solve(data,fakerr);
00321 }
00322
00323
00324 bool CLSqrInvert::solve(Array1D<double> data, Array1D<double> err){
00325 return solve(data,err2covmtx(err));
00326 }
00327
00328
00329 bool CLSqrInvert::solve(Array1D<double> data, Array2D<double> cov){
00330
00331 if (!check_data_dim(data,cov,K)) exit(-1);
00332
00333
00334 Array2D<double> KT(transpose(K));
00335
00336
00337 Array2D<double> B(data.dim(),data.dim());
00338 B=svdinvert(cov);
00339
00340
00341 KT=matmult(KT,B);
00342 covm=svdinvert(matmult(KT,K));
00343 m=covm*(KT*data);
00344
00345 return true;
00346 }
00347
00348
00349
00350 Array2D<double> CLSqrInvert::err2covmtx(Array1D<double> err){
00351 return makeDiagSquared(err);
00352 }
00353
00354
00355 Array2D<double> CLSqrInvert::corrmodel(void){
00356 Array2D<double> corr(covm.dim1(),covm.dim2(),0.);
00357 Array1D<double> err(errmodel());
00358 for (int i=0;i<corr.dim1();i++){
00359 for (int j=0;j<corr.dim2();j++){
00360 corr[i][j]=covm[i][j]/err[i]/err[j];
00361 }
00362 }
00363 return corr;
00364 }
00365
00366
00367 Array1D<double> CLSqrInvert::errmodel(void){
00368 Array1D<double> err(covm.dim1());
00369 for(int i=0;i<err.dim1();i++) err[i]=sqrt(abs(covm[i][i]));
00370 return err;
00371 }
00372
00373
00374 void CLSqrInvert::usePriorModel(Array1D<double> mprior, Array2D<double> covmprior){
00375 if (check_prior_dim(mprior,covmprior,K)){
00376 useprior=true;
00377 priorm=mprior;
00378 priorcovm=covmprior;
00379 } else {
00380 cerr << "Prior model dimensions != kernel dimensions"<<endl;
00381 useprior=false;
00382 }
00383 }
00384
00385
00386 bool CLSqrInvert::check_data_dim(
00387 const Array1D<double>& data,
00388 const Array2D<double>& covmtx,
00389 const Array2D<double>& K
00390 ){
00391 if (data.dim()!=K.dim1())
00392 {
00393 cerr<< "data and kernel have different dimensions!"<<endl;
00394 return false;
00395 }
00396 if (data.dim()!=covmtx.dim1())
00397 {
00398 cerr<< "data vector and data covarience matrix have different dimensions!"<<endl;
00399 return false;
00400 }
00401 if (covmtx.dim2()!=covmtx.dim1())
00402 {
00403 cerr<< "ddata covarience matrix is not square!"<<endl;
00404 return false;
00405 }
00406
00407 return true;
00408 }
00409
00410
00411 bool CLSqrInvert::check_prior_dim(
00412 const Array1D<double>& mprior,
00413 const Array2D<double>& covmprior,
00414 const Array2D<double>& K
00415 ){
00416 if (mprior.dim()!=K.dim2())
00417 {
00418 cerr<< "prior model and kernel have different dimensions!"<<endl;
00419 return false;
00420 }
00421 if (mprior.dim()!=covmprior.dim1())
00422 {
00423 cerr<< "prior model vector and covarience matrix have different dimensions!"<<endl;
00424 return false;
00425 }
00426 if (covmprior.dim2()!=covmprior.dim1())
00427 {
00428 cerr<< "prior model covarience matrix is not square!"<<endl;
00429 return false;
00430 }
00431
00432 return true;
00433 }
00434
00435
00436
00437
00438 bool CLSqrInvertConstrained::check_constraint_dim(
00439 const Array2D<double>& K,
00440 const Array2D<double>& conmtx,
00441 const Array1D<double>& convec
00442 ){
00443 if (conmtx.dim2()!=K.dim2())
00444 {
00445 cerr<< "constraint matrix and kernel have different dimensions!"<<endl;
00446 return false;
00447 }
00448 if (convec.dim()!=conmtx.dim1())
00449 {
00450 cerr<< "constraint vector and constraint matrix have different dimensions!"<<endl;
00451 return false;
00452 }
00453
00454 return true;
00455 }
00456
00457
00458
00459
00460 bool CLSqrInvertSVDBigGauss::solve(Array1D<double> data, Array2D<double> cov){
00461
00462 if (!check_data_dim(data,cov,K)) exit(-1);
00463
00464
00465 Array2D<double> KT(transpose(K));
00466
00467
00468 Array2D<double> B(data.dim(),data.dim());
00469 B=svdinvert(cov);
00470
00471
00472 KT=matmult(KT,B);
00473 Array2D<double> Dminv(matmult(KT,K));
00474 covm=svdinvert(Dminv);
00475 Array1D<double> x(KT*data);
00476 m=covm*x;
00477
00478
00479
00480 double chi2_data=0.0, chi2_cons=0.0;
00481 {
00482 Array1D<double> dum1(K*m-data), dum2(B*dum1);
00483 chi2_data=inner_prod(dum1,dum2);
00484 }
00485 {
00486 Array1D<double> dum1(conmtx*m-convec);
00487 chi2_cons=inner_prod(dum1,dum1);
00488 }
00489
00490 double trade;
00491
00492 if ((chi2_data>_EPSILON)&&(chi2_cons>_EPSILON))
00493 trade=1e2*std::max(chi2_data/chi2_cons,chi2_data);
00494 else trade=1e2*std::max(static_cast<double>(data.dim()),chi2_data);
00495
00496
00497
00498
00499 covm=svdinvert(Dminv+trade*matmult(transpose(conmtx),conmtx));
00500 m=covm*(x+trade*(transpose(conmtx)*convec));
00501
00502
00503 return 1;
00504 }
00505
00506
00507
00508
00509 bool CLSqrInvertQRBigGauss::solve(Array1D<double> data, Array2D<double> cov){
00510
00511 if (!check_data_dim(data,cov,K)) exit(-1);
00512
00513
00514 Array2D<double> KT(transpose(K));
00515
00516
00517 Array2D<double> B(data.dim(),data.dim());
00518 B=svdinvert(cov);
00519
00520
00521 KT=matmult(KT,B);
00522 Array2D<double> Dminv(matmult(KT,K));
00523
00524
00525 Array1D<double> y(uplow_block(KT*data,convec));
00526 Array2D<double> A(uplow_block(Dminv,conmtx));
00527
00528
00529 QR<double> decomp(A);
00530 Array2D<double> QT(transpose(decomp.getQ()));
00531 Array2D<double> R(decomp.getR());
00532
00533
00534 m=decomp.solve(y);
00535
00536
00537 covm=svdinvert(A);
00538
00539
00540 return !decomp.isFullRank();
00541 }
00542
00543
00544
00545
00546 bool CLSqrInvertSVDLagrange::solve(Array1D<double> data, Array2D<double> cov){
00547 if (!check_data_dim(data,cov,K)) exit(-1);
00548
00549
00550 Array2D<double> KT(transpose(K));
00551
00552
00553 Array2D<double> B(data.dim(),data.dim());
00554 B=svdinvert(cov);
00555
00556
00557 Array2D<double> KTB(matmult(KT,B));
00558
00559
00560 Array1D<double> y(uplow_block(KTB*data,convec));
00561
00562
00563 Array2D<double> nullmtx(conmtx.dim1(),conmtx.dim1(),0.);
00564 Array2D<double> M(uplow_block(
00565 leftright_block(matmult(KTB,K), 0.5*transpose(conmtx)),
00566 leftright_block( conmtx, nullmtx)
00567 ));
00568
00569
00570 Array2D<double> Minv(svdinvert(M));
00571 Array1D<double> x(Minv*y);
00572
00573
00574 Array1D<double> newm(conmtx.dim2());
00575 Array1D<double> newlagm(x.dim()-newm.dim());
00576 for (int i=0;i<newm.dim();i++){newm[i]=x[i];}
00577 for (int i=newm.dim();i<x.dim();i++){newlagm[i-newm.dim()]=x[i];}
00578 m=newm;
00579 lagm=newlagm;
00580
00581
00582 Array2D<double> d2y(y.dim(),y.dim(),0.);
00583 Array2D<double> invcovm(matmult(KTB,K)),newcovm(m.dim(),m.dim());
00584 for (int i=0;i<invcovm.dim1();i++){
00585 for (int j=0;j<invcovm.dim2();j++) d2y[i][j]=invcovm[i][j];
00586 }
00587 Array2D<double> d2x(matmult(matmult(Minv,d2y),transpose(Minv)));
00588 for (int i=0;i<m.dim();i++){
00589 for (int j=0;j<m.dim();j++) newcovm[i][j]=d2x[i][j];
00590 }
00591 covm=newcovm;
00592
00593
00594
00595 return true;
00596 }
00597
00598
00599
00600
00601 bool CLSqrInvertQRLagrange::solve(Array1D<double> data, Array2D<double> cov){
00602 if (!check_data_dim(data,cov,K)) exit(-1);
00603 cerr << "CLSqrInvertQRLagrange::solve not implemented yet"<<endl;
00604 return true;
00605 }
00606