Add Kalman innovation diagnostics

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

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,

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,

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]

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()