diff --git a/src/pyrecest/distributions/conditional/sd_half_cond_sd_half_grid_distribution.py b/src/pyrecest/distributions/conditional/sd_half_cond_sd_half_grid_distribution.py index ae92fb677..d34004348 100644 --- a/src/pyrecest/distributions/conditional/sd_half_cond_sd_half_grid_distribution.py +++ b/src/pyrecest/distributions/conditional/sd_half_cond_sd_half_grid_distribution.py @@ -57,11 +57,11 @@ def __init__(self, grid, grid_values, enforce_pdf_nonnegative=True): def _check_normalization(self, tol=0.01): """Warn if any column is not normalized to 1 over the hemisphere.""" - # Match HyperhemisphericalGridDistribution.get_manifold_size(), which - # uses the grid embedding dimension for the existing quadrature rule. + # Match HyperhemisphericalGridDistribution.get_manifold_size(): a grid + # in R^d represents S^{d-1}, so the quadrature surface uses d - 1. hemisphere_surface = 0.5 * ( AbstractHypersphereSubsetDistribution.compute_unit_hypersphere_surface( - self.grid.shape[1] + self.grid.shape[1] - 1 ) ) ints = mean(self.grid_values, axis=0) * hemisphere_surface diff --git a/src/pyrecest/filters/_linear_gaussian.py b/src/pyrecest/filters/_linear_gaussian.py index d74dc16d5..57b59fe23 100644 --- a/src/pyrecest/filters/_linear_gaussian.py +++ b/src/pyrecest/filters/_linear_gaussian.py @@ -204,6 +204,36 @@ def linear_gaussian_predict( return predicted_mean, predicted_covariance +def linear_gaussian_innovation( + mean, covariance, measurement, measurement_matrix, meas_noise +): + """Return innovation and innovation covariance for a linear measurement.""" + mean = _as_vector(mean, "mean") + covariance = _as_matrix(covariance, "covariance") + measurement = _as_vector(measurement, "measurement") + measurement_matrix = _as_matrix(measurement_matrix, "measurement_matrix") + meas_noise = _as_matrix(meas_noise, "meas_noise") + + state_dim = mean.shape[0] + meas_dim = measurement_matrix.shape[0] + + if covariance.shape != (state_dim, state_dim): + raise ValueError("covariance must have shape (state_dim, state_dim)") + if measurement_matrix.shape[1] != state_dim: + raise ValueError("measurement_matrix has incompatible shape") + if measurement.shape[0] != meas_dim: + raise ValueError("measurement has incompatible shape") + if meas_noise.shape != (meas_dim, meas_dim): + raise ValueError("meas_noise must have shape (meas_dim, meas_dim)") + + innovation = measurement - measurement_matrix @ mean + innovation_cov = ( + measurement_matrix @ covariance @ transpose(measurement_matrix) + meas_noise + ) + innovation_cov = 0.5 * (innovation_cov + transpose(innovation_cov)) + return innovation, innovation_cov + + def linear_gaussian_update( mean, covariance, diff --git a/src/pyrecest/filters/kalman_filter.py b/src/pyrecest/filters/kalman_filter.py index 232603ff0..e23d655f9 100644 --- a/src/pyrecest/filters/kalman_filter.py +++ b/src/pyrecest/filters/kalman_filter.py @@ -4,9 +4,11 @@ from pyrecest.distributions import GaussianDistribution from ._linear_gaussian import ( + linear_gaussian_innovation, linear_gaussian_predict, linear_gaussian_update, linear_gaussian_update_robust, + normalized_innovation_squared, ) from .abstract_filter import AbstractFilter from .manifold_mixins import EuclideanFilterMixin @@ -211,6 +213,30 @@ def update_identity( action=action, ) + def innovation_linear(self, measurement, measurement_matrix, meas_noise): + """Return innovation and innovation covariance for a linear measurement.""" + return linear_gaussian_innovation( + self._filter_state.mu, + self._filter_state.C, + measurement, + measurement_matrix, + meas_noise, + ) + + def normalized_innovation_squared_linear( + self, + measurement, + measurement_matrix, + meas_noise, + ): + """Return the normalized innovation squared for a linear measurement.""" + innovation, innovation_covariance = self.innovation_linear( + measurement, + measurement_matrix, + meas_noise, + ) + return normalized_innovation_squared(innovation, innovation_covariance) + def update_linear( self, measurement, @@ -307,6 +333,29 @@ def update_linear_robust( return diagnostics return None + def update_model_robust( + self, + measurement_model, + measurement, + **kwargs, + ): + """Robustly update with a structural linear Gaussian model object.""" + measurement_matrix = _get_required_model_attribute( + measurement_model, + "measurement_matrix", + ) + meas_noise = _get_required_model_attribute( + measurement_model, + "meas_noise", + "measurement_noise_cov", + ) + return self.update_linear_robust( + measurement, + measurement_matrix, + meas_noise, + **kwargs, + ) + def update_model( self, measurement_model, diff --git a/src/pyrecest/filters/so3_grid_transition.py b/src/pyrecest/filters/so3_grid_transition.py index 4c588c630..577974f0c 100644 --- a/src/pyrecest/filters/so3_grid_transition.py +++ b/src/pyrecest/filters/so3_grid_transition.py @@ -88,10 +88,10 @@ def so3_right_multiplication_grid_transition( column_maxima = reshape(amax(exponents, axis=0), (1, exponents.shape[1])) scores = exp(exponents - column_maxima) - manifold_size = ( - 0.5 - * AbstractHypersphereSubsetDistribution.compute_unit_hypersphere_surface( - quaternion_grid.shape[1] + manifold_dim = quaternion_grid.shape[1] - 1 + manifold_size = 0.5 * ( + AbstractHypersphereSubsetDistribution.compute_unit_hypersphere_surface( + manifold_dim ) ) column_integrals = sum(scores, axis=0, keepdims=True) / scores.shape[0] diff --git a/tests/filters/test_kalman_nis.py b/tests/filters/test_kalman_nis.py new file mode 100644 index 000000000..ca3c8c3c7 --- /dev/null +++ b/tests/filters/test_kalman_nis.py @@ -0,0 +1,28 @@ +import unittest + +from pyrecest.backend import allclose, array, diag, eye +from pyrecest.filters.kalman_filter import KalmanFilter + + +class KalmanNisTest(unittest.TestCase): + def test_nis_and_state_is_unchanged(self): + kf = KalmanFilter((array([1.0, -1.0]), diag(array([2.0, 3.0])))) + mean_before = kf.get_point_estimate() + cov_before = kf.filter_state.C + + innovation, innovation_covariance = kf.innovation_linear( + array([4.0, 2.0]), eye(2), diag(array([5.0, 7.0])) + ) + nis = kf.normalized_innovation_squared_linear( + array([4.0, 2.0]), eye(2), diag(array([5.0, 7.0])) + ) + + self.assertTrue(allclose(innovation, array([3.0, 3.0]))) + self.assertTrue(allclose(innovation_covariance, diag(array([7.0, 10.0])))) + self.assertTrue(allclose(nis, array(3.0**2 / 7.0 + 3.0**2 / 10.0))) + self.assertTrue(allclose(kf.get_point_estimate(), mean_before)) + self.assertTrue(allclose(kf.filter_state.C, cov_before)) + + +if __name__ == "__main__": + unittest.main()