Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

refactor: Reduce abuse of auto in mbf smoother #3630

Merged
merged 11 commits into from
Sep 23, 2024
Merged

refactor: Reduce abuse of auto in mbf smoother #3630

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
a7d1974
Reduce use of auto in mbf smoother
Sep 19, 2024
04b4349
no ref for eigen map
Sep 19, 2024
28e1db9
minor change
Sep 19, 2024
65efaa7
use some typedefs
Sep 19, 2024
3d97b37
naming convention
Sep 19, 2024
e3a46f3
no eval
Sep 19, 2024
5c0b3d1
camel for hpp
Sep 19, 2024
f157aa8
no ref for map
Sep 19, 2024
253f327
jacobian
Sep 19, 2024
33cfc3c
Merge branch 'main' into ReduceAutoInMsbSmoother
paulgessinger Sep 23, 2024
d90274c
Merge branch 'main' into ReduceAutoInMsbSmoother
kodiakhq[bot] Sep 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 13 additions & 15 deletions Core/include/Acts/TrackFitting/MbfSmoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,21 +51,21 @@ class MbfSmoother {

using TrackStateProxy = typename traj_t::TrackStateProxy;

TrackStateProxy start_ts = trajectory.getTrackState(entryIndex);
TrackStateProxy startTs = trajectory.getTrackState(entryIndex);

// Notation consistent with the Wikipedia article
// https://en.wikipedia.org/wiki/Kalman_filter
BoundMatrix big_lambda_hat = BoundMatrix::Zero();
BoundVector small_lambda_hat = BoundVector::Zero();
BoundMatrix bigLambdaHat = BoundMatrix::Zero();
BoundVector smallLambdaHat = BoundVector::Zero();

trajectory.applyBackwards(start_ts.index(), [&](TrackStateProxy ts) {
trajectory.applyBackwards(startTs.index(), [&](TrackStateProxy ts) {
// ensure the track state has a smoothed component
ts.addComponents(TrackStatePropMask::Smoothed);

InternalTrackState internalTrackState(ts);

// Smoothe the current state
calculateSmoothed(internalTrackState, big_lambda_hat, small_lambda_hat);
calculateSmoothed(internalTrackState, bigLambdaHat, smallLambdaHat);

// We smoothed the last state - no need to update the lambdas
if (!ts.hasPrevious()) {
Expand All @@ -74,10 +74,9 @@ class MbfSmoother {

// Update the lambdas depending on the type of track state
if (ts.typeFlags().test(TrackStateFlag::MeasurementFlag)) {
visitMeasurement(internalTrackState, big_lambda_hat, small_lambda_hat);
visitMeasurement(internalTrackState, bigLambdaHat, smallLambdaHat);
} else {
visitNonMeasurement(internalTrackState, big_lambda_hat,
small_lambda_hat);
visitNonMeasurement(internalTrackState, bigLambdaHat, smallLambdaHat);
}
});

Expand Down Expand Up @@ -143,18 +142,17 @@ class MbfSmoother {

/// Calculate the smoothed parameters and covariance.
void calculateSmoothed(InternalTrackState& ts,
const BoundMatrix& big_lambda_hat,
const BoundVector& small_lambda_hat) const;
const BoundMatrix& bigLambdaHat,
const BoundVector& smallLambdaHat) const;

/// Visit a non-measurement track state and update the lambdas.
void visitNonMeasurement(const InternalTrackState& ts,
BoundMatrix& big_lambda_hat,
BoundVector& small_lambda_hat) const;
BoundMatrix& bigLambdaHat,
BoundVector& smallLambdaHat) const;

/// Visit a measurement track state and update the lambdas.
void visitMeasurement(const InternalTrackState& ts,
BoundMatrix& big_lambda_hat,
BoundVector& small_lambda_hat) const;
void visitMeasurement(const InternalTrackState& ts, BoundMatrix& bigLambdaHat,
BoundVector& smallLambdaHat) const;
};

} // namespace Acts
68 changes: 36 additions & 32 deletions Core/src/TrackFitting/MbfSmoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,68 +11,72 @@
namespace Acts {

void MbfSmoother::calculateSmoothed(InternalTrackState& ts,
const BoundMatrix& big_lambda_hat,
const BoundVector& small_lambda_hat) const {
const BoundMatrix& bigLambdaHat,
const BoundVector& smallLambdaHat) const {
ts.smoothedCovariance = ts.filteredCovariance - ts.filteredCovariance *
big_lambda_hat *
bigLambdaHat *
ts.filteredCovariance;
ts.smoothed = ts.filtered - ts.filteredCovariance * small_lambda_hat;
ts.smoothed = ts.filtered - ts.filteredCovariance * smallLambdaHat;
}

void MbfSmoother::visitNonMeasurement(const InternalTrackState& ts,
BoundMatrix& big_lambda_hat,
BoundVector& small_lambda_hat) const {
const auto F = ts.jacobian;
BoundMatrix& bigLambdaHat,
BoundVector& smallLambdaHat) const {
const Acts::BoundMatrix& F = ts.jacobian;

big_lambda_hat = F.transpose() * big_lambda_hat * F;
small_lambda_hat = F.transpose() * small_lambda_hat;
bigLambdaHat = F.transpose() * bigLambdaHat * F;
smallLambdaHat = F.transpose() * smallLambdaHat;
}

void MbfSmoother::visitMeasurement(const InternalTrackState& ts,
BoundMatrix& big_lambda_hat,
BoundVector& small_lambda_hat) const {
BoundMatrix& bigLambdaHat,
BoundVector& smallLambdaHat) const {
assert(ts.measurement.has_value());

const auto& measurement = ts.measurement.value();
const InternalTrackState::Measurement& measurement = ts.measurement.value();
const Acts::BoundMatrix& F = ts.jacobian;
CarloVarni marked this conversation as resolved.
Show resolved Hide resolved

visit_measurement(measurement.calibratedSize, [&](auto N) -> void {
constexpr std::size_t kMeasurementSize = decltype(N)::value;

using MeasurementMatrix =
Eigen::Matrix<ActsScalar, kMeasurementSize, eBoundSize>;
using CovarianceMatrix =
Eigen::Matrix<ActsScalar, kMeasurementSize, kMeasurementSize>;
using KalmanGainMatrix =
Eigen::Matrix<ActsScalar, eBoundSize, kMeasurementSize>;

typename TrackStateTraits<kMeasurementSize, true>::Calibrated calibrated{
measurement.calibrated};
typename TrackStateTraits<kMeasurementSize, true>::CalibratedCovariance
calibratedCovariance{measurement.calibratedCovariance};

// Measurement matrix
const auto H = measurement.projector
.template topLeftCorner<kMeasurementSize, eBoundSize>()
.eval();
const MeasurementMatrix H =
measurement.projector
.template topLeftCorner<kMeasurementSize, eBoundSize>();

// Residual covariance
const auto S =
(H * ts.predictedCovariance * H.transpose() + calibratedCovariance)
.eval();
const CovarianceMatrix S =
(H * ts.predictedCovariance * H.transpose() + calibratedCovariance);
// TODO Sinv could be cached by the filter step
const auto S_inv = S.inverse().eval();
const CovarianceMatrix SInv = S.inverse();

// Kalman gain
// TODO K could be cached by the filter step
const auto K = (ts.predictedCovariance * H.transpose() * S_inv).eval();

const auto C_hat = (BoundMatrix::Identity() - K * H).eval();
const auto y = (calibrated - H * ts.predicted).eval();
const KalmanGainMatrix K = (ts.predictedCovariance * H.transpose() * SInv);

const auto big_lambda_tilde =
(H.transpose() * S_inv * H + C_hat.transpose() * big_lambda_hat * C_hat)
.eval();
const auto small_lambda_tilde =
(-H.transpose() * S_inv * y + C_hat.transpose() * small_lambda_hat)
.eval();
const Acts::BoundMatrix CHat = (Acts::BoundMatrix::Identity() - K * H);
const Eigen::Matrix<ActsScalar, kMeasurementSize, 1> y =
(calibrated - H * ts.predicted);

const auto F = ts.jacobian;
const Acts::BoundMatrix bigLambdaTilde =
(H.transpose() * SInv * H + CHat.transpose() * bigLambdaHat * CHat);
const Eigen::Matrix<ActsScalar, eBoundSize, 1> smallLambdaTilde =
(-H.transpose() * SInv * y + CHat.transpose() * smallLambdaHat);

big_lambda_hat = F.transpose() * big_lambda_tilde * F;
small_lambda_hat = F.transpose() * small_lambda_tilde;
bigLambdaHat = F.transpose() * bigLambdaTilde * F;
smallLambdaHat = F.transpose() * smallLambdaTilde;
});
}

Expand Down
Loading