曲线拟合

  1 #ifndef __LEAST_SQUARES_FITTING__
  2 #define __LEAST_SQUARES_FITTING__
  3 #include <map>
  4 #include <vector>
  5 #include <cmath>
  6 using namespace std;
  7 class PolyFunction {
  8     public:
  9     PolyFunction() : m_dim(0), m_expr(){}
 10     PolyFunction(vector<double>& c) : m_dim(0), m_expr()
 11         {
 12         int len = c.size();
 13         for (int i = 0; i < len; ++i) {
 14                 if (c[i]) {
 15             m_expr.insert(make_pair(i, c[i]));
 16             m_dim = i;
 17         }
 18         }
 19     }
 20     ~PolyFunction(){}
 21     int getCols()
 22     {
 23         return m_mat[0].size();
 24     }
 25 
 26     int getRows()
 27     {
 28         return m_mat.size();
 29     }
 30 
 31     double operator ()(double x)
 32     {
 33         double ans = m_expr[m_dim];
 34         for (int i = m_dim; i > 0; --i) {
 35         if (m_expr.end() != m_expr.find(i)) {
 36             ans = m_expr[i - 1] + x * ans;
 37         }
 38         }
 39         return ans;
 40     }
 41     private:
 42     int m_dim;
 43     map<int, double> m_expr;
 44 };
 45 
 46 class Matrix {
 47     public:
 48     Matrix(){}
 49     Matrix(int m, int n) : m_mat(m, vector<double>(n))
 50         {}
 51     vector<double>& operator [](int i)
 52     {
 53         return m_mat[i];
 54     }
 55     private:
 56     vector< vector<double> > m_mat;
 57 };
 58 
 59 typedef struct {
 60     size_t n;
 61     size_t p;
 62     Matrix A;
 63     Matrix Q;
 64     Matrix QSI;
 65     vector<double> S;
 66     vector<double> t;
 67     vector<double> xt;
 68     vector<double> D;
 69     } workspace;
 70 
 71 static int multifit_linear_svd(Matrix& X, vector<double>& y, 
 72     double tol,
 73     int balance,
 74     size_t& rank,
 75     vector<double>& c,
 76     Matrix& cov,
 77     double& chisq,
 78     workspace& work)
 79 {
 80     //
 81     //typedef struct {
 82     //  size_t size1;
 83     //  size_t size2;
 84     //  size_t tda;
 85     //  double * data;
 86     //  gsl_block * block;
 87     //  int owner;
 88     //} gsl_matrix;
 89     //
 90     if (X.getRows() != y.size())
 91     return -1;
 92     if (X.getCols() != c.size())
 93     return -1;
 94     if (cov.getRows() != cov.getCols())
 95     return -1;
 96     if (c.size() != cov.getCols())
 97     return -1;
 98     if (X.getRows() != work.n || X.getCols() != work.p)
 99     return -1;
100     if (tol <= 0)
101     return -1;
102 
103     const size_t n = X.getRows();
104     const size_t p = X.getCols();
105 
106     size_t i, j, p_eff;
107     Matrix& A = work.A;
108     Matrix& Q = work.Q;
109     Matrix& QSI = work.QSI;
110     vector<double>& S = work.S;
111     vector<double>& xt = work.xt;
112     vector<double>& D = work.D;
113 
114     //copy X to worspace, A <= X;
115     A = X;
116     //balance the columns of the matrix A if requested
117     if (balance)
118     linalg_balance_columns(A, D);//////////////////////////// 1
119     else {
120     for (int k = 0; k < D.size(); ++k)
121         D[k] = 1.0;
122     }
123 
124     //decompose A into U S Q^T
125     linalg_SV_decomp_mod(A, QSI, Q, S, xt);
126 
127     //solve y = A c for c
128     blas_dgemv(CblasTrans, 1.0, A, y, 0.0, xt);
129 
130     //scale the matrix Q, Q‘ = Q S^-1
131     QSI = Q;
132 
133     double alpha0 = S[0];
134     p_eff = 0;
135     for (j = 0; j < p; j++) {
136     }
137 
138     
139 
140 }
141 
142 vector<double>& LeastSquaresFitting(vector<double> x, vector<double> y, int N)
143 {
144     int M = (x.size() < y.size()) ? x.size() : y.size();
145     double chisq;
146     Matrix XX(M, N + 1);
147     vector<double> c(N + 1);
148     Matrix cov(N + 1, N + 1);
149     for (int i = 0; i < M; ++i) {
150     XX[i][0] = 1.0;
151     for (int j = 1; j < N + 1; ++j)
152         XX[i][j] = pow(x[i], j);
153     }
154     //gsl_multifit_linear_workspace *work = gsl_multifit_linear_alloc(M, N + 1);
155     //typedef struct 
156     //{
157     //size_t n; /* number of observations */
158     //size_t p; /* number of parameters */
159     //gsl_matrix * A;
160     //gsl_matrix * Q;
161     //gsl_matrix * QSI;
162     //gsl_vector * S;
163     //gsl_vector * t;
164     //gsl_vector * xt;
165     //gsl_vector * D;
166     //} gsl_multifit_linear_workspace;
167     //struct {
168     //    size_t n;
169     //    size_t p;
170     //    Matrix A;
171     //    Matrix Q;
172     //    Matrix QSI;
173     //    vector<double> S;
174     //    vector<double> t;
175     //    vector<double> xt;
176     //    vector<double> D;
177     //} 
178     workspace work = {
179     M, 
180     N + 1, 
181     Matrix(M, N + 1), 
182     Matrix(N + 1, N + 1),
183     Matrix(N + 1, N + 1), 
184     vector<double>(N + 1),
185     vector<double>(M),
186     vector<double>(N + 1),
187     vector<double>(N + 1)
188     };
189     
190     //gsl_multifit_linear(XX, y, c, cov, chisq, work);
191     multifit_linear(XX, y, 2.2204460492503131e-16/*EPSILON*/, 1, c, cov, chisq, work);
192     //return c;
193 }
194 #endif //__LEAST_SQUARES_FITTING__