Time error projected kalman filter #1036
Open
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
|
Is there any literature to that? |
Didn't find literature, just blog post that I liked |
schmidma
requested changes
Jun 5, 2024
| @@ -322,6 +322,7 @@ | |||
| "process_noise": [0.005, 0.005, 0.2, 0.2], | |||
| "measurement_noise_moving": [0.5, 2.0], | |||
| "measurement_noise_resting": [300.0, 500.0], | |||
| "measurement_time_noise": 40.0, | |||
Comment on lines
+6
to
+22
| pub trait TimeErrorProjectedKalmanFilter<const STATE_DIMENSION: usize> { | ||
| fn predict<const CONTROL_DIMENSION: usize>( | ||
| &mut self, | ||
| state_prediction: SMatrix<f32, STATE_DIMENSION, STATE_DIMENSION>, | ||
| control_input_model: SMatrix<f32, STATE_DIMENSION, CONTROL_DIMENSION>, | ||
| control: SVector<f32, CONTROL_DIMENSION>, | ||
| process_noise: SMatrix<f32, STATE_DIMENSION, STATE_DIMENSION>, | ||
| ); | ||
| fn update<const MEASUREMENT_DIMENSION: usize>( | ||
| &mut self, | ||
| measurement_prediction: SMatrix<f32, MEASUREMENT_DIMENSION, STATE_DIMENSION>, | ||
| measurement: SVector<f32, MEASUREMENT_DIMENSION>, | ||
| measurement_noise: SMatrix<f32, MEASUREMENT_DIMENSION, MEASUREMENT_DIMENSION>, | ||
| state_derivative: SVector<f32, STATE_DIMENSION>, | ||
| measurement_time_noise: f32, | ||
| ); | ||
| } |
There was a problem hiding this comment.
Suggested change
| pub trait TimeErrorProjectedKalmanFilter<const STATE_DIMENSION: usize> { | |
| fn predict<const CONTROL_DIMENSION: usize>( | |
| &mut self, | |
| state_prediction: SMatrix<f32, STATE_DIMENSION, STATE_DIMENSION>, | |
| control_input_model: SMatrix<f32, STATE_DIMENSION, CONTROL_DIMENSION>, | |
| control: SVector<f32, CONTROL_DIMENSION>, | |
| process_noise: SMatrix<f32, STATE_DIMENSION, STATE_DIMENSION>, | |
| ); | |
| fn update<const MEASUREMENT_DIMENSION: usize>( | |
| &mut self, | |
| measurement_prediction: SMatrix<f32, MEASUREMENT_DIMENSION, STATE_DIMENSION>, | |
| measurement: SVector<f32, MEASUREMENT_DIMENSION>, | |
| measurement_noise: SMatrix<f32, MEASUREMENT_DIMENSION, MEASUREMENT_DIMENSION>, | |
| state_derivative: SVector<f32, STATE_DIMENSION>, | |
| measurement_time_noise: f32, | |
| ); | |
| } | |
| pub trait TimeErrorProjectedKalmanFilter<const STATE_DIMENSION: usize> { | |
| fn update_with_timing_noise<const MEASUREMENT_DIMENSION: usize>( | |
| &mut self, | |
| measurement_prediction: SMatrix<f32, MEASUREMENT_DIMENSION, STATE_DIMENSION>, | |
| measurement: SVector<f32, MEASUREMENT_DIMENSION>, | |
| measurement_noise: SMatrix<f32, MEASUREMENT_DIMENSION, MEASUREMENT_DIMENSION>, | |
| state_derivative: SVector<f32, STATE_DIMENSION>, | |
| measurement_time_noise: f32, | |
| ); | |
| } |
| use types::multivariate_normal_distribution::MultivariateNormalDistribution; | ||
|
|
||
| use crate::kalman_filter::KalmanFilter; | ||
|
|
There was a problem hiding this comment.
Please link the blogpost somewhere here
Comment on lines
+51
to
+64
| let residual = measurement - measurement_prediction * self.mean; | ||
| let time_error_adjusted_covariance = self.covariance | ||
| + measurement_noise_variance * state_derivative * state_derivative.transpose(); | ||
| let residual_covariance = measurement_prediction | ||
| * time_error_adjusted_covariance | ||
| * measurement_prediction.transpose() | ||
| + measurement_noise; | ||
| let kalman_gain = self.covariance | ||
| * measurement_prediction.transpose() | ||
| * residual_covariance | ||
| .try_inverse() | ||
| .expect("Residual covariance matrix is not invertible"); | ||
| self.mean += kalman_gain * residual; | ||
| self.covariance -= kalman_gain * measurement_prediction * self.covariance; |
There was a problem hiding this comment.
I'm not sure whether we want to split up the Kalman update into smaller steps, to reuse the functionality between the normal and the time-invariant version #codeDuplication
Comment on lines
+340
to
+347
| let resting_state_derivative = nalgebra::vector![ | ||
| hypothesis.resting_state.mean.z, | ||
| hypothesis.resting_state.mean.w, | ||
| (configuration.velocity_decay_factor - 1.) * hypothesis.resting_state.mean.z | ||
| / cycle_time.as_secs_f32(), | ||
| (configuration.velocity_decay_factor - 1.) * hypothesis.resting_state.mean.w | ||
| / cycle_time.as_secs_f32(), | ||
| ]; |
There was a problem hiding this comment.
Why is this repeated, if it is the same derivative as above?
Comment on lines
+298
to
+299
| KalmanFilter::predict( | ||
| &mut hypothesis.moving_state, |
There was a problem hiding this comment.
If you'd rename the trait function as commented below, you can revert this explicit syntax back to the self.xxx
| @@ -97,6 +104,11 @@ impl BallFilter { | |||
| context: &CycleContext, | |||
| ) -> Vec<Hypothesis> { | |||
| for (detection_time, balls) in measurements { | |||
| let cycle_time = detection_time | |||
There was a problem hiding this comment.
Suggested change
| let cycle_time = detection_time | |
| let last_cycle_duration = detection_time |
? What do you think?
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Why? What?
Mitigates the effects of inaccurate measurement times.
The innovation covariance is adjusted by the derivative of the state, causing the filter to be more uncertain in the direction of state change.
Also fixes the covariance plotting in twix, which had an incorrect orientation before.
Fixes #
ToDo / Known Issues
If this is a WIP describe which problems are to be fixed.
Ideas for Next Iterations (Not This PR)
If there are some improvements that could be done in a next iteration, describe them here.
How to Test
Ball Filteroverlay in twix