Skip to content

Commit cdef1e9

Browse files
stephenswatstephenswat
committed
Compute Kálmán filter chi2 from predicted state
This commit migrates the computation of the chi2 value in our gain matrix updater to use the predicted state rather than the filtered state. This is mathematically equivalent but potentially more stable.
1 parent 33b9c47 commit cdef1e9

File tree

2 files changed

+76
-77
lines changed

2 files changed

+76
-77
lines changed

‎core/include/traccc/fitting/kalman_filter/gain_matrix_updater.hpp‎

Lines changed: 25 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -74,7 +74,6 @@ struct gain_matrix_updater {
7474
// Some identity matrices
7575
// @TODO: Make constexpr work
7676
const auto I66 = matrix::identity<bound_matrix_type>();
77-
const auto I_m = matrix::identity<matrix_type<D, D>>();
7877

7978
// Measurement data on surface
8079
matrix_type<D, 1> meas_local;
@@ -123,6 +122,31 @@ struct gain_matrix_updater {
123122
const matrix_type<e_bound_size, D> projected_cov =
124123
algebra::matrix::transposed_product<false, true>(predicted_cov, H);
125124

125+
{
126+
// Calculate the chi square
127+
const matrix_type<D, D> R =
128+
V + (H * predicted_cov * algebra::matrix::transpose(H));
129+
// Residual between measurement and predicted vector
130+
const matrix_type<D, 1> residual = meas_local - H * predicted_vec;
131+
const matrix_type<1, 1> chi2_mat =
132+
algebra::matrix::transposed_product<true, false>(
133+
residual, matrix::inverse(R)) *
134+
residual;
135+
const scalar chi2_val = getter::element(chi2_mat, 0, 0);
136+
137+
if (chi2_val < 0.f) {
138+
TRACCC_ERROR_HOST_DEVICE("Chi2 negative");
139+
return kalman_fitter_status::ERROR_UPDATER_CHI2_NEGATIVE;
140+
}
141+
142+
if (!std::isfinite(chi2_val)) {
143+
TRACCC_ERROR_HOST_DEVICE("Chi2 infinite");
144+
return kalman_fitter_status::ERROR_UPDATER_CHI2_NOT_FINITE;
145+
}
146+
147+
trk_state.filtered_chi2() = chi2_val;
148+
}
149+
126150
const matrix_type<D, D> M = H * projected_cov + V;
127151

128152
// Kalman gain matrix
@@ -162,38 +186,15 @@ struct gain_matrix_updater {
162186
return kalman_fitter_status::ERROR_QOP_ZERO;
163187
}
164188

165-
// Residual between measurement and (projected) filtered vector
166-
const matrix_type<D, 1> residual = meas_local - H * filtered_vec;
167-
168-
// Calculate the chi square
169-
const matrix_type<D, D> R = (I_m - H * K) * V;
170-
const matrix_type<1, 1> chi2 =
171-
algebra::matrix::transposed_product<true, false>(
172-
residual, matrix::inverse(R)) *
173-
residual;
174-
175-
const scalar chi2_val{getter::element(chi2, 0, 0)};
176-
177189
TRACCC_VERBOSE_HOST("Filtered residual: " << residual);
178190
TRACCC_DEBUG_HOST("R:\n" << R);
179191
TRACCC_DEBUG_HOST_DEVICE("det(R): %f", matrix::determinant(R));
180192
TRACCC_DEBUG_HOST("R_inv:\n" << matrix::inverse(R));
181193
TRACCC_VERBOSE_HOST_DEVICE("Chi2: %f", chi2_val);
182194

183-
if (chi2_val < 0.f) {
184-
TRACCC_ERROR_HOST_DEVICE("Chi2 negative");
185-
return kalman_fitter_status::ERROR_UPDATER_CHI2_NEGATIVE;
186-
}
187-
188-
if (!std::isfinite(chi2_val)) {
189-
TRACCC_ERROR_HOST_DEVICE("Chi2 infinite");
190-
return kalman_fitter_status::ERROR_UPDATER_CHI2_NOT_FINITE;
191-
}
192-
193195
// Set the chi2 for this track and measurement
194196
trk_state.filtered_params().set_vector(filtered_vec);
195197
trk_state.filtered_params().set_covariance(filtered_cov);
196-
trk_state.filtered_chi2() = chi2_val;
197198

198199
if (math::fmod(trk_state.filtered_params().theta(),
199200
2.f * constant<traccc::scalar>::pi) == 0.f) {

‎core/include/traccc/fitting/kalman_filter/two_filters_smoother.hpp‎

Lines changed: 51 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,27 @@ struct two_filters_smoother {
8484
const matrix_type<e_bound_size, 1>& predicted_vec =
8585
bound_params.vector();
8686

87+
matrix_type<D, e_bound_size> H =
88+
measurements.at(trk_state.measurement_index())
89+
.subs.template projector<D>();
90+
if (dim == 1) {
91+
getter::element(H, 1u, 0u) = 0.f;
92+
getter::element(H, 1u, 1u) = 0.f;
93+
}
94+
95+
// Spatial resolution (Measurement covariance)
96+
matrix_type<D, D> V;
97+
edm::get_measurement_covariance<algebra_t>(
98+
measurements.at(trk_state.measurement_index()), V);
99+
if (dim == 1) {
100+
getter::element(V, 1u, 1u) = 1.f;
101+
}
102+
103+
TRACCC_DEBUG_HOST("Measurement position: " << meas_local);
104+
TRACCC_DEBUG_HOST("Measurement variance:\n" << V);
105+
TRACCC_DEBUG_HOST("Predicted residual: " << meas_local -
106+
H * predicted_vec);
107+
87108
// Predicted covaraince of bound track parameters
88109
const matrix_type<e_bound_size, e_bound_size>& predicted_cov =
89110
bound_params.covariance();
@@ -134,29 +155,8 @@ struct two_filters_smoother {
134155
// Wrap the phi and theta angles in their valid ranges
135156
normalize_angles(trk_state.smoothed_params());
136157

137-
matrix_type<D, e_bound_size> H =
138-
measurements.at(trk_state.measurement_index())
139-
.subs.template projector<D>();
140-
if (dim == 1) {
141-
getter::element(H, 1u, 0u) = 0.f;
142-
getter::element(H, 1u, 1u) = 0.f;
143-
}
144-
145158
const matrix_type<D, 1> residual_smt = meas_local - H * smoothed_vec;
146159

147-
// Spatial resolution (Measurement covariance)
148-
matrix_type<D, D> V;
149-
edm::get_measurement_covariance<algebra_t>(
150-
measurements.at(trk_state.measurement_index()), V);
151-
if (dim == 1) {
152-
getter::element(V, 1u, 1u) = 1.f;
153-
}
154-
155-
TRACCC_DEBUG_HOST("Measurement position: " << meas_local);
156-
TRACCC_DEBUG_HOST("Measurement variance:\n" << V);
157-
TRACCC_DEBUG_HOST("Predicted residual: " << meas_local -
158-
H * predicted_vec);
159-
160160
// Eq (3.39) of "Pattern Recognition, Tracking and Vertex
161161
// Reconstruction in Particle Detectors"
162162
const matrix_type<D, D> R_smt =
@@ -204,7 +204,6 @@ struct two_filters_smoother {
204204

205205
const auto I66 =
206206
matrix::identity<matrix_type<e_bound_size, e_bound_size>>();
207-
const auto I_m = matrix::identity<matrix_type<D, D>>();
208207

209208
const matrix_type<e_bound_size, D> projected_cov =
210209
algebra::matrix::transposed_product<false, true>(predicted_cov, H);
@@ -252,39 +251,38 @@ struct two_filters_smoother {
252251

253252
bound_params.set_covariance(filtered_cov);
254253

255-
// Residual between measurement and (projected) filtered vector
256-
const matrix_type<D, 1> residual = meas_local - H * filtered_vec;
257-
258-
// Calculate backward chi2
259-
const matrix_type<D, D> R = (I_m - H * K) * V;
260-
// assert(matrix::determinant(R) != 0.f); // @TODO: This fails
261-
assert(std::isfinite(matrix::determinant(R)));
262-
const matrix_type<1, 1> chi2 =
263-
algebra::matrix::transposed_product<true, false>(
264-
residual, matrix::inverse(R)) *
265-
residual;
266-
267-
const scalar chi2_val{getter::element(chi2, 0, 0)};
268-
269-
TRACCC_VERBOSE_HOST("Filtered residual: " << residual);
270-
TRACCC_DEBUG_HOST("R:\n" << R);
271-
TRACCC_DEBUG_HOST_DEVICE("det(R): %f", matrix::determinant(R));
272-
TRACCC_DEBUG_HOST("R_inv:\n" << matrix::inverse(R));
273-
TRACCC_VERBOSE_HOST_DEVICE("Chi2: %f", chi2_val);
274-
275-
if (chi2_val < 0.f) {
276-
TRACCC_ERROR_HOST_DEVICE("Chi2 negative: %f", chi2_val);
277-
return kalman_fitter_status::ERROR_SMOOTHER_CHI2_NEGATIVE;
278-
}
279-
280-
if (!std::isfinite(chi2_val)) {
281-
TRACCC_ERROR_HOST_DEVICE("Chi2 infinite");
282-
return kalman_fitter_status::ERROR_SMOOTHER_CHI2_NOT_FINITE;
254+
{
255+
// Calculate the chi square
256+
const matrix_type<D, D> R =
257+
V - (H * predicted_cov * algebra::matrix::transpose(H));
258+
// Residual between measurement and predicted vector
259+
const matrix_type<D, 1> residual = meas_local - H * predicted_vec;
260+
const matrix_type<1, 1> chi2_mat =
261+
algebra::matrix::transposed_product<true, false>(
262+
residual, matrix::inverse(R)) *
263+
residual;
264+
const scalar chi2_val = getter::element(chi2_mat, 0, 0);
265+
266+
TRACCC_VERBOSE_HOST("Predicted residual: " << residual);
267+
TRACCC_DEBUG_HOST("R:\n" << R);
268+
TRACCC_DEBUG_HOST_DEVICE("det(R): %f", matrix::determinant(R));
269+
TRACCC_DEBUG_HOST("R_inv:\n" << matrix::inverse(R));
270+
TRACCC_VERBOSE_HOST_DEVICE("Chi2: %f", chi2_val);
271+
272+
if (chi2_val < 0.f) {
273+
TRACCC_ERROR_HOST_DEVICE("Chi2 negative: %f", chi2_val);
274+
return kalman_fitter_status::ERROR_SMOOTHER_CHI2_NEGATIVE;
275+
}
276+
277+
if (!std::isfinite(chi2_val)) {
278+
TRACCC_ERROR_HOST_DEVICE("Chi2 infinite");
279+
return kalman_fitter_status::ERROR_SMOOTHER_CHI2_NOT_FINITE;
280+
}
281+
282+
// Set backward chi2
283+
trk_state.backward_chi2() = chi2_val;
283284
}
284285

285-
// Set backward chi2
286-
trk_state.backward_chi2() = chi2_val;
287-
288286
// Wrap the phi and theta angles in their valid ranges
289287
normalize_angles(bound_params);
290288

0 commit comments

Comments
 (0)