This project is a collection of scripts that can be used to evaluate the performance of outlier rejection techniques in GTSAM. The first script of importance is unicycle_sim.py. This script outputs .npz files that represent a single run of the simulation. These .npz files can be read in by other scripts that can then use GTSAM (or something else if you want) to optimize for the estimated trajectory of the unicycle. So far, I have one type of graph, which is implemented in basic_graph.py. Within this graph, you can apply different robust estimators, including Huber, Tukey, etc.
This script is designed to simulate a unicycle model that generates truth. It also has some generic functions that can be used to add different types of errors to either the dynamics or the measurements. Therefore, to change what type of noise is added to either the dynamics or measures, you only have to change the lines that say pn_func = for the process noise and mn_func = for the measurement noise. By using different functions, you can create measurements with different percentages of outliers, or with different types of noise, or time correlated noise, or....
This file is setup right now to run the unicycle simulation for a large number of times, storing the results in different run_*.npz files. The directory they are in should hopefully be named so it is obvious what scenario setup the runs are. For each run, it randomly chooses the location of landmarks. The "commands" given the unicycle will be the same for each run, but due to process noise the path will vary from run to run. The measurements are a function of the path and the landmarks (and the measurement noise).
The basic estimator is a gtsam graph (we don't use iSAM2 or such. The examples are not big enough here) which reads in one of the .npz files with the measurements. We assume we know the base covariances (i.e. they are hard-coded / not read in). We then perform the optimization. The current code is created so that you can quickly do the optimization for each run in a directory, and apply different modifications to the graph noise models, but the structure of the graph itself is the same. Note that this file creates a custom factor for a landmark ranging measurement where the landmark is at a known location. This is not part of gtsam by default, but seemed better than adding variable nodes for the landmark locations, and then constraining them to the correct location.