Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
275 changes: 213 additions & 62 deletions src/pyrecest/filters/mode_rbpf_manifold_ukf_tracker.py

Large diffs are not rendered by default.

4 changes: 3 additions & 1 deletion src/pyrecest/filters/velocity_aided_mem_qkf_tracker.py
Original file line number Diff line number Diff line change
Expand Up @@ -153,7 +153,9 @@ def _fuse_orientation_with_heading_values(
)
gain = orientation_variance / innovation_variance
posterior_orientation = orientation + gain * innovation
posterior_orientation_variance = orientation_variance - gain * orientation_variance
posterior_orientation_variance = (
orientation_variance - gain * orientation_variance
)
posterior_orientation_variance = self._regularize_variance(
posterior_orientation_variance
)
Expand Down
55 changes: 37 additions & 18 deletions src/pyrecest/smoothers/fixed_lag_random_matrix_smoother.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,9 @@
from dataclasses import dataclass
from typing import Any, Sequence

from pyrecest.backend import asarray, copy as backend_copy, eye, linalg
from pyrecest.backend import asarray
from pyrecest.backend import copy as backend_copy
from pyrecest.backend import eye, linalg
from pyrecest.filters.random_matrix_tracker import RandomMatrixTracker

from .abstract_smoother import AbstractSmoother
Expand All @@ -30,9 +32,11 @@ def from_tracker(cls, tracker: RandomMatrixTracker) -> "RandomMatrixTrackerState
backend_copy(tracker.covariance),
backend_copy(tracker.extent),
float(tracker.alpha),
None
if tracker.kinematic_state_to_pos_matrix is None
else backend_copy(tracker.kinematic_state_to_pos_matrix),
(
None
if tracker.kinematic_state_to_pos_matrix is None
else backend_copy(tracker.kinematic_state_to_pos_matrix)
),
)

def copy(self) -> "RandomMatrixTrackerState":
Expand All @@ -43,9 +47,11 @@ def copy(self) -> "RandomMatrixTrackerState":
backend_copy(self.covariance),
backend_copy(self.extent),
float(self.alpha),
None
if self.kinematic_state_to_pos_matrix is None
else backend_copy(self.kinematic_state_to_pos_matrix),
(
None
if self.kinematic_state_to_pos_matrix is None
else backend_copy(self.kinematic_state_to_pos_matrix)
),
)

def to_tracker(self) -> RandomMatrixTracker:
Expand All @@ -55,9 +61,11 @@ def to_tracker(self) -> RandomMatrixTracker:
backend_copy(self.kinematic_state),
backend_copy(self.covariance),
backend_copy(self.extent),
None
if self.kinematic_state_to_pos_matrix is None
else backend_copy(self.kinematic_state_to_pos_matrix),
(
None
if self.kinematic_state_to_pos_matrix is None
else backend_copy(self.kinematic_state_to_pos_matrix)
),
)
tracker.alpha = float(self.alpha)
return tracker
Expand Down Expand Up @@ -124,7 +132,9 @@ def _as_state(state) -> RandomMatrixTrackerState:
)

@classmethod
def _normalize_state_sequence(cls, states: Sequence) -> list[RandomMatrixTrackerState]:
def _normalize_state_sequence(
cls, states: Sequence
) -> list[RandomMatrixTrackerState]:
return [cls._as_state(state) for state in states]

def _positive_extent_weight(self, alpha) -> float:
Expand Down Expand Up @@ -189,12 +199,17 @@ def _smooth_window(
).T
gains[time_idx] = smoother_gain

smoothed_kinematic_state = filtered_state.kinematic_state + smoother_gain @ (
next_smoothed.kinematic_state - predicted_state.kinematic_state
smoothed_kinematic_state = (
filtered_state.kinematic_state
+ smoother_gain
@ (next_smoothed.kinematic_state - predicted_state.kinematic_state)
)
smoothed_covariance = (
filtered_state.covariance
+ smoother_gain
@ (next_smoothed.covariance - predicted_state.covariance)
@ smoother_gain.T
)
smoothed_covariance = filtered_state.covariance + smoother_gain @ (
next_smoothed.covariance - predicted_state.covariance
) @ smoother_gain.T
smoothed_extent, smoothed_alpha = self._smooth_extent(
filtered_state,
predicted_state,
Expand Down Expand Up @@ -230,7 +245,9 @@ def smooth(
return [state.copy() for state in filt_list], [[] for _ in filt_list]

if predicted_states is None:
raise ValueError("predicted_states must be provided for non-zero lag smoothing.")
raise ValueError(
"predicted_states must be provided for non-zero lag smoothing."
)
pred_list = self._normalize_state_sequence(predicted_states)
if len(pred_list) != len(filt_list) - 1:
raise ValueError(
Expand Down Expand Up @@ -288,7 +305,9 @@ def append(
eye(state_dim) if system_matrix is None else asarray(system_matrix)
)
elif predicted_state is not None:
raise ValueError("predicted_state must not be provided for the first filtered state.")
raise ValueError(
"predicted_state must not be provided for the first filtered state."
)

self._filtered_buffer.append(new_filtered_state)
if len(self._filtered_buffer) <= self.lag:
Expand Down
Loading