diff --git a/README.rst b/README.rst index cddfcf21..25e60e46 100644 --- a/README.rst +++ b/README.rst @@ -30,6 +30,10 @@ The core idea behind this project is to abstract-away the state definition such By implementing a few classes, the user can model almost any problem. Documentation can be found at https://decargroup.github.io/navlie +Disclaimer +---------- +While navlie is starting to get some maturity, its still definitely not perfect. The code is probably still simple enough that you can check out the source directly to get a better understanding. If (when) you find a bug, please feel free to open an Github issue. Contributions/thoughts are welcome, and if anything regarding documentation is still grossly unclear, just let us know :). + Setup ----- @@ -264,12 +268,14 @@ Finally, this repo has the following state estimation algorithms implemented: Contributing ------------ -If you wish to make some changes, create a branch, make your changes, and then make a pull request. Here are some conventions that should be followed: +If you wish to make some changes, fork this repo, make your changes, and then make a pull request. Here are some conventions that should be followed: + +- Code style should follow the PEP8 style guide. https://peps.python.org/pep-0008. We recommend using `black --line-length 80 .` to format the code. +- Everything should be type hinted as much as possible. Essentially, in the VS Code dark theme, you should not have any white text. -- Code style should follow the PEP8 style guide. https://peps.python.org/pep-0008 -- Everything should be type hinted as much as possible. Essentially, in the VS Code dark theme, you should not have any white text anywhere. +The goal of this project is to write general algorithms that work for any implementation of the abstract `State`, `ProcessModel` and `MeasurementModel`. As such, please give thought to how this could be done to any algorithm you implement. As a rule of thumb, code outside of the `navlie/lib` folder should not depend on any of the classes in `navlie/lib`, although sometimes this rule is broken. -The goal of this project is to write general algorithms that work for any implementation of the abstract `State`, `ProcessModel` and `MeasurementModel`. As such, please give thought to how this could be done to any algorithm you implement. +If you want to discuss anything regarding this repo, feel free to email `charles.c.cossette@gmail.com`. Contributing to the Documentation @@ -286,7 +292,7 @@ After this is done, change to the `./docs/` directory and run make html -and the documentation will be updated, and viewable by opening the ``docs/index.html`` file in your browser. In terms of actually writing documentation, we use the numpy format, which can be seen in some of the existing docstrings in the code, and used as a template. +after which the documentation will be updated, and viewable by opening the ``docs/index.html`` file in your browser. In terms of actually writing documentation, we use the numpy format, which can be seen in some of the existing docstrings in the code, and used as a template. Alternatively and prefereably, install the `autoDocstring extension for VSCode. ` and change the docstring format in the settings to `numpy`. diff --git a/docs/source/tutorial/jacobians.ipynb b/docs/source/tutorial/jacobians.ipynb index 097aaab6..ce634985 100644 --- a/docs/source/tutorial/jacobians.ipynb +++ b/docs/source/tutorial/jacobians.ipynb @@ -7,9 +7,15 @@ "source": [ "# Jacobians in navlie\n", "\n", - "As you may know, many state estimation algorithm require access to process model and measurement model Jacobians, with respect to the state and sometimes other inputs. For states belonging to Lie groups, algorithms will require _Lie Jacobians_, which differ from traditional derivates as they conform to the constraints of the group. For more backround on Lie Jacobians/derivatives, see [this paper](https://arxiv.org/pdf/1812.01537.pdf). \n", + "As you may know, many state estimation algorithm require access to process model and measurement model Jacobians, with respect to the state and sometimes other inputs. For states belonging to Lie groups, algorithms will require _Lie Jacobians_, which differ from traditional derivatives as they conform to the constraints of the group. The abstraction provided by the $\\oplus$ and $\\ominus$ operators (implemented with `State.plus` and `State.minus` respectively) allow for a generic definition of a derivative:\n", "\n", - "The good news is that navlie computes Lie Jacobians for you by default using finite difference. However, finite difference can have some drawbacks, such as being computationally expensive and less accurate than analytic derivatives. In this notebook, we will show you how to use analytic derivatives in navlie, which offer the maximum accuracy and speed.\n", + "$$\n", + "\\left.\\frac{D f(\\mathcal{X})}{D \\mathcal{X}}\\right|_{\\bar{\\mathcal{X}} }\\triangleq \\left.\\frac{\\partial f(\\bar{\\mathcal{X}} \\oplus \\delta \\mathbf{x}) \\ominus f(\\bar{\\mathcal{X}})}{\\partial \\delta \\mathbf{x}}\\right|_{\\delta \\mathbf{x} = \\mathbf{0}},\n", + "$$\n", + "\n", + "which can be shown to fall back to a traditional derivatives when $\\oplus$ and $\\ominus$ are defined to be regular addition/subtraction. This derivative definition is used universally throughout navlie, and roughly follows what is done in the [Micro Lie Theory paper](https://arxiv.org/pdf/1812.01537.pdf). In that reference, seperate definitions are given for \"left\" and \"right\" derivatives, whereas we have aggregated them into a single definition, with left and right derivatives naturally arising from the choice of $\\oplus$ and $\\ominus$ operators.\n", + "\n", + "If you dont want to worry about this, the good news is that navlie computes Lie Jacobians for you by default using finite difference. However, finite difference can have some drawbacks, such as being computationally expensive and less accurate than analytic derivatives. In this notebook, we will show you how to use analytic derivatives in navlie, which offer the maximum accuracy and speed.\n", "\n", "## Jacobians - Traditional Approach\n", "Recall the traditional approach to the previous example. We had defined the state to be $\\mathbf{x} = [\\theta, x, y]^T$ and the process model to be\n", @@ -48,7 +54,7 @@ " def __init__(self, input_covariance):\n", " self.Q = input_covariance\n", "\n", - " def evaluate(self, x: VectorState, u: nav.VectorInput, dt: float) -> VectorState:\n", + " def evaluate(self, x: VectorState, u: VectorInput, dt: float) -> VectorState:\n", " x_next = x.copy()\n", " x_next.value[0] += u.value[0] * dt\n", " x_next.value[1] += u.value[1] * dt * np.cos(x.value[0])\n", @@ -252,7 +258,7 @@ } ], "source": [ - "from navlie.lib import SE2State\n", + "from navlie.lib import SE2State, VectorInput\n", "from scipy.linalg import expm\n", "\n", "\n", @@ -276,15 +282,15 @@ " def __init__(self, input_covariance_matrix):\n", " self.Q = input_covariance_matrix\n", "\n", - " def evaluate(self, x:SE2State, u:nav.VectorInput, dt:float):\n", + " def evaluate(self, x:SE2State, u:VectorInput, dt:float):\n", " u = 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", " return x_next\n", - " def input_covariance(self, x:SE2State, u:nav.VectorInput, dt:float):\n", + " def input_covariance(self, x:SE2State, u:VectorInput, dt:float):\n", " return self.Q\n", "\n", - " def jacobian(self, x:SE2State, u:nav.VectorInput, dt:float):\n", + " def jacobian(self, x:SE2State, u:VectorInput, dt:float):\n", " u = np.array([u.value[0], u.value[1], 0])\n", " return adjoint_se2(expm(-wedge_se2(u * dt)))\n", "\n", @@ -306,7 +312,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "Note that when using Lie groups, our Jacobian no longer has dependence on the state itself. This can be a tangible advantage when the state estimate has high uncertainty, where using a traditional approach when can result in excessive linearization errors when the state estimate is far from the true value." + "Note that when using Lie groups, our Jacobian no longer has dependence on the state itself. This can be a tangible advantage when the state estimate has high uncertainty, where using a traditional approach can result in excessive linearization errors when the state estimate is far from the true value." ] } ], diff --git a/docs/source/tutorial/lie_groups.ipynb b/docs/source/tutorial/lie_groups.ipynb index aa7a017b..f78c876b 100644 --- a/docs/source/tutorial/lie_groups.ipynb +++ b/docs/source/tutorial/lie_groups.ipynb @@ -204,13 +204,13 @@ " def __init__(self, input_covariance_matrix):\n", " self.Q = input_covariance_matrix\n", "\n", - " def evaluate(self, x:SE2State, u:nav.VectorInput, dt:float):\n", + " def evaluate(self, x:SE2State, u:nav.lib.VectorInput, dt:float):\n", " u = 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", " return x_next\n", " \n", - " def input_covariance(self, x:SE2State, u:nav.VectorInput, dt:float):\n", + " def input_covariance(self, x:SE2State, u:nav.lib.VectorInput, dt:float):\n", " return self.Q\n", "\n", "Q = np.eye(2) * 0.1**2\n", diff --git a/docs/source/tutorial/traditional.ipynb b/docs/source/tutorial/traditional.ipynb index 6f52c86b..f18bd608 100644 --- a/docs/source/tutorial/traditional.ipynb +++ b/docs/source/tutorial/traditional.ipynb @@ -86,7 +86,7 @@ " def __init__(self, input_covariance):\n", " self.Q = input_covariance\n", "\n", - " def evaluate(self, x: VectorState, u: nav.VectorInput, dt: float) -> VectorState:\n", + " def evaluate(self, x: VectorState, u: VectorInput, dt: float) -> VectorState:\n", " x_next = x.copy()\n", " x_next.value[0] += u.value[0] * dt\n", " x_next.value[1] += u.value[1] * dt * np.cos(x.value[0])\n", diff --git a/examples/ex_random_walk.py b/examples/ex_random_walk.py index ae14d56e..ea259ea0 100644 --- a/examples/ex_random_walk.py +++ b/examples/ex_random_walk.py @@ -56,7 +56,7 @@ def main(): # The data generator will add noise on top of the already random signal if # ``input_covariance`` is not zero. So here we remove this. - u: nav.VectorInput = input_data[k] + u: nav.lib.VectorInput = input_data[k] u.value = np.zeros(u.value.shape) # Zero-out the input # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/examples/tutorial.py b/examples/tutorial.py index b1a539c5..cb3557c9 100644 --- a/examples/tutorial.py +++ b/examples/tutorial.py @@ -9,7 +9,7 @@ # Define the process model class BicycleModel(nav.ProcessModel): - def evaluate(self, x: VectorState, u: nav.VectorInput, dt: float) -> VectorState: + def evaluate(self, x: VectorState, u: nav.lib.VectorInput, dt: float) -> VectorState: x_next = x.copy() x_next.value[0] += u.value[0] * dt * np.cos(x.value[2]) x_next.value[1] += u.value[0] * dt * np.sin(x.value[2]) diff --git a/navlie/__init__.py b/navlie/__init__.py index f5474ed9..9cd28a55 100644 --- a/navlie/__init__.py +++ b/navlie/__init__.py @@ -40,4 +40,5 @@ jacobian, ) -from .lib import VectorInput # for backwards compatibility + +from .lib.states import StampedValue # for backwards compatibility diff --git a/navlie/types.py b/navlie/types.py index 8e78c0a5..90180fc7 100644 --- a/navlie/types.py +++ b/navlie/types.py @@ -3,7 +3,7 @@ """ import numpy as np -from typing import List, Any +from typing import List, Any, Tuple from abc import ABC, abstractmethod @@ -47,15 +47,37 @@ def copy(self) -> "Input": class State(ABC): """ - An abstract state :math:`\mathbf{x}` is an object containing the following attributes: + An abstract state :math:`\mathcal{X}` is an object containing the following attributes: - - a value of some sort; - - a certain number of degrees of freedom (dof); - - an update rule that modified the state value given an update vector - ``dx`` containing ``dof`` elements. + - a value of some sort; + - a certain number of degrees of freedom (dof); + - ``plus`` and ``minus`` methods that generalize addition and subtracting to + to this object. Optionally, it is often useful to assign a timestamp (``stamp``) and a label (``state_id``) to differentiate state instances from others. + + When implementing a new state type, you should inherit from this class as + shown in the tutorial. + + .. note:: + The ``plus`` and ``minus`` must correspond to each other, in the sense + that the following must hold: + + .. math:: + + \delta \mathbf{x} = (\mathcal{X} \oplus \delta \mathbf{x}) \ominus \mathcal{X} + + for any state :math:`\mathcal{X}` and any perturbation :math:`\delta \mathbf{x}`. + In practice this can be tested with something along the lines of: + + .. code-block:: python + + x = State(...) # some state object + dx = np.random.randn(x.dof) + dx_test = x.plus(dx).minus(x) + assert np.allclose(dx, dx_test) + """ __slots__ = ["value", "dof", "stamp", "state_id"] @@ -95,14 +117,20 @@ def copy(self) -> "State": def plus_jacobian(self, dx: np.ndarray) -> np.ndarray: """ - Jacobian of the ``plus`` operator. For Lie groups, this is known as the - *group Jacobian*. + Jacobian of the ``plus`` operator. That is, using Lie derivative notation, + + .. math:: + + \mathbf{J} = \\frac{D (\mathcal{X} \oplus \delta \mathbf{x})}{D \delta \mathbf{x}} + + + For Lie groups, this is known as the *group Jacobian*. """ return self.plus_jacobian_fd(dx) def plus_jacobian_fd(self, dx, step_size=1e-8) -> np.ndarray: """ - Calculates the model jacobian with finite difference. + Calculates the plus jacobian with finite difference. """ dx_bar = dx jac_fd = np.zeros((self.dof, self.dof)) @@ -117,11 +145,13 @@ def plus_jacobian_fd(self, dx, step_size=1e-8) -> np.ndarray: def minus_jacobian(self, x: "State") -> np.ndarray: """ - Jacobian of the ``minus`` operator with respect to self. That is, if - - y = x1.minus(x2) + Jacobian of the ``minus`` operator with respect to self. + + .. math:: - then this is the Jacobian of ``y`` with respect to ``x1``. + \mathbf{J} = \\frac{D (\mathcal{Y} \ominus \mathcal{X})}{D \mathcal{Y}} + + That is, if ``dx = y.minus(x)`` then this is the Jacobian of ``dx`` with respect to ``y``. For Lie groups, this is the inverse of the *group Jacobian* evaluated at ``dx = x1.minus(x2)``. """ @@ -129,7 +159,7 @@ def minus_jacobian(self, x: "State") -> np.ndarray: def minus_jacobian_fd(self, x: "State", step_size=1e-8) -> np.ndarray: """ - Calculates the model jacobian with finite difference. + Calculates the minus jacobian with finite difference. """ x_bar = x jac_fd = np.zeros((self.dof, self.dof)) @@ -159,16 +189,21 @@ class MeasurementModel(ABC): of the form .. math:: - \mathbf{y} = \mathbf{g}(\mathbf{x}) + \mathbf{v} - where :math:`\mathbf{v} \sim \mathcal{N}(\mathbf{0}, \mathbf{R})`. + \mathbf{y} = \mathbf{g}(\mathcal{X}) + \mathbf{v} + + where :math:`\mathbf{v} \sim \mathcal{N}(\mathbf{0}, \mathbf{R})`. To + implement a measurement model, you must inherit from this class and + implement the ``evaluate`` method, which *must* return a numpy array. You + must also specify covariance matrix :math:`\mathbf{R}` by implementing the + ``covariance`` method. """ @abstractmethod def evaluate(self, x: State) -> np.ndarray: """ - Evaluates the measurement model :math:`\mathbf{g}(\mathbf{x})`. + Evaluates the measurement model :math:`\mathbf{g}(\mathcal{X})`. """ pass @@ -181,12 +216,15 @@ def covariance(self, x: State) -> np.ndarray: def jacobian(self, x: State) -> np.ndarray: """ - Evaluates the measurement model Jacobian - :math:`\mathbf{G} = \partial \mathbf{g}(\mathbf{x})/ \partial \mathbf{x}`. + Evaluates the measurement model Jacobian with respect to the state. + + .. math:: + + \mathbf{G} = \\frac{D \mathbf{g}(\mathcal{X})}{D \mathcal{X}} """ return self.jacobian_fd(x) - def evaluate_with_jacobian(self, x: State) -> (np.ndarray, np.ndarray): + def evaluate_with_jacobian(self, x: State) -> Tuple[np.ndarray, np.ndarray]: """ Evaluates the measurement model and simultaneously returns the Jacobian as its second output argument. This is useful to override for @@ -225,17 +263,50 @@ class ProcessModel(ABC): Abstract process model base class for process models of the form .. math:: - \mathbf{x}_k = \mathbf{f}(\mathbf{x}_{k-1}, \mathbf{u}, \Delta t) + \mathbf{w}_{k} + \mathcal{X}_k = f(\mathcal{X}_{k-1}, \mathbf{u}_{k-1}, \Delta t) \oplus \mathbf{w}_{k} + + where :math:`\mathbf{u}_{k-1}` is the input, :math:`\Delta t` is the time period + between the two states, and :math:`\mathbf{w}_{k} \sim + \mathcal{N}(\mathbf{0}, \mathbf{Q}_k)` is additive Gaussian noise. + + To define a process model, you must inherit from this class and implement + the ``evaluate`` method. You must also specify covariance information in one + of either two ways. + + **1. Specifying the covariance matrix directly:** + + The first way is to specify the :math:`\mathbf{Q}_k` covariance matrix + directly by overriding the ``covariance`` method. This covariance matrix + represents the distribution of process model errors directly. + + **2. Specifing the covariance of additive noise on the input:** + The second way is to specify the covariance of noise that is additive to + the input. That is, if the process model is of the form + + .. math:: + + \mathcal{X}_k = f(\mathcal{X}_{k-1}, \mathbf{u}_{k-1} + + \mathbf{w}^u_{k-1}, \Delta t) + + where :math:`\mathbf{w}^u_{k-1} \sim \mathcal{N}(\mathbf{0}, + \mathbf{Q}^u_{k-1})`. In this case, you should override the + ``input_covariance`` method, at which point the covariance of the process + model error is approximated using a linearization procedure, + + .. math:: + + \mathbf{Q}_k = \mathbf{L} \mathbf{Q}^u_{k-1} \mathbf{L}^T - where :math:`\mathbf{u}` is the input, :math:`\Delta t` is the time - period between the two states, and :math:`\mathbf{w}_{k} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k)` - is additive Gaussian noise. + where :math:`\mathbf{L} = D \mathbf{f}(\mathcal{X}_{k-1}, \mathbf{u}_{k-1}, dt) / + D \mathbf{u}_{k-1}` is the *input jacobian*. This is calculated using finite + difference by default, but can be overridden by implementing the + ``input_jacobian`` method. """ @abstractmethod def evaluate(self, x: State, u: Input, dt: float) -> State: """ - Implementation of :math:`\mathbf{f}(\mathbf{x}_{k-1}, \mathbf{u}, \Delta t)`. + Implementation of :math:`{f}(\mathcal{X}_{k-1}, \mathbf{u}, \Delta t)`. Parameters ---------- @@ -256,7 +327,7 @@ def evaluate(self, x: State, u: Input, dt: float) -> State: def covariance(self, x: State, u: Input, dt: float) -> np.ndarray: """ - Covariance matrix math:`\mathbf{Q}_k` of the additive Gaussian + Covariance matrix :math:`\mathbf{Q}_k` of the additive Gaussian noise :math:`\mathbf{w}_{k} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k)`. If this method is not overridden, the covariance of the process model error is approximated from the input covariance using a linearization @@ -285,8 +356,7 @@ def jacobian(self, x: State, u: Input, dt: float) -> np.ndarray: Implementation of the process model Jacobian with respect to the state. .. math:: - \mathbf{F} = \partial \mathbf{f}(\mathbf{x}_{k-1}, \mathbf{u}, \Delta t) - / \partial \mathbf{x}_{k-1} + \mathbf{F} = \\frac{D {f}(\mathcal{X}_{k-1}, \mathbf{u}, \Delta t)}{D \mathcal{X}_{k-1}} Parameters @@ -359,7 +429,7 @@ def sqrt_information(self, x: State, u: Input, dt: float) -> np.ndarray: def input_covariance(self, x: State, u: Input, dt: float) -> np.ndarray: """ - Covariance matrix of additive noise *on the input*. + Covariance matrix of additive noise *on the input*. Parameters ---------- @@ -383,8 +453,9 @@ def input_covariance(self, x: State, u: Input, dt: float) -> np.ndarray: class Measurement: """ - A data container containing a generic measurement's value, timestamp, - and corresponding model. + A simple data container containing a generic measurement's value, timestamp, + and corresponding model stored as a ``MeasurementModel`` object. This + container can be used as-is without inheritance. """ __slots__ = ["value", "stamp", "model", "state_id"] @@ -435,18 +506,35 @@ def minus(self, y_check: np.ndarray) -> np.ndarray: By default, assumes that the measurement is a column vector, and thus, the ``minus`` operator is simply vector subtraction. """ - return self.value.reshape((-1, 1)) - y_check.reshape((-1, 1)) class StateWithCovariance: """ - A data container containing a State object and a covariance array. + A data container containing a ``State`` object and a covariance array. + This class can be used as-is without inheritance. """ __slots__ = ["state", "covariance"] def __init__(self, state: State, covariance: np.ndarray): + """ + + Parameters + ---------- + state : State + A state object, usually representing the mean of a distribution. + covariance : np.ndarray + A square, symmetric covariance matrix associated with the state. + + Raises + ------ + ValueError + If the covariance matrix is not square. + ValueError + If the covariance matrix does not correspond with the state degrees + of freedom. + """ if covariance.shape[0] != covariance.shape[1]: raise ValueError("covariance must be an n x n array.") diff --git a/tests/unit/test_filter.py b/tests/unit/test_filter.py index 6cb44b34..70f5a831 100644 --- a/tests/unit/test_filter.py +++ b/tests/unit/test_filter.py @@ -14,7 +14,7 @@ def test_iterated_ekf(): range_model = nav.lib.RangePointToAnchor([0, 4], R) process_model = nav.lib.SingleIntegrator(Q) y = nav.generate_measurement(x, range_model) - u = nav.VectorInput([1, 2], stamp=0.0) + u = nav.lib.VectorInput([1, 2], stamp=0.0) x = nav.StateWithCovariance(x, P) kf = nav.IteratedKalmanFilter(process_model) @@ -34,7 +34,7 @@ def test_iterated_ekf_no_line_search(): range_model = nav.lib.RangePointToAnchor([0, 4], R) process_model = nav.lib.SingleIntegrator(Q) y = nav.generate_measurement(x, range_model) - u = nav.VectorInput([1, 2], stamp=0.0) + u = nav.lib.VectorInput([1, 2], stamp=0.0) x = nav.StateWithCovariance(x, P) kf = nav.IteratedKalmanFilter(process_model, line_search=False) @@ -56,7 +56,7 @@ def test_iterated_ekf_equivalence(): range_model = nav.lib.RangePointToAnchor([0, 4], R) process_model = nav.lib.SingleIntegrator(Q) y = nav.generate_measurement(x, range_model) - u = nav.VectorInput([1, 2], stamp=0.0) + u = nav.lib.VectorInput([1, 2], stamp=0.0) x = nav.StateWithCovariance(x, P) kf = nav.IteratedKalmanFilter(process_model, max_iters=1, line_search=False)