Skip to content
Merged
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
23 changes: 16 additions & 7 deletions src/pyrecest/filters/iterated_batch_mem_qkf_tracker.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@

# pylint: disable=no-name-in-module,no-member,too-many-arguments,too-many-locals,protected-access
import numpy as np

from pyrecest.backend import array, diag, linalg, mean, zeros

from .mem_qkf_tracker import MEMQKFTracker
Expand Down Expand Up @@ -151,9 +150,13 @@ def update(
shape_measurement_covariance=shape_measurement_covariance,
)
)
posterior_orientation = prior_orientation + self.damping * self._axial_delta(
prior_orientation,
full_orientation,
posterior_orientation = (
prior_orientation
+ self.damping
* self._axial_delta(
prior_orientation,
full_orientation,
)
)

full_axes, posterior_axis_covariance = self._batch_axis_posterior(
Expand Down Expand Up @@ -208,7 +211,9 @@ def _batch_extent_center_and_additive_covariance(
return mean(measurements, axis=1), zeros((2, 2))

center_estimate = measurement_matrix @ self.kinematic_state
additive_shape_covariance = measurement_matrix @ self.covariance @ measurement_matrix.T
additive_shape_covariance = (
measurement_matrix @ self.covariance @ measurement_matrix.T
)
return center_estimate, additive_shape_covariance

# pylint: disable=too-many-arguments,too-many-positional-arguments
Expand Down Expand Up @@ -247,7 +252,9 @@ def _batch_orientation_posterior(
]
)

extent_covariance = extent_transform @ multiplicative_noise_cov @ extent_transform.T
extent_covariance = (
extent_transform @ multiplicative_noise_cov @ extent_transform.T
)
orientation_covariance = orientation_variance * array(
[
[
Expand Down Expand Up @@ -428,7 +435,9 @@ def _batch_axis_posterior(
- axis_sensitivity @ (prior_axes - semi_axes)
)
posterior_axes = prior_axes + axis_gain @ innovation
posterior_axis_covariance = axis_covariance - axis_gain @ axis_cross_covariance.T
posterior_axis_covariance = (
axis_covariance - axis_gain @ axis_cross_covariance.T
)
return posterior_axes, self._symmetrize(posterior_axis_covariance)

@staticmethod
Expand Down