@@ -45,11 +45,16 @@ std::tuple<double, std::error_code> GainMatrixUpdater::visitMeasurementImpl(
4545
4646 ACTS_VERBOSE (" Measurement projector H:\n " << H);
4747
48- const auto K = (trackState.predictedCovariance () * H.transpose () *
49- (H * trackState.predictedCovariance () * H.transpose () +
50- calibratedCovariance)
51- .inverse ())
52- .eval ();
48+ auto filtered = trackState.filtered ();
49+ auto filteredCovariance = trackState.filteredCovariance ();
50+ const auto predicted = trackState.predicted ();
51+ const auto predictedCovariance = trackState.predictedCovariance ();
52+
53+ const auto K =
54+ (predictedCovariance * H.transpose () *
55+ (H * predictedCovariance * H.transpose () + calibratedCovariance)
56+ .inverse ())
57+ .eval ();
5358
5459 ACTS_VERBOSE (" Gain Matrix K:\n " << K);
5560
@@ -58,17 +63,16 @@ std::tuple<double, std::error_code> GainMatrixUpdater::visitMeasurementImpl(
5863 return {0 , KalmanFitterError::UpdateFailed};
5964 }
6065
61- trackState.filtered () =
62- trackState.predicted () + K * (calibrated - H * trackState.predicted ());
66+ filtered = predicted + K * (calibrated - H * predicted);
6367 // Normalize phi and theta
64- trackState. filtered () = normalizeBoundParameters (trackState. filtered () );
65- trackState. filteredCovariance () = ( BoundSquareMatrix::Identity () - K * H) *
66- trackState. predictedCovariance () ;
67- ACTS_VERBOSE (" Filtered parameters: " << trackState. filtered () .transpose ());
68- ACTS_VERBOSE (" Filtered covariance:\n " << trackState. filteredCovariance () );
68+ filtered = normalizeBoundParameters (filtered);
69+ filteredCovariance =
70+ ( BoundSquareMatrix::Identity () - K * H) * predictedCovariance;
71+ ACTS_VERBOSE (" Filtered parameters: " << filtered.transpose ());
72+ ACTS_VERBOSE (" Filtered covariance:\n " << filteredCovariance);
6973
7074 ParametersVector residual;
71- residual = calibrated - H * trackState. filtered () ;
75+ residual = calibrated - H * filtered;
7276 ACTS_VERBOSE (" Residual: " << residual.transpose ());
7377
7478 CovarianceMatrix m =
0 commit comments