Add track fitting UKF #249
Conversation
| /// @tparam DIM_V Dimension of the process noise vector. | ||
| /// @tparam DIM_N Dimension of the measurement noise vector. | ||
| template <int32_t DIM_X = 6, int32_t DIM_Z = 3, int32_t DIM_V = 2, int32_t DIM_N = 3> | ||
| class TrackFitterUKF : public KalmanFilter<DIM_X, DIM_Z> { |
There was a problem hiding this comment.
Name is a placeholder. Once we get a working code I think we should refactor it to make it more accessible to users, but I want to do that once we have a working version.
| void setCovarianceQ(const Matrix<DIM_V, DIM_V> &matQ) | ||
| { | ||
| m_matQ = matQ; // Store the process noise covariance matrix | ||
| } |
There was a problem hiding this comment.
Will be removed or replaced by a function call. This matrix changes each iteration (will be part of refactor, for now it's fine)
| void setCovarianceR(const Matrix<DIM_N, DIM_N> &matR) | ||
| { | ||
| m_matR = matR; // Store the measurement noise covariance matrix | ||
| } |
There was a problem hiding this comment.
Also changes each iteration handled in task probably
| std::array<float32_t, DIM_V> calculateProcessNoiseMean() | ||
| { | ||
| // Calculate the expectation value of the process noise using the current value of the state vector m_vecX | ||
| std::array<float32_t, DIM_V> processNoiseMean{0}; | ||
|
|
||
| // TODO: Set the mean energy loss based on the momentum and particle type. Probably best to track stopping power? | ||
| return processNoiseMean; | ||
| } |
There was a problem hiding this comment.
@Yassid This is where the energy loss and scattering will need to be implemented. This function should return the mean value of the energy loss.
| Matrix<DIM_V, DIM_V> calculateProcessNoiseCovariance() | ||
| { | ||
| // Calculate the process noise covariance matrix | ||
| Matrix<DIM_V, DIM_V> matQ{Matrix<DIM_V, DIM_V>::Zero()}; | ||
| matQ = m_matQ; // Use the stored process noise covariance matrix | ||
|
|
||
| // TODO: Set the process noise covariance for angular straggle and energy loss. | ||
| return matQ; | ||
| } |
There was a problem hiding this comment.
@Yassid This is what builds the covariance matrix of the process noise. So energy and angular straggeling
| } | ||
| }; | ||
|
|
||
| TEST_F(TrackFitterUKFExampleTest, Prediction) |
There was a problem hiding this comment.
This test is copy/paste from OpenKF
| // Expectations from the python results: | ||
| // ====================================== | ||
| // x = | ||
| // [ 2.4758845 0.53327217 0.21649734 -0.21214576] | ||
| // P = | ||
| // [[ 0.01433114 -0.01026142 0.00651178 -0.00465059] | ||
| // [-0.01026142 0.0295458 -0.0046378 0.01344241] | ||
| // [ 0.00651178 -0.0046378 0.13023154 -0.00210188] | ||
| // [-0.00465059 0.01344241 -0.00210188 0.1333886 ]] |
There was a problem hiding this comment.
These values are from a python implementation of the correction I wrote. It's what I used to validate since I didn't have applicable test results from OpenKF. Python and cpp implementations agree so I think the math was implemented correctly
| ASSERT_NEAR(m_ukf.matP()(3, 3), 0.1333886F, FLOAT_EPSILON); | ||
| } | ||
|
|
||
| class TrackFitterUKFPhysicsTest : public testing::Test { |
There was a problem hiding this comment.
@Yassid This is the physics test case to fill. I think this is how we should initially implement and verify the physics.
| static kf::Vector<DIM_X> funcF(const kf::Vector<DIM_X> &x, const kf::Vector<DIM_V> &v) | ||
| { | ||
| // TODO: This needs to be filled with an RK4 solver for the physics model | ||
| kf::Vector<DIM_X> y{x}; | ||
|
|
||
| // For now, we just return the state vector as is | ||
| return y; | ||
| } |
There was a problem hiding this comment.
@Yassid This is the propagator. It takes in the state vector and noise vector as defined in the Overleaf.
| TEST_F(TrackFitterUKFPhysicsTest, PhysicsPrediction) | ||
| { | ||
| // TODO: This needs to be filled with a proper physics test. | ||
| kf::Vector<DIM_X> x; // Initial state vector | ||
|
|
||
| kf::Matrix<DIM_X, DIM_X> P; // Initial state vector covariance matrix | ||
|
|
||
| // Note: process noise is defined in the UKF class, so we don't need to set it here. | ||
|
|
||
| kf::Vector<DIM_Z> z; // Measurement vector to be used in the correction step | ||
| z << std::sqrt(5), std::atan2(1.f, 2.f); | ||
|
|
||
| kf::Matrix<DIM_N, DIM_N> R; // Covariance matrix for the measurement noise | ||
|
|
||
| m_ukf.vecX() = x; | ||
| m_ukf.matP() = P; | ||
|
|
||
| m_ukf.setCovarianceR(R); | ||
| m_ukf.predictUKF(funcF); | ||
| } |
There was a problem hiding this comment.
@Yassid Once we have the propagator, we will need to fill in the rest of this test
We may also want to add some specific tets cases for the funcF method
|
@Yassid I put comments on this PR where we will need to add your code. It might be easiest to work off of this branch and then merge it all at once rather than merge to develop and then work from there. But I'm open to suggestions. |
Our prediction step matches the results of the OpenKF example and is passing that portion of the test. It is failing the update step, but this might not be a problem as we are not adding the measurement noise to the augmented state vector, as the OpenKF example does. Instead we are treating the measurement noise analytically. This might cause the small differences in the results, but I need to run through the math to confirm that.
Removed some unnecessary updates to kappa Fixed missleading comment
Also adds tests.
It seems to be propagating initially in the wrong direction. It gets "dragged" back to following the points, but that seems to blow up the momentum uncertainty (which makes sense if the initial momentum is somehow in the wrong direction, and the measurement points need to force it to change its mind). I think that causes it to fail somewhere around point 23, becuase of sigma points are sampling such a large distribution due to the momentum uncertainty that they start stopping in the gas (or have a negative momentum). Checks for physicality need to be added (step size, momentum values, etc.)
Fix issue with stragling being calculated over last timestep rather then the whole propagation from previous point to next measurement POCA. Add additional functions to UKF to disable straggling. Add functions to pull intermediate results from UKF. Add some additional CATIMA straggling functions (pull from calculation, not just range tables that are pre-calculated).
Smoother is more stable, but it still fail part way through, so we are still poorly conditioned somewhere.
Ensures each update to a covariance matrix maintains PD.
1 participant
No description provided.