~christopher-hunt08/maus/maus_integrated_kalman
» Revision
739
: src/common_cpp/Recon/Kalman/MAUSSciFiIntegratedPropagator.cc
« back to all changes in this revision
Viewing changes to src/common_cpp/Recon/Kalman/MAUSSciFiIntegratedPropagator.cc
- browse files at revision 739
- compare with another revision
- download diff
- download tarball
- view history from revision 739
- Committer: Christopher Hunt
- Date: 2018-07-04 14:12:12 UTC
- Revision ID: christopher.hunt08@imperial.ac.uk-20180704141212-orzzl40mi53xc6kh
Added files for field integrated kalman filter. Now to test it...
- src/map/MapCppTrackerIntegratedPRSeed
- src/map/MapCppTrackerIntegratedTrackFit
added
removed
src/common_cpp/Recon/Kalman/MAUSSciFiIntegratedPropagator.cc
1
/* This file is part of MAUS: http://micewww.pp.rl.ac.uk:8080/projects/maus
2
*
3
* MAUS is free software: you can redistribute it and/or modify
4
* it under the terms of the GNU General Public License as published by
5
* the Free Software Foundation, either version 3 of the License, or
6
* (at your option) any later version.
7
*
8
* MAUS is distributed in the hope that it will be useful,
9
* but WITHOUT ANY WARRANTY; without even the implied warranty of
10
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11
* GNU General Public License for more details.
12
*
13
* You should have received a copy of the GNU General Public License
14
* along with MAUS. If not, see <http://www.gnu.org/licenses/>.
15
*
16
*/
17
18
19
#include "src/common_cpp/Recon/Kalman/MAUSSciFiIntegratedPropagator.hh"
20
21
#include "src/common_cpp/Utils/Exception.hh"
22
23
#include <cmath>
24
25
namespace MAUS {
26
27
FieldIntegratedPropagator::FieldIntegratedPropagator(SciFiGeometryHelper* helper) :
28
Kalman::Propagator_base(5),
29
_errorTracking(),
30
_perfectErrorTracking() {
31
_errorTracking.SetTrackingModel(Kalman::Global::ErrorTracking::em_forwards_dynamic);
32
_errorTracking.SetGeometryModel(Kalman::Global::ErrorTracking::axial_lookup);
33
_errorTracking.SetEnergyLossModel(Kalman::Global::ErrorTracking::bethe_bloch_forwards);
34
35
_perfectErrorTracking.SetTrackingModel(Kalman::Global::ErrorTracking::em_forwards_dynamic);
36
_perfectErrorTracking.SetGeometryModel(Kalman::Global::ErrorTracking::axial_lookup);
37
_perfectErrorTracking.SetEnergyLossModel(Kalman::Global::ErrorTracking::no_eloss);
38
}
39
40
void FieldIntegratedPropagator::Propagate(const Kalman::TrackPoint& start_tp, Kalman::TrackPoint& end_tp) {
41
42
Kalman::State old_filtered = start_tp.GetFiltered();
43
TMatrixD old_covariance = old_filtered.GetCovariance();
44
45
std::cerr << "Start State:\n";
46
TMatrixD test = old_filtered.GetVector();
47
std::cerr << " " << test(0, 0) << " " << test(2, 0) << " " << test(4, 0) << " " << test(1, 0) << " " << test(3, 0) << "\n";
48
49
double* V = _V_convert_to_array(old_filtered, start_tp.GetPosition());
50
double* M = _M_convert_to_array(old_filtered, start_tp.GetPosition());
51
52
_errorTracking.Propagate(V, end_tp.GetPosition());
53
_perfectErrorTracking.PropagateTransferMatrix(M, end_tp.GetPosition());
54
55
std::cerr << "MATRIX V\n";
56
for (int i = 0; i < 8; ++i) {
57
std::cerr << V[i] << " ";
58
}
59
std::cerr << "\n";
60
std::cerr << "MATRIX M\n";
61
for (int i = 0; i < 8; ++i) {
62
std::cerr << M[i] << " ";
63
}
64
std::cerr << "\n";
65
66
Kalman::State predicted = _V_convert_to_state(V);
67
68
69
PropagatorMatrix() = _M_convert_to_matrix(M);
70
NoiseMatrix() = predicted.GetCovariance() - old_covariance;
71
72
std::cerr << "Propagator Test:\n";
73
test = PropagatorMatrix()*old_filtered.GetVector();
74
std::cerr << " " << test(0, 0) << " " << test(2, 0) << " " << test(4, 0) << " " << test(1, 0) << " " << test(3, 0) << "\n";
75
76
std::cerr << "\n";
77
78
std::cerr << "\n";
79
PropagatorMatrix().Print();
80
std::cerr << "\n";
81
82
for (int i = 0; i < 6; ++i) {
83
for (int j = 0; j < 6; ++j) {
84
std::cerr << M[8+(i*6)+j] << '\t';
85
}
86
std::cerr << "\n";
87
}
88
std::cerr << "\n";
89
90
delete[] M;
91
92
end_tp.SetPredicted(predicted);
93
}
94
95
96
TMatrixD FieldIntegratedPropagator::CalculatePropagator(const Kalman::TrackPoint& start,
97
const Kalman::TrackPoint& end) {
98
Kalman::State old_filtered = start.GetFiltered();
99
TMatrixD old_covariance = old_filtered.GetCovariance();
100
101
double* M = _M_convert_to_array(old_filtered, start.GetPosition());
102
103
_errorTracking.PropagateTransferMatrix(M, end.GetPosition());
104
105
TMatrix propagator = _M_convert_to_matrix(M);
106
107
return propagator;
108
}
109
110
111
TMatrixD FieldIntegratedPropagator::CalculateProcessNoise(const Kalman::TrackPoint& start,
112
const Kalman::TrackPoint& end) {
113
Kalman::State old_filtered = start.GetFiltered();
114
TMatrixD old_covariance = old_filtered.GetCovariance();
115
116
double* V = _V_convert_to_array(old_filtered, start.GetPosition());
117
118
_errorTracking.Propagate(V, end.GetPosition());
119
120
Kalman::State predicted = _V_convert_to_state(V);
121
122
TMatrixD new_noise = predicted.GetCovariance() - old_covariance;
123
124
return new_noise;
125
}
126
127
128
double* FieldIntegratedPropagator::_V_convert_to_array(Kalman::State state, double position) {
129
TMatrixD vector = state.GetVector();
130
TMatrixD covariance = state.GetCovariance();
131
132
double* V = new double[29];
133
134
double pz = std::sqrt(vector(4, 0)*vector(4, 0) - Recon::Constants::MuonMass*Recon::Constants::MuonMass - vector(1, 0)*vector(1, 0) - vector(3, 0)*vector(3, 0));
135
136
V[0] = 0.0;
137
V[1] = vector(0, 0);
138
V[2] = vector(2, 0);
139
V[3] = position;
140
V[4] = vector(4, 0);
141
V[5] = vector(1, 0);
142
V[6] = vector(3, 0);
143
V[7] = pz;
144
145
V[8] = 0.0; // Variance T
146
V[9] = 0.0;
147
V[10] = 0.0;
148
V[11] = 0.0;
149
V[12] = 0.0;
150
V[13] = 0.0;
151
152
V[14] = covariance(0, 0); // Variance X
153
V[15] = covariance(0, 2);
154
V[16] = covariance(0, 4);
155
V[17] = covariance(0, 1);
156
V[18] = covariance(0, 3);
157
158
V[19] = covariance(2, 2); // Variance Y
159
V[20] = covariance(2, 4);
160
V[21] = covariance(2, 1);
161
V[22] = covariance(2, 3);
162
163
V[23] = covariance(4, 4); // Variance E
164
V[24] = covariance(4, 1);
165
V[25] = covariance(4, 3);
166
167
V[26] = covariance(1, 1); // Variance px
168
V[27] = covariance(1, 3);
169
170
V[28] = covariance(3, 3); // Variance py
171
172
return V;
173
}
174
175
Kalman::State FieldIntegratedPropagator::_V_convert_to_state(double* array) {
176
TMatrixD vector(5, 1);
177
TMatrixD covariance(5, 5);
178
179
vector(0, 0) = array[1]; // x
180
vector(1, 0) = array[5]; // px
181
vector(2, 0) = array[2]; // y
182
vector(3, 0) = array[6]; // py
183
vector(4, 0) = array[4]; // E
184
185
covariance(0, 0) = array[14]; // x x
186
covariance(0, 2) = array[15]; // x y
187
covariance(0, 4) = array[16]; // x E
188
covariance(0, 1) = array[17]; // x px
189
covariance(0, 3) = array[18]; // x py
190
191
covariance(2, 2) = array[19]; // y y
192
covariance(2, 4) = array[20]; // y E
193
covariance(2, 1) = array[21]; // y px
194
covariance(2, 3) = array[22]; // y py
195
196
covariance(4, 4) = array[23]; // E E
197
covariance(4, 1) = array[24]; // E px
198
covariance(4, 3) = array[25]; // E py
199
200
covariance(1, 1) = array[26]; // px px
201
covariance(1, 3) = array[27]; // px py
202
203
covariance(3, 3) = array[28]; // py py
204
205
// Fill in the others...
206
covariance(1, 0) = covariance(0, 1);
207
covariance(2, 0) = covariance(0, 2);
208
covariance(3, 0) = covariance(0, 3);
209
covariance(4, 0) = covariance(0, 4);
210
211
covariance(4, 2) = covariance(2, 4);
212
covariance(1, 2) = covariance(2, 1);
213
covariance(3, 2) = covariance(2, 3);
214
215
covariance(1, 4) = covariance(4, 1);
216
covariance(3, 4) = covariance(4, 3);
217
218
covariance(3, 1) = covariance(1, 3);
219
220
Kalman::State state(vector, covariance);
221
222
delete[] array;
223
return state;
224
}
225
226
double* FieldIntegratedPropagator::_M_convert_to_array(Kalman::State state, double position) {
227
TMatrixD vector = state.GetVector();
228
229
double* M = new double[44];
230
231
double pz = std::sqrt(vector(4, 0)*vector(4, 0) - Recon::Constants::MuonMass*Recon::Constants::MuonMass - vector(1, 0)*vector(1, 0) - vector(3, 0)*vector(3, 0));
232
233
M[0] = 0.0;
234
M[1] = vector(0, 0);
235
M[2] = vector(2, 0);
236
M[3] = position;
237
M[4] = vector(4, 0);
238
M[5] = vector(1, 0);
239
M[6] = vector(3, 0);
240
M[7] = pz;
241
242
for ( int i = 8; i < 44; ++i ) {
243
M[i] = 0.0;
244
}
245
M[8] = 1.0; // Make it the identity
246
M[15] = 1.0;
247
M[22] = 1.0;
248
M[29] = 1.0;
249
M[36] = 1.0;
250
M[43] = 1.0;
251
252
return M;
253
}
254
255
TMatrixD FieldIntegratedPropagator::_M_convert_to_matrix(double* array) {
256
257
TMatrixD matrix(5, 5);
258
259
matrix(4, 4) = array[15]; // E E
260
matrix(4, 0) = array[16]; // E x
261
matrix(4, 1) = array[17]; // E px
262
matrix(4, 2) = array[18]; // E y
263
matrix(4, 3) = array[19]; // E py
264
265
matrix(0, 4) = array[21]; // x E
266
matrix(0, 0) = array[22]; // x x
267
matrix(0, 1) = array[23]; // x px
268
matrix(0, 2) = array[24]; // x y
269
matrix(0, 3) = array[25]; // x py
270
271
matrix(1, 4) = array[27]; // px E
272
matrix(1, 0) = array[28]; // px x
273
matrix(1, 1) = array[29]; // px px
274
matrix(1, 2) = array[30]; // px y
275
matrix(1, 3) = array[31]; // px py
276
277
matrix(2, 4) = array[33]; // y E
278
matrix(2, 0) = array[34]; // y x
279
matrix(2, 1) = array[35]; // y px
280
matrix(2, 2) = array[36]; // y y
281
matrix(2, 3) = array[37]; // y py
282
283
matrix(3, 4) = array[39]; // py E
284
matrix(3, 0) = array[40]; // py x
285
matrix(3, 1) = array[41]; // py px
286
matrix(3, 2) = array[42]; // py y
287
matrix(3, 3) = array[43]; // py py
288
289
// delete[] array;
290
return matrix;
291
}
292
} // namespace MAUS
293
Loggerhead is a web-based interface for Breezy
Version: 2.0.1