Merge pull request #112 from decargroup/add_tutorial

Add a short tutorial on CompositeState

docs/source/api.rst

@@ -7,6 +7,7 @@ Below is a list of all the modules in the navlie package.  Click on any of the l
    :recursive:
    :template: custom-module-template.rst
 
+   navlie.composite
    navlie.datagen
    navlie.filters
    navlie.imm

docs/source/tutorial.rst

@@ -23,4 +23,5 @@ All the dependencies should get installed by this command and the package should
    1. Getting Started <self>
    2. Toy Problem - Traditional <./tutorial/traditional.ipynb>
    3. Toy Problem - Lie groups <./tutorial/lie_groups.ipynb>
-   4. Specifying Jacobians <./tutorial/jacobians.ipynb>
+   4. Specifying Jacobians <./tutorial/jacobians.ipynb>
+   4. Composite States <./tutorial/composite.ipynb>

docs/source/tutorial/lie_groups.ipynb

@@ -205,9 +205,9 @@
     "        self.Q = input_covariance_matrix\n",
     "\n",
     "    def evaluate(self, x:SE2State, u:nav.lib.VectorInput, dt:float):\n",
-    "        u = np.array([u.value[0], u.value[1], 0])\n",
+    "        vel = np.array([u.value[0], u.value[1], 0])\n",
     "        x_next = x.copy()\n",
-    "        x_next.value = x.value @ expm(wedge_se2(u * dt))\n",
+    "        x_next.value = x.value @ expm(wedge_se2(vel * dt))\n",
     "        return x_next\n",
     "    \n",
     "    def input_covariance(self, x:SE2State, u:nav.lib.VectorInput, dt:float):\n",

navlie/composite.py

@@ -527,15 +527,29 @@ def covariance(
 class CompositeMeasurementModel(MeasurementModel):
     """
     Wrapper for a standard measurement model that assigns the model to a specific
-    substate (referenced by `state_id`) inside a CompositeState.
+    substate (referenced by `state_id`) inside a CompositeState. This class
+    will take care of extracting the relevant substate from the CompositeState,
+    and then applying the measurement model to it. It will also take care of
+    padding the Jacobian with zeros appropriately to match the degrees of freedom
+    of the larger CompositeState.
     """
 
     def __init__(self, model: MeasurementModel, state_id):
+        """
+        Parameters
+        ----------
+        model : MeasurementModel
+            Standard measurement model, which is appropriate only for a single
+            substate in the CompositeState.
+        state_id : Any
+            The unique ID of the relevant substate in the CompositeState, to 
+            assign the measurement model to.
+        """
         self.model = model
         self.state_id = state_id
 
     def __repr__(self):
-        return f"{self.model}(of substate {self.state_id})"
+        return f"{self.model} (of substate '{self.state_id}')"
 
     def evaluate(self, x: CompositeState) -> np.ndarray:
         return self.model.evaluate(x.get_state_by_id(self.state_id))
@@ -556,8 +570,8 @@ def covariance(self, x: CompositeState) -> np.ndarray:
 class CompositeMeasurement(Measurement):
     def __init__(self, y: Measurement, state_id: Any):
         """
-        Converts a standard Measurement into a CompositeMeasurement, which
-        replaces the model with a CompositeMeasurementModel.
+        Converts a standard ``Measurement`` into a CompositeMeasurement, which
+        simply replaces the model with a CompositeMeasurementModel.
 
         Parameters
         ----------

navlie/filters.py

@@ -449,8 +449,8 @@ def correct(
 def generate_sigmapoints(
     dof: int, method: str
 ) -> Tuple[np.ndarray, np.ndarray]:
-    """Generates unit sigma points from three available
-    methods.
+    """
+    Generates unit sigma points from three available methods.
 
     Parameters
     ----------
@@ -466,7 +466,8 @@ def generate_sigmapoints(
     Returns
     -------
     Tuple[np.ndarray, np.ndarray]
-        returns the unit sigma points and the weights
+        returns the unit sigma points and the weights, respectively. For the 
+        sigmapoints, each *column* will represent a sigma point.
     """
     if method == "unscented":
         kappa = 2