Skip to content
Closed
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
Original file line number Diff line number Diff line change
Expand Up @@ -65,9 +65,7 @@ def integrand(u):
b23_0 = iv(0, t23)
b23_1 = iv(1, t23)
base = b01_0 * b23_0
exp_factor = _np.exp(
0.5 * (Z[0] + Z[1]) * u + 0.5 * (Z[2] + Z[3]) * u_comp
)
exp_factor = _np.exp(0.5 * (Z[0] + Z[1]) * u + 0.5 * (Z[2] + Z[3]) * u_comp)
dF = _np.array(
[
exp_factor * 0.5 * u * (b01_1 * b23_0 + base),
Expand Down
2 changes: 1 addition & 1 deletion src/pyrecest/filters/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -79,11 +79,11 @@
from .random_matrix_tracker import RandomMatrixTracker
from .se2_ukf import SE2UKF
from .so3_product_particle_filter import SO3ProductParticleFilter
from .state_space_subdivision_filter import StateSpaceSubdivisionFilter
from .spherical_harmonics_eot_tracker import (
SphericalHarmonicsEOTTracker,
SphericalHarmonicsExtendedObjectTracker,
)
from .state_space_subdivision_filter import StateSpaceSubdivisionFilter
from .toroidal_particle_filter import ToroidalParticleFilter
from .toroidal_wrapped_normal_filter import ToroidalWrappedNormalFilter
from .track_manager import (
Expand Down
21 changes: 11 additions & 10 deletions src/pyrecest/filters/spherical_harmonics_eot_tracker.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
from __future__ import annotations

import numpy as np

import pyrecest.backend

# pylint: disable=no-member,no-name-in-module,too-many-lines
Expand Down Expand Up @@ -34,7 +33,9 @@
from .abstract_extended_object_tracker import AbstractExtendedObjectTracker


class SphericalHarmonicsEOTTracker(AbstractExtendedObjectTracker): # pylint: disable=too-many-instance-attributes
class SphericalHarmonicsEOTTracker(
AbstractExtendedObjectTracker
): # pylint: disable=too-many-instance-attributes
"""3-D star-convex EOT tracker with spherical-harmonic extent coefficients.

The state is ``[cx, cy, cz, c_00, c_1,-1, c_1,0, c_1,1, ...]``. The
Expand Down Expand Up @@ -256,7 +257,9 @@ def _complex_coeff_mat_to_real(complex_coeff_mat):
* real(complex_coeff_mat[degree, degree + order])
)
else:
real_coeff_mat[degree, degree] = real(complex_coeff_mat[degree, degree])
real_coeff_mat[degree, degree] = real(
complex_coeff_mat[degree, degree]
)
return real_coeff_mat

@staticmethod
Expand Down Expand Up @@ -305,13 +308,11 @@ def evaluate_radius_from_coefficients(coefficients, directions):
radii = zeros(directions.shape[1])
for degree in range(coeff_mat.shape[0]):
for order in range(-degree, degree + 1):
basis_values = (
SphericalHarmonicsDistributionReal.real_spherical_harmonic_basis_function(
degree,
order,
theta,
phi,
)
basis_values = SphericalHarmonicsDistributionReal.real_spherical_harmonic_basis_function(
degree,
order,
theta,
phi,
)
radii += coeff_mat[degree, degree + order] * basis_values
return radii
Expand Down
12 changes: 9 additions & 3 deletions tests/filters/test_spherical_harmonics_eot_tracker.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,9 @@ def test_coefficient_vector_matrix_roundtrip(self):
coefficients = array([0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0])

coeff_mat = SphericalHarmonicsEOTTracker.coefficients_to_matrix(coefficients)
coefficients_roundtrip = SphericalHarmonicsEOTTracker.matrix_to_coefficients(coeff_mat)
coefficients_roundtrip = SphericalHarmonicsEOTTracker.matrix_to_coefficients(
coeff_mat
)

self.assertEqual(coeff_mat.shape, (3, 5))
npt.assert_allclose(coefficients_roundtrip, coefficients)
Expand Down Expand Up @@ -64,7 +66,9 @@ def test_measurement_function_projects_points_to_current_surface(self):
]
)

predicted = tracker.measurement_function(tracker.get_point_estimate(), measurements)
predicted = tracker.measurement_function(
tracker.get_point_estimate(), measurements
)

expected_points = array(
[
Expand All @@ -87,7 +91,9 @@ def test_update_increases_radius_toward_far_measurement(self):

tracker.update(array([2.0, 0.0, 0.0]), meas_noise_cov=0.01 * eye(3))

self.assertGreater(tracker.evaluate_radius(array([1.0, 0.0, 0.0])), prior_radius)
self.assertGreater(
tracker.evaluate_radius(array([1.0, 0.0, 0.0])), prior_radius
)
self.assertLess(tracker.covariance[3, 3], prior_covariance[3, 3])
self.assertTrue(isfinite(tracker.latest_predicted_measurement).all())
self.assertTrue(linalg.eigvalsh(tracker.covariance)[0] > -1e-8)
Expand Down