Repository files navigation

An on-manifold state estimation library for robotics.

The core idea behind this project is to abstract-away the state definition such that a single estimator implementation can operate on a variety of state manifolds, such as the usual vector space, and any Lie group. At the moment, algorithms and features of this package include:

• Extended Kalman Filter
• Iterated Extended Kalman Filter
• Sigmapoint Kalman Filters (Unscented, Spherical Cubature, Gauss-Hermite)
• Interacting Multiple Model Filter
• Batch MAP Estimation
• A large collection of common process and measurement models
• Out-of-the-box on-manifold numerical jacobian using finite differencing
• Various utils for plotting, error, and consistency evaluation
• Monte Carlo experiment executor with result aggregation
• A preintegration module for linear, wheel odometry, and IMU process models

By implementing a few classes, the user can model almost any problem. Documentation can be found athttps://decargroup.github.io/navlie

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 :).

Clone this repo, change to its directory, and execute

pip install -e.

This command should automatically install all dependencies, including our package pymlg (found athttps://github.com/decargroup/pymlg) for back-end Lie group mathematical operations.

Some starting examples running EKFs can be found in the examples/ folder. Simply run these as python3 scripts

To run integration tests,

pytest tests

or, to run tests on a specific test file,

pytest -ra tests/integration/filename.py

The algorithms and models in this repo are centered around the abstract State class. An instance of State is an object containing, at a minimum, the following attributes:

• value: a value of some sort;
• dof: the degrees of freedom associated with the state.

It will also contain the following mandatory methods that must be implemented by the user.

• plus(): A generic "addition" operation given a dx vector with as many elements as the dof of this state.
• minus(): A generic "subtraction" operation given another State object of the same type, which returns a numpy array of error values.
• copy(): A method that returns a new object of the same type, and with the same attibute values.

Optionally, it is often useful to assign a timestamp (stamp) and a label (state_id) to differentiate state instances from others. The snippet below shows how to define a simple vector-space state:

fromnavlie.typesimportStateimportnumpyasnpclassVectorState(State):"""    A standard vector-based state, with value represented by a 1D numpy array.    """def__init__(self,value:np.ndarray,stamp:float=None,state_id=None):super(VectorState,self).__init__(value=value,dof=value.size,stamp=stamp,state_id=state_id,        )defplus(self,dx:np.ndarray):new=self.copy()new.value=new.value+dxreturnnewdefminus(self,x:"VectorState")->np.ndarray:returnself.value-x.valuedefcopy(self)->"VectorState":returnVectorState(self.value.copy(),self.stamp,self.state_id)

As another more complicated example, a state object belonging to the SE(3) Lie group can be implemented as

fromnavlie.typesimportStatefrompymlgimportSE3importnumpyasnpclassSE3State(State):def__init__(self,value:np.ndarray,stamp:float=None,state_id=None):super(SE3State,self).__init__(value=value,dof=6,stamp=stamp,state_id=state_id,        )defplus(self,dx:np.ndarray):new=self.copy()new.value=new.value @SE3.Exp(dx)returnnewdefminus(self,x:"SE3State")->np.ndarray:returnSE3.Log(SE3.inverse(x.value) @self.value)defcopy(self)->"SE3State":returnSE3State(self.value.copy(),self.stamp,self.state_id)

There are a few more core types in this package. The main ones are the ProcessModel and MeasurementModel classes. Both of these are abstract classes requiring the user to implement

• an evaluate() method,
• a covariance() method,
• and optionally a jacobian() method.

For example, a simple "single integrator" (velocity input) model can be implemented as follows:

classSingleIntegrator(ProcessModel):"""    The single-integrator process model is a process model of the form        x_k = x_{k-1} + dt * u_{k-1}    """def__init__(self,Q:np.ndarray):self._Q=Qdefevaluate(self,x:VectorState,u:VectorInput,dt:float)->np.ndarray:"""        Returns a state with an updated value according to a process model.        """x.value=x.value+dt*u.valuereturnxdefjacobian(self,x:VectorState,u:VectorInput,dt:float)->np.ndarray:"""        (optional) Jacobian of the process model with respect to the state.        """returnnp.identity(x.dof)defcovariance(self,x:VectorState,u:VectorInput,dt:float)->np.ndarray:"""        Returns the covariance of the process model errors.        """returndt**2*self._Q

Similarly, a single distance-to-landmark measurement model can be implemented as

classRangePointToAnchor(MeasurementModel):"""    Range measurement from a point state to an anchor (which is also another    point).    """def__init__(self,anchor_position:List[float],R:float):self._r_cw_a=np.array(anchor_position)self._R=np.array(R)defevaluate(self,x:VectorState)->np.ndarray:r_zw_a=x.valuey=np.linalg.norm(self._r_cw_a-r_zw_a)returnydefjacobian(self,x:VectorState)->np.ndarray:r_zw_a=x.valuer_zc_a=r_zw_a-self._r_cw_ay=np.linalg.norm(r_zc_a)returnr_zc_a.reshape((1,-1))/ydefcovariance(self,x:VectorState)->np.ndarray:returnself._R

In fact, for both ProcessModel and MeasurementModel, subclasses will inherit a finite-difference numerical differentiation method jacobian_fd(), which also serves as the default implementation if jacobian is not overriden. Nevertheless, it allows for an easy way to check your jacobian() implementation! (evaluate() method must be implemented for this to work, see some of the files in tests/ for an example of this.)

Many state, process, and measurement models are already written and part of the built-in library and, as an example, can be accessed with

fromnavlie.lib.statesimportVectorState,SE3Statefromnavlie.lib.modelsimportRangePoseToAnchor,Altitude

The following state types are currently part of the lib:

• VectorState
• SO2State
• SO3State
• SE2State
• SE3State
• SE23State
• 'SL3State'
• IMUState (contains IMU biases as part of the state)
• CompositeState (for holding many sub-states as a single state)

The following process models are currently part of the lib:

• SingleIntegrator
• BodyFrameVelocity
• RelativeBodyFrameVelocity
• CompositeProcessModel
• IMUKinematics

The following measurement models are currently part of the lib:

• RangePointToAnchor
• RangePoseToAnchor
• RangePoseToPose
• RangeRelativePose
• GlobalPosition
• Altitude
• Gravitometer
• and many more

Finally, this repo has the following state estimation algorithms implemented:

• ExtendedKalmanFilter
• IteratedKalmanFilter
• UnscentedKalmanFilter
• InteractingModelFilter
• and more

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.

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.

If you want to discuss anything regarding this repo, feel free to email charles.c.cossette@gmail.com.

You must first install the dependencies for the documentation. This can be done by running

pip install -r docs/requirements.txt

After this is done, change to the ./docs/ directory and run

make html

after which the documentation will be updated, and viewable by opening thedocs/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.