fix: add missing x0/Pi0 goal fusion to PPHybridFilterLinear#75
Merged
iahncajigas merged 3 commits intomainfrom Mar 23, 2026
Merged
fix: add missing x0/Pi0 goal fusion to PPHybridFilterLinear#75iahncajigas merged 3 commits intomainfrom
iahncajigas merged 3 commits intomainfrom
Conversation
Adds the initial state fusion step (Srinivasan et al. Eq. 2.23)
that was present in PPDecodeFilterLinear but missing from the hybrid
filter since its original implementation:
Pi0New = pinv(inv(Pi0) + inv(PitT[:,:,0]))
x0New = Pi0New @ (inv(Pi0) @ x0 + inv(PitT) @ PhitT @ yT)
This fuses the initial state prior with the terminal constraint,
giving the goal-directed filter a better starting point that
incorporates knowledge of where the trajectory must end.
Also regenerates fig06 with the corrected algorithm.
Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
The goal-directed predict step in PPHybridFilterLinear should use B[:,:,0] (initial modified dynamics) and QT[:,:,-1] (terminal modified noise) at every time step, matching the original Srinivasan et al. formulation and the MATLAB implementation. The previous code used time-varying B[:,:,time_index] and QT[:,:,time_index], which caused the goal-directed filter to undershoot the target. Mirrors MATLAB PR cajigaslab/nSTAT#29. Also regenerates all example05 figures with corrected tracking. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
…l-fusion # Conflicts: # docs/figures/example05/fig06_hybrid_decoding_summary.png
This file contains hidden or 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
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
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.
Summary
Adds the initial state fusion step (Srinivasan et al. Eq. 2.23) to PPHybridFilterLinear. This step fuses the initial state prior with the terminal constraint:
This was present in
PPDecodeFilterLinearsince the original 2013 implementation but was never added to the hybrid filter variants. Without it, the goal-directed hybrid filter starts from the raw prior without incorporating target information.Mirrors MATLAB PR cajigaslab/nSTAT#27.
Test plan
🤖 Generated with Claude Code