~maus-scifi/maus/tracker_devel

« back to all changes in this revision

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

  • Committer: Christopher Hunt
  • Date: 2015-01-29 16:17:20 UTC
  • mto: This revision was merged to the branch mainline in revision 1143.
  • Revision ID: christopher.hunt08@imperial.ac.uk-20150129161720-n8pjzvdoffnsqj3o
Updatest to kalman. Use PatRec for state vector and covariance seeds

Show diffs side-by-side

added added

removed removed

Lines of Context:
125
125
  }
126
126
  //
127
127
  // Set up first state and its covariance.
128
 
  TMatrixD C(_n_parameters, _n_parameters);
129
 
  C.Zero();
130
 
  for ( int i = 0; i < _n_parameters; ++i ) {
131
 
    C(i, i) = _seed_cov;
132
 
  }
133
128
  _kalman_sites[0]->set_a(_a0, KalmanState::Projected);
134
 
  _kalman_sites[0]->set_covariance_matrix(C, KalmanState::Projected);
 
129
  _kalman_sites[0]->set_covariance_matrix(_full_covariance, KalmanState::Projected);
135
130
}
136
131
 
137
132
TMatrixD KalmanSeed::ComputeInitialStateVector(const SciFiHelicalPRTrack* seed,
138
133
                                               const SciFiSpacePointPArray &spacepoints) {
139
 
  // Get seed values.
140
 
  double r  = seed->get_R();
141
134
  // Get pt in MeV.
142
135
  double c  = CLHEP::c_light;
143
136
  // Charge guess should come from PR.
147
140
    _Bz = -_Bz;
148
141
  }
149
142
 
 
143
  // Length of tracker
 
144
  double length = 1100.0;
 
145
 
 
146
  // Get seed values.
 
147
  double r  = seed->get_R();
150
148
  double pt = _particle_charge*c*_Bz*r;
151
 
 
152
149
  double dsdz  = fabs(seed->get_dsdz());
153
 
 
154
 
  double pz = fabs(pt/dsdz);
155
 
 
156
 
  double x, y, z;
157
 
  x = spacepoints.front()->get_position().x();
158
 
  y = spacepoints.front()->get_position().y();
159
 
  z = spacepoints.front()->get_position().z();
160
 
 
161
 
  double phi_0;
162
 
  if ( _tracker == 1 ) {
163
 
    phi_0 = seed->get_phi().back();
 
150
  double x0 = seed->get_circle_x0(); // Circle Center x
 
151
  double y0 = seed->get_circle_y0(); // Circle Center y
 
152
  double s = seed->get_line_sz_c() - _particle_charge*length*dsdz; // Path length at start plane
 
153
  double phi_0 = s / r; // Phi at start plane
 
154
  double phi = phi_0 + TMath::PiOver2(); // Direction of momentum
 
155
 
 
156
  ThreeVector patrec_momentum( pt*cos(phi), pt*sin(phi), fabs(pt/dsdz) );
 
157
  double P = patrec_momentum.mag();
 
158
  double patrec_bias; // Account for two planes of energy loss
 
159
  if ( _tracker == 0 ) {
 
160
    patrec_bias = (P + 1.4) / P;
164
161
  } else {
165
 
    phi_0 = seed->get_phi().front();
 
162
    patrec_bias = (P - 1.4) / P;
166
163
  }
167
 
 
168
 
  double phi = phi_0 + TMath::PiOver2();
169
 
  double px  = pt*cos(phi);
170
 
  double py  = pt*sin(phi);
171
 
 
172
 
  // Remove PR momentum bias.
173
 
  // ThreeVector mom(px, py, pz);
174
 
  // double reduction_factor = (mom.mag()-1.4)/mom.mag();
175
 
  // ThreeVector new_momentum = mom*reduction_factor;
 
164
  patrec_momentum = patrec_bias * patrec_momentum;
 
165
 
 
166
  double x = x0 + r*cos(phi_0);
 
167
  double px = patrec_momentum.x();
 
168
  double y = y0 + r*sin(phi_0);
 
169
  double py = patrec_momentum.y();
 
170
  double kappa = _particle_charge / patrec_momentum.z();
176
171
 
177
172
  TMatrixD a(_n_parameters, 1);
178
173
  a(0, 0) = x;
179
 
  a(1, 0) = px; // new_momentum.x();
 
174
  a(1, 0) = px; 
180
175
  a(2, 0) = y;
181
 
  a(3, 0) = py; // new_momentum.y();
182
 
  a(4, 0) = _particle_charge/pz; // _particle_charge/new_momentum.z();
 
176
  a(3, 0) = py;
 
177
  a(4, 0) = kappa;
183
178
 
184
179
  return a;
185
180
}
186
181
 
