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

Changes from 3 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
31 changes: 17 additions & 14 deletions Core/src/TrackFitting/MbfSmoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ void MbfSmoother::calculateSmoothed(InternalTrackState& ts,
void MbfSmoother::visitNonMeasurement(const InternalTrackState& ts,
BoundMatrix& big_lambda_hat,
BoundVector& small_lambda_hat) const {
const auto F = ts.jacobian;
const InternalTrackState::Jacobian F = ts.jacobian;
CarloVarni marked this conversation as resolved.
Show resolved Hide resolved
CarloVarni marked this conversation as resolved.
Show resolved Hide resolved

big_lambda_hat = F.transpose() * big_lambda_hat * F;
small_lambda_hat = F.transpose() * small_lambda_hat;
Expand All @@ -33,7 +33,8 @@ void MbfSmoother::visitMeasurement(const InternalTrackState& ts,
BoundVector& small_lambda_hat) const {
assert(ts.measurement.has_value());

const auto& measurement = ts.measurement.value();
const InternalTrackState::Measurement& measurement = ts.measurement.value();
const InternalTrackState::Jacobian 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;
Expand All @@ -44,33 +45,35 @@ void MbfSmoother::visitMeasurement(const InternalTrackState& ts,
calibratedCovariance{measurement.calibratedCovariance};

// Measurement matrix
const auto H = measurement.projector
.template topLeftCorner<kMeasurementSize, eBoundSize>()
.eval();
const typename TrackStateTraits<kMeasurementSize, true>::Projector H =
CarloVarni marked this conversation as resolved.
Show resolved Hide resolved
measurement.projector
.template topLeftCorner<kMeasurementSize, eBoundSize>()
.eval();
CarloVarni marked this conversation as resolved.
Show resolved Hide resolved

// Residual covariance
const auto S =
const Eigen::Matrix<ActsScalar, kMeasurementSize, kMeasurementSize> S =
CarloVarni marked this conversation as resolved.
Show resolved Hide resolved
(H * ts.predictedCovariance * H.transpose() + calibratedCovariance)
.eval();
// TODO Sinv could be cached by the filter step
const auto S_inv = S.inverse().eval();
const Eigen::Matrix<ActsScalar, kMeasurementSize, kMeasurementSize> S_inv =
S.inverse().eval();

// Kalman gain
// TODO K could be cached by the filter step
const auto K = (ts.predictedCovariance * H.transpose() * S_inv).eval();
const Eigen::Matrix<ActsScalar, eBoundSize, kMeasurementSize> 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 BoundMatrix C_hat = (BoundMatrix::Identity() - K * H).eval();
const Eigen::Matrix<ActsScalar, kMeasurementSize, 1> y =
(calibrated - H * ts.predicted).eval();

const auto big_lambda_tilde =
const Eigen::Matrix<ActsScalar, eBoundSize, eBoundSize> big_lambda_tilde =
(H.transpose() * S_inv * H + C_hat.transpose() * big_lambda_hat * C_hat)
.eval();
const auto small_lambda_tilde =
const Eigen::Matrix<ActsScalar, eBoundSize, 1> small_lambda_tilde =
(-H.transpose() * S_inv * y + C_hat.transpose() * small_lambda_hat)
.eval();

const auto F = ts.jacobian;

big_lambda_hat = F.transpose() * big_lambda_tilde * F;
small_lambda_hat = F.transpose() * small_lambda_tilde;
});
Expand Down
Loading