← Back to branch summary

~jan.greis/maus/1811

« back to all changes in this revision

Viewing changes to src/common_cpp/Recon/SciFi/LeastSquaresFitter.cc

Merging start

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/common_cpp/Recon/SciFi/LeastSquaresFitter.cc
16
16
 */
17
17
 
18
18
// C headers
19
 
#include <CLHEP/Matrix/Matrix.h>
20
 
#include <CLHEP/Units/PhysicalConstants.h>
 
19
#include "TMatrixD.h"
21
20
 
22
21
// MAUS headers
23
22
#include "src/common_cpp/Recon/SciFi/LeastSquaresFitter.hh"
27
26
namespace LeastSquaresFitter {
28
27
 
29
28
void linear_fit(const std::vector<double> &_x, const std::vector<double> &_y,
30
 
                const std::vector<double> &_y_err, MAUS::SimpleLine &line) {
31
 
 
32
 
  int n_points = static_cast<int>(_x.size());
33
 
 
34
 
  CLHEP::HepMatrix A(n_points, 2); // rows, columns
35
 
  CLHEP::HepMatrix V(n_points, n_points); // error matrix
36
 
  CLHEP::HepMatrix Y(n_points, 1); // measurements
 
29
                const std::vector<double> &_y_err, MAUS::SimpleLine &line, TMatrixD& covariance) {
 
30
 
 
31
  // Set up the matrices
 
32
  int n_points = static_cast<int>(_x.size());  // Number of measurements
 
33
  TMatrixD A(n_points, 2);                     // Represents the functional form; rows, columns
 
34
  TMatrixD V_m(n_points, n_points);            // Covariance matrix of measurements
 
35
  TMatrixD Y(n_points, 1);                     // Measurements
37
36
 
38
37
  for ( int i = 0; i < static_cast<int>(_x.size()); ++i ) {
39
 
    // std::cout <<"( " << _x[i] << "," << _y[i] << " )" << std::endl;
40
38
    A[i][0] = 1;
41
39
    A[i][1] = _x[i];
42
 
    V[i][i] = ( _y_err[i] * _y_err[i] );
 
40
    V_m[i][i] = ( _y_err[i] * _y_err[i] );
43
41
    Y[i][0] = _y[i];
44
42
  }
45
43
 
46
 
  CLHEP::HepMatrix At, tmpy, yparams;
47
 
 
48
 
  int ierr;
49
 
  V.invert(ierr);
50
 
  At = A.T();
51
 
 
52
 
  tmpy = At * V * A;
53
 
  tmpy.invert(ierr);
54
 
  yparams = tmpy * At * V * Y;
55
 
 
56
 
  line.set_c(yparams[0][0]);
57
 
  line.set_m(yparams[1][0]);
58
 
  line.set_c_err(sqrt(tmpy[0][0]));
59
 
  line.set_m_err(sqrt(tmpy[1][1]));
60
 
 
61
 
  CLHEP::HepMatrix C, result;
62
 
 
63
 
  C = Y - (A * yparams);
64
 
  result = C.T() * V * C;
 
44
  // Perform the inversions and multiplications which make up the least squares fit
 
45
  double* det = NULL;                   // To hold the determinant
 
46
  V_m.Invert(det);                      // Invert in place
 
47
  TMatrixD At(A);                       // Copy A to At
 
48
  At.T();                               // Transpose At (leaving A unchanged)
 
49
  TMatrixD V_p(At * V_m * A);           // The covariance matrix of the parameters of model (inv)
 
50
  V_p.Invert(det);                      // Invert in place
 
51
  covariance = V_p;
 
52
  TMatrixD P(V_p * At * V_m * Y);       // The least sqaures estimate of the parameters
 
53
 
 
54
  // Extract the fit parameters
 
55
  line.set_c(P[0][0]);
 
56
  line.set_m(P[1][0]);
 
57
  line.set_c_err(sqrt(V_p[0][0]));
 
58
  line.set_m_err(sqrt(V_p[1][1]));
 
59
 
 
60
  // Calculate the fit chisq
 
61
  TMatrixD C(Y - (A * P));
 
62
  TMatrixD Ct(C);
 
63
  Ct.T();
 
64
  TMatrixD result(Ct * V_m * C);
65
65
  line.set_chisq(result[0][0]);
66
66
  line.set_chisq_dof(result[0][0] / n_points);
67
67
} // ~linear_fit(...)
68
68
 
69
69
bool circle_fit(const double sd_1to4, const double sd_5, const double R_res_cut,
70
 
                const std::vector<MAUS::SciFiSpacePoint*> &spnts, MAUS::SimpleCircle &circle) {
 
70
                const std::vector<MAUS::SciFiSpacePoint*> &spnts, MAUS::SimpleCircle &circle,
 
71
                TMatrixD& covariance) {
71
72
 
72
 
  int n_points = static_cast<int>(spnts.size());
73
 
  CLHEP::HepMatrix A(n_points, 3); // rows, columns
74
 
  CLHEP::HepMatrix V(n_points, n_points); // error matrix
75
 
  CLHEP::HepMatrix K(n_points, 1);
 
73
  // Set up the matrices
 
74
  int n_points = static_cast<int>(spnts.size()); // Number of measurements
 
75
  TMatrixD A(n_points, 3);                       // Represents the functional form; rows, columns
 
76
  TMatrixD V_m(n_points, n_points);              // Covariance matrix of measurements
 
77
  TMatrixD K(n_points, 1);                       // Vector of 1s, represents kappa in circle formula
76
78
 
77
79
  for ( int i = 0; i < static_cast<int>(spnts.size()); ++i ) {
78
80
    // This part will change once I figure out proper errors
89
91
    A[i][1] = x_i;
90
92
    A[i][2] = y_i;
91
93
 
92
 
    V[i][i] = ( sd * sd );
 
94
    V_m[i][i] = ( sd * sd );
93
95
    K[i][0] = 1.;
94
96
  }
95
97
 
96
 
  CLHEP::HepMatrix At, tmpx, tmp_params;
97
 
  int ierr;
98
 
  V.invert(ierr);
99
 
  At = A.T();
100
 
  tmpx = At * V * A;
101
 
  tmpx.invert(ierr);
102
 
  tmp_params = tmpx * At * V * K;
 
98
  // Perform the inversions and multiplications which make up the least squares fit
 
99
  double* det = NULL;              // To hold the determinant after inversions
 
100
  V_m.Invert(det);                 // Invert the measurement covariance matrix in place
 
101
  TMatrixD At(A);                  // Create a copy of A
 
102
  At.T();                          // Transpose At (leaving A unchanged)
 
103
  TMatrixD V_p(At * V_m * A);      // The covariance matrix of the parameters of model (inv)
 
104
  V_p.Invert(det);                 // Invert in place
 
105
  TMatrixD P(V_p * At * V_m * K);  // The least sqaures estimate of the parameters
103
106
 
104
 
  // These values will be used for delta_R calculation
 
107
  // Extract the fit parameters
105
108
  double alpha, beta, gamma;
106
 
  alpha = tmp_params[0][0];
107
 
  beta = tmp_params[1][0];
108
 
  gamma = tmp_params[2][0];
 
109
  alpha = P[0][0];
 
110
  beta = P[1][0];
 
111
  gamma = P[2][0];
109
112
 
110
113
  // Convert the linear parameters into the circle center and radius
111
114
  double x0, y0, R;
116
119
  else
117
120
    R = sqrt((4 * alpha) + (beta * beta) + (gamma * gamma)) / (2 * alpha);
118
121
 
 
122
  // Transform the covariance matrix to the same basis
 
123
  TMatrixD jacobian( 3, 3 );
 
124
  jacobian(0,0) = beta / (2.0*alpha*alpha);
 
125
  jacobian(0,1) = -1.0 / (2.0*alpha);
 
126
  jacobian(1,0) = gamma / (2.0*alpha*alpha);
 
127
  jacobian(1,2) = -1.0 / (2.0*alpha);
 
128
  jacobian(2,0) = ( -1.0/(2.0*alpha) ) * ( ( (beta*beta + gamma*gamma) / (2.0*alpha) ) + 1 ) /
 
129
                                                sqrt( ( (beta*beta + gamma*gamma) / 4.0 ) + alpha );
 
130
  jacobian(2,1) = ( beta/(4.0*alpha*alpha) ) /
 
131
                            sqrt( ( (beta*beta + gamma*gamma)/(4.0*alpha*alpha) ) + ( 1.0/alpha ) );
 
132
  jacobian(2,2) = ( gamma/(4.0*alpha*alpha) ) /
 
133
                            sqrt( ( (beta*beta + gamma*gamma)/(4.0*alpha*alpha) ) + ( 1.0/alpha ) );
 
134
  TMatrixD jacobianT(3,3);
 
135
  jacobianT.Transpose( jacobian );
 
136
 
 
137
  covariance = jacobian * V_p * jacobianT;
 
138
 
119
139
  // if ( R < 0. )
120
140
  //  std::cout << "R was < 0 but taking abs_val for physical correctness\n";
121
141
  R = fabs(R);
129
149
  circle.set_beta(beta);
130
150
  circle.set_gamma(gamma);
131
151
 
132
 
  CLHEP::HepMatrix C, result;
133
 
 
134
 
  C = K - (A * tmp_params);
135
 
  result = C.T() * V * C;
 
152
  // Calculate the fit chisq
 
153
  TMatrixD C(K - (A * P));
 
154
  TMatrixD Ct(C);
 
155
  Ct.T();
 
156
  TMatrixD result(Ct * V_m * C);
136
157
  double chi2 = result[0][0];
137
 
  circle.set_chisq(chi2); // should I leave this un-reduced?
 
158
  circle.set_chisq(chi2);       // Left unreduced (not dividing by NDF)
138
159
  return true;
139
160
} // ~circle_fit(...)
140
161