Create geophysical RBPF implementation#232
Conversation
There was a problem hiding this comment.
Pull request overview
This PR implements a Rao-Blackwellized Particle Filter (RBPF) for geophysical navigation applications, extending the existing particle filter to handle geophysical bias states. The implementation allows particles to handle non-linearities in position while using Extended Kalman Filters for INS states (velocity, attitude) and geophysical biases within each particle.
Changes:
- Modified RBPF to support additional "extra" states (geophysical biases) with per-particle covariance tracking
- Added configuration parameters for geophysical bias initialization and process noise
- Updated measurement models to support bias state indexing from the end of state vectors
- Added configuration files for RBPF simulations with magnetic anomaly measurements
- Integrated health monitoring into particle filter processing
Reviewed changes
Copilot reviewed 12 out of 95 changed files in this pull request and generated 7 comments.
Show a summary per file
| File | Description |
|---|---|
| core/src/rbpf.rs | Core RBPF implementation extended to support extra states with per-particle covariance matrices |
| core/src/sim.rs | Configuration structure extended with particle filter parameters for bias states |
| geonav/src/lib.rs | Measurement models updated to extract state from provided vectors and support bias indexing |
| sim/src/main.rs | CLI arguments and configuration handling for RBPF with geophysical bias states |
| sim/src/common.rs | Log directory creation helper to ensure parent directories exist |
| conf/rbpf_truth.toml | Configuration for truth-based RBPF simulation |
| conf/rbpf_mag.toml | Configuration for RBPF with magnetic anomaly measurements |
| data/output/ekf/truth/2025-11-09_17-34-01.png | Generated plot artifact (should not be committed) |
| pixi.toml | Whitespace cleanup |
Comments suppressed due to low confidence (4)
sim/src/main.rs:545
- The
geo_bias_dimcalculation assumes that each map (gravity and magnetic) contributes exactly one bias state. This is implicit and could be fragile if the bias model changes. Consider adding a comment explaining this assumption or using named constants to make the relationship explicit.
geonav/src/lib.rs:592 - The bias indexing logic checks
offset == 0which would never be true in valid usage sincebias_from_endof 0 would mean "0 from the end", which is meaningless. Consider changing the check tooffset <= 0oroffset < 1for clarity, or document that offset must be >= 1. Alternatively, this check might be redundant since the conditionstate.len() < offsetalready handles invalid cases.
analysis/src/analysis/preprocess.py:7 - Import of 'futures' is not used.
from concurrent import futures
analysis/src/analysis/preprocess.py:153
- Unnecessary 'pass' statement.
pass
| linear_update_applied: bool, | ||
| } | ||
|
|
||
| impl RaoBlackwellizedParticleFilter { |
There was a problem hiding this comment.
The helper function linear_state_dim() is defined as a private method but is used extensively throughout the code. Consider adding documentation to explain its purpose: it returns the total dimension of the linear state including base states (velocity + attitude = 6) and any extra states (e.g., geophysical biases).
| impl RaoBlackwellizedParticleFilter { | |
| impl RaoBlackwellizedParticleFilter { | |
| /// Returns the total dimension of the shared linear state used by the RBPF. | |
| /// | |
| /// The base linear state consists of 6 error states: | |
| /// 3 velocity errors and 3 attitude (orientation) errors. Any additional | |
| /// application-specific states (for example, geophysical biases) are | |
| /// appended after these base states and counted via `extra_state_dim`. |
| linear_cov: linear_cov.clone(), | ||
| weight, | ||
| }); |
There was a problem hiding this comment.
The covariance matrix is cloned for every particle during initialization. With 1000 particles (as in the config files) and potentially large state dimensions, this could be memory-intensive. Consider whether this design is necessary or if a more memory-efficient approach could be used. Each particle maintains its own covariance, which is correct for RBPF but should be documented for clarity about the memory implications.
| if self.config.extra_state_dim > 0 && self.config.extra_state_process_noise_std > 0.0 { | ||
| for i in 0..self.config.extra_state_dim { | ||
| let idx = LINEAR_STATE_DIM_BASE + i; | ||
| let noise = normal.sample(&mut self.rng) | ||
| * self.config.extra_state_process_noise_std | ||
| * dt; | ||
| x_l_pred[idx] += noise; | ||
| } | ||
| } |
There was a problem hiding this comment.
The noise sampling for extra states adds random walk noise directly to the predicted state. This is done independently of the Kalman update, which differs from how the base states (velocity/attitude) are handled through the full Rao-Blackwellization framework. While this may be intentional for modeling slow-varying biases, the inconsistency should be documented to explain why extra states bypass the standard prediction equations.
| fn particle_state_vector_full(&self, particle: &RbpfParticle) -> DVector<f64> { | ||
| let mut state = self.particle_state_vector(particle).as_slice().to_vec(); | ||
| if self.config.extra_state_dim > 0 { | ||
| state.extend_from_slice( | ||
| particle | ||
| .linear_state | ||
| .rows(LINEAR_STATE_DIM_BASE, self.config.extra_state_dim) | ||
| .as_slice(), | ||
| ); | ||
| } | ||
| DVector::from_vec(state) | ||
| } |
There was a problem hiding this comment.
The particle_state_vector_full function extends the state vector with extra states for measurement evaluation. However, the ordering assumption (extra states appended at the end) is implicit. This should be documented clearly, especially since measurement models like GravityMeasurement use bias_from_end to index these states from the end of the vector.
| for i in 0..LINEAR_STATE_DIM_BASE { | ||
| particle.linear_state[i] -= mean_lin_base[i]; | ||
| } |
There was a problem hiding this comment.
During recentering, only the base linear states (first 6 elements) are adjusted, while extra states remain untouched. This asymmetry should be documented to explain why bias states are not recentered. If biases are meant to remain uncentered to preserve their estimated values, this design choice should be explicit.
| # Find all CSV files under the input directory | ||
| all_csv = list(input_path.rglob("*.csv")) | ||
|
|
There was a problem hiding this comment.
Variable all_csv is not used.
| # Find all CSV files under the input directory | |
| all_csv = list(input_path.rglob("*.csv")) |
| # def process_dataset(dataset: Path): | ||
| for dataset in tqdm(datasets): | ||
| # dataset_path = os.path.join(args.input_dir, dataset) | ||
| cleaned_data = pd.DataFrame() |
Implement a Rao-Blackwellized Particle Filter (RBPF) for geophysical applications, allowing for the handling of non-linearities in position through particles while utilizing Extended Kalman Filters for INS states. Each particle maintains a geophysical bias state with minimal random walk noise. Configuration options for the number of particles and process noise characteristics are added, accessible via CLI and configuration files.
Fixes #230