~maus-scifi/maus/tracker_devel

« back to all changes in this revision

Viewing changes to src/common_cpp/Recon/Kalman/KalmanSeed.cc

  • Committer: cheid001
  • Date: 2015-04-27 21:34:57 UTC
  • Revision ID: cheid001-20150427213457-cqvn7u73q1715sfm
style updates and test corrections, still doesn't pass all tests

Show diffs side-by-side

added added

removed removed

Lines of Context:
154
154
 
155
155
  // TODO: Actually propagate the track parrameters and covariance matrix back to start plane.
156
156
  //       This is an approximation.
157
 
  ThreeVector patrec_momentum( pt*cos(phi), pt*sin(phi), fabs(pt/dsdz) );
 
157
  ThreeVector patrec_momentum(pt*cos(phi), pt*sin(phi), fabs(pt/dsdz));
158
158
  double P = patrec_momentum.mag();
159
159
  double patrec_bias; // Account for two planes of energy loss
160
160
  if ( _tracker == 0 ) {
172
172
 
173
173
  TMatrixD a(_n_parameters, 1);
174
174
  a(0, 0) = x;
175
 
  a(1, 0) = px; 
 
175
  a(1, 0) = px;
176
176
  a(2, 0) = y;
177
177
  a(3, 0) = py;
178
178
  a(4, 0) = kappa;
226
226
//  }
227
227
 
228
228
  std::vector<double> cov = seed->get_covariance();
229
 
  TMatrixD patrec_covariance( _n_parameters, _n_parameters );
230
 
  TMatrixD covariance( _n_parameters, _n_parameters );
 
229
  TMatrixD patrec_covariance(_n_parameters, _n_parameters);
 
230
  TMatrixD covariance(_n_parameters, _n_parameters);
231
231
 
232
232
  if ( cov.size() != (unsigned int)_n_parameters*_n_parameters ) {
233
 
    throw MAUS::Exception( MAUS::Exception::recoverable, 
 
233
    throw MAUS::Exception(MAUS::Exception::recoverable,
234
234
                          "Dimension of covariance matrix does not match the state vector",
235
235
                          "KalmanSeed::ComputeInitalCovariance(Helical)");
236
236
  }
241
241
  double r = seed->get_R();
242
242
  double s = seed->get_line_sz_c() - _particle_charge*length*dsdz; // Path length at start plane
243
243
  double phi = s / r; // Phi at start plane // TODO: Is this the correct phi?!
244
 
  double sin = std::sin( phi );
245
 
  double cos = std::cos( phi );
246
 
  double sin_plus = std::sin( phi + TMath::PiOver2() );
247
 
  double cos_plus = std::cos( phi + TMath::PiOver2() );
 
244
  double sin = std::sin(phi);
 
245
  double cos = std::cos(phi);
 
246
  double sin_plus = std::sin(phi + TMath::PiOver2());
 
247
  double cos_plus = std::cos(phi + TMath::PiOver2());
248
248
  double ts = seed->get_dsdz();
249
249
 
250
 
  TMatrixD jacobian( _n_parameters, _n_parameters );
251
 
  jacobian(0,0) = 1.0;
252
 
  jacobian(0,2) = cos + phi*sin;
253
 
  jacobian(0,3) = -sin;
254
 
 
255
 
  jacobian(1,2) = mc*cos_plus + mc*phi*sin_plus;
256
 
  jacobian(1,3) = -mc*sin_plus;
257
 
 
258
 
  jacobian(2,1) = 1.0;
259
 
  jacobian(2,2) = sin - phi*cos;
260
 
  jacobian(2,3) = cos;
261
 
 
262
 
  jacobian(3,2) = mc*sin_plus - mc*phi*cos_plus;
263
 
  jacobian(1,3) = mc*cos_plus;
264
 
 
265
 
  jacobian(4,3) = -ts / (mc*r*r);
266
 
  jacobian(4,4) = 1.0 / (mc*r);
 
250
  TMatrixD jacobian(_n_parameters, _n_parameters);
 
251
  jacobian(0, 0) = 1.0;
 
252
  jacobian(0, 2) = cos + phi*sin;
 
253
  jacobian(0, 3) = -sin;
 
254
 
 
255
  jacobian(1, 2) = mc*cos_plus + mc*phi*sin_plus;
 
256
  jacobian(1, 3) = -mc*sin_plus;
 
257
 
 
258
  jacobian(2, 1) = 1.0;
 
259
  jacobian(2, 2) = sin - phi*cos;
 
260
  jacobian(2, 3) = cos;
 
261
 
 
262
  jacobian(3, 2) = mc*sin_plus - mc*phi*cos_plus;
 
263
  jacobian(1, 3) = mc*cos_plus;
 
264
 
 
265
  jacobian(4, 3) = -ts / (mc*r*r);
 
266
  jacobian(4, 4) = 1.0 / (mc*r);
267
267
 
268
268
  TMatrixD jacobianT(_n_parameters, _n_parameters);
269
 
  jacobianT.Transpose( jacobian );
 
269
  jacobianT.Transpose(jacobian);
270
270
 
271
 
  for ( int i = 0; i < _n_parameters; ++i ) {
272
 
    for ( int j = 0; j < _n_parameters; ++j ) {
273
 
      patrec_covariance(i,j) = cov.at( i*_n_parameters + j );
 
271
  for (int i = 0; i < _n_parameters; ++i) {
 
272
    for (int j = 0; j < _n_parameters; ++j) {
 
273
      patrec_covariance(i, j) = cov.at(i*_n_parameters + j);
274
274
    }
275
275
  }
276
276
 
286
286
//  }
287
287
 
288
288
  std::vector<double> cov = seed->get_covariance();
289
 
  TMatrixD patrec_covariance( _n_parameters, _n_parameters );
290
 
  TMatrixD covariance( _n_parameters, _n_parameters );
 
289
  TMatrixD patrec_covariance(_n_parameters, _n_parameters);
 
290
  TMatrixD covariance(_n_parameters, _n_parameters);
291
291
 
292
 
  if ( cov.size() != (unsigned int)_n_parameters*_n_parameters ) {
293
 
    throw MAUS::Exception( MAUS::Exception::recoverable, 
 
292
  if (cov.size() != (unsigned int)_n_parameters*_n_parameters) {
 
293
    throw MAUS::Exception(MAUS::Exception::recoverable,
294
294
                          "Dimension of covariance matrix does not match the state vector",
295
295
                          "KalmanSeed::ComputeInitalCovariance(Straight)");
296
296
  }
297
297
 
298
 
  TMatrixD jacobian( _n_parameters, _n_parameters );
299
 
  jacobian(0,0) = 1.0;
300
 
  jacobian(1,1) = 1.0;
301
 
  jacobian(2,2) = 1.0;
302
 
  jacobian(3,3) = 1.0;
303
 
  jacobian(0,1) = 1100.0; // TODO: Read the correct value from the geometry
304
 
  jacobian(2,3) = 1100.0;
 
298
  TMatrixD jacobian(_n_parameters, _n_parameters);
 
299
  jacobian(0, 0) = 1.0;
 
300
  jacobian(1, 1) = 1.0;
 
301
  jacobian(2, 2) = 1.0;
 
302
  jacobian(3, 3) = 1.0;
 
303
  jacobian(0, 1) = 1100.0; // TODO: Read the correct value from the geometry
 
304
  jacobian(2, 3) = 1100.0;
305
305
 
306
306
  TMatrixD jacobianT(_n_parameters, _n_parameters);
307
 
  jacobianT.Transpose( jacobian );
 
307
  jacobianT.Transpose(jacobian);
308
308
 
309
 
  for ( int i = 0; i < _n_parameters; ++i ) {
310
 
    for ( int j = 0; j < _n_parameters; ++j ) {
311
 
      patrec_covariance(i,j) = cov.at( i*_n_parameters + j );
 
309
  for (int i = 0; i < _n_parameters; ++i) {
 
310
    for (int j = 0; j < _n_parameters; ++j) {
 
311
      patrec_covariance(i, j) = cov.at(i*_n_parameters + j);
312
312
    }
313
313
  }
314
314