187
182
TMatrixD KalmanSeed::ComputeInitialStateVector(const SciFiStraightPRTrack* seed,
188
183
                                               const SciFiSpacePointPArray &spacepoints) {
189
 
  double x, y, z;
190
 
 
191
 
  x = spacepoints.front()->get_position().x();
192
 
  y = spacepoints.front()->get_position().y();
193
 
  z = spacepoints.front()->get_position().z();
194
 
 
 
184
  // Length of tracker
 
185
  double length = 1100.0;
 
186
 
 
187
  double x0 = seed->get_x0();
 
188
  double y0 = seed->get_y0();
195
189
  double mx = seed->get_mx();
196
190
  double my = seed->get_my();
197
191
 
 
192
  // Get position at the starting end of tracker
 
193
  double x = x0 + mx*length;
 
194
  double y = y0 + my*length;
 
195
 
198
196
  TMatrixD a(_n_parameters, 1);
199
197
  a(0, 0) = x;
200
198
  a(1, 0) = mx;
220
218
  std::sort(spacepoints.begin(), spacepoints.end(), SortByZ<SciFiSpacePoint>);
221
219
}
222
220
 
 
221
TMatrixD KalmanSeed::ComputeInitialCovariance(const SciFiHelicalPRTrack* seed) {
 
222
//  TMatrixD covariance(_n_parameters, _n_parameters);
 
223
//  for ( int i = 0; i < _n_parameters; ++i ) {
 
224
//    covariance(i, i) = _seed_cov;
 
225
//  }
 
226
 
 
227
  std::vector<double> cov = seed->get_covariance();
 
228
  TMatrixD patrec_covariance( _n_parameters, _n_parameters );
 
229
  TMatrixD covariance( _n_parameters, _n_parameters );
 
230
 
 
231
  if ( cov.size() != _n_parameters*_n_parameters ) {
 
232
    throw MAUS::Exception( MAUS::Exception::recoverable, 
 
233
                          "Dimension of covariance matrix does not match the state vector",
 
234
                          "KalmanSeed::ComputeInitalCovariance(Helical)");
 
235
  }
 
236
 
 
237
  double length = 1100.0;
 
238
  double dsdz  = fabs(seed->get_dsdz());
 
239
  double mc = _particle_charge*CLHEP::c_light*_Bz; // Magnetic constant
 
240
  double r = seed->get_R();
 
241
  double s = seed->get_line_sz_c() - _particle_charge*length*dsdz; // Path length at start plane
 
242
  double phi = s / r; // Phi at start plane // TODO: Is this the correct phi?!
 
243
  double sin = std::sin( phi );
 
244
  double cos = std::cos( phi );
 
245
  double sin_plus = std::sin( phi + TMath::PiOver2() );
 
246
  double cos_plus = std::cos( phi + TMath::PiOver2() );
 
247
  double ts = seed->get_dsdz();
 
248
 
 
249
  TMatrixD jacobian( _n_parameters, _n_parameters );
 
250
  jacobian(0,0) = 1.0;
 
251
  jacobian(0,2) = cos + phi*sin;
 
252
  jacobian(0,3) = -sin;
 
253
 
 
254
  jacobian(1,2) = mc*cos_plus + mc*phi*sin_plus;
 
255
  jacobian(1,3) = -mc*sin_plus;
 
256
 
 
257
  jacobian(2,1) = 1.0;
 
258
  jacobian(2,2) = sin - phi*cos;
 
259
  jacobian(2,3) = cos;
 
260
 
 
261
  jacobian(3,2) = mc*sin_plus - mc*phi*cos_plus;
 
262
  jacobian(1,3) = mc*cos_plus;
 
263
 
 
264
  jacobian(4,3) = -ts / (mc*r*r);
 
265
  jacobian(4,4) = 1.0 / (mc*r);
 
266
 
 
267
  TMatrixD jacobianT(_n_parameters, _n_parameters);
 
268
  jacobianT.Transpose( jacobian );
 
269
 
 
270
  for ( int i = 0; i < _n_parameters; ++i ) {
 
271
    for ( int j = 0; j < _n_parameters; ++j ) {
 
272
      patrec_covariance(i,j) = cov.at( i*_n_parameters + j );
 
273
    }
 
274
  }
 
275
 
 
276
  covariance = jacobian*patrec_covariance*jacobianT;
 
277
 
 
278
  return covariance;
 
279
}
 
280
 
 
281
TMatrixD KalmanSeed::ComputeInitialCovariance(const SciFiStraightPRTrack* seed) {
 
282
//  _full_covariance.Zero();
 
283
//  for ( int i = 0; i < _n_parameters; ++i ) {
 
284
//    _full_covariance(i, i) = _seed_cov;
 
285
//  }
 
286
 
 
287
  std::vector<double> cov = seed->get_covariance();
 
288
  TMatrixD patrec_covariance( _n_parameters, _n_parameters );
 
289
  TMatrixD covariance( _n_parameters, _n_parameters );
 
290
 
 
291
  if ( cov.size() != _n_parameters*_n_parameters ) {
 
292
    throw MAUS::Exception( MAUS::Exception::recoverable, 
 
293
                          "Dimension of covariance matrix does not match the state vector",
 
294
                          "KalmanSeed::ComputeInitalCovariance(Straight)");
 
295
  }
 
296
 
 
297
  TMatrixD jacobian( _n_parameters, _n_parameters );
 
298
  jacobian(0,0) = 1.0;
 
299
  jacobian(1,1) = 1.0;
 
300
  jacobian(2,2) = 1.0;
 
301
  jacobian(3,3) = 1.0;
 
302
  jacobian(0,1) = 1100.0; // TODO: Read the correct value from the geometry
 
303
  jacobian(2,3) = 1100.0;
 
304
 
 
305
  TMatrixD jacobianT(_n_parameters, _n_parameters);
 
306
  jacobianT.Transpose( jacobian );
 
307
 
 
308
  for ( int i = 0; i < _n_parameters; ++i ) {
 
309
    for ( int j = 0; j < _n_parameters; ++j ) {
 
310
      patrec_covariance(i,j) = cov.at( i*_n_parameters + j );
 
311
    }
 
312
  }
 
313
 
 
314
  covariance = jacobian*patrec_covariance*jacobianT;
 
315
 
 
316
  return covariance;
 
317
}
 
318
 
223
319
} // ~namespace MAUS