Merge pull request #100 from decargroup/add_tutorial

More docs on core types

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. <https://marketplace.visualstudio.com/items?itemName=njpwerner.autodocstring>` and change the docstring format in the settings to `numpy`.
 

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."
    ]
   }
  ],

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",

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",

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
 
         # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

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

navlie/__init__.py

@@ -40,4 +40,5 @@
     jacobian,
 )
 
-from .lib import VectorInput  # for backwards compatibility
+
+from .lib.states import StampedValue # for backwards compatibility