Skip to content

Adding a new Estimator

One might want to introduce a new type of state estimation or simply adapt one of the existing estimators to a new platform. In order to do this, follow the instructions below.

We divide the procedure to create a new estimator into two sets of tasks:

  1. CRS-side: this is where the actual estimator logic goes as well as (sensor) models
  2. ROS4CRS-side: this is where the data management goes (parsing of sensor messages etc.)

:warning: This guide provides an overview on where you might need to add or modify code for your application. For details orient yourself on the already existing implementations. Feel free to extend/correct this guide.

1. Estimator Logic (CRS-side)

All the estimator-specific logic and computations go to src/crs/estimators . In there

  • Either use one of the existing folders and place the implementation of the estimator in a new header file inside the include folder.
  • Or create a new folder.

In both cases orient yourself at the existing estimator implementations.

In case you want to use novel sensors, add a respective implementation in src/crs/sensor_models.

2. ROS integration (ROS4CRS-side)

Now we need to make sure that your estimator can actually be executed in ROS. To this end we need to create a ROS package for your estimator and make sure that it is executed with the right arguments and correct input-output behaviour.

src/ros4crs/ros_estimators/...

  1. You might need to extend the dataconverter utility, depending on what sensors your estimator uses and in what form that data needs to be. Do this by adding the required function(s) parseSomeSensorData() to src/ros4crs/ros_estimators/include/ros_estimators/data_converter.h and src/ros4crs/ros_estimators/src/data_converter.cpp .
  2. Create a class for your estimator in src/ros4crs/ros_estimators/... by adding header and source files in .../include/ros_estimators/<YOUR_ESTIMATOR_FOLDER> and .../src/<YOUR_ESTIMATOR_FOLDER> respectively. This class should contain callback functions to handle new control actions as well as new sensor measurements and functions to publish the current state estimate.
  3. Extend the estimator component registry to include your estimator.
    1. In src/ros4crs/ros_estimators/include/ros_estimators/component_registry/ add a section to the file resolve_estimator.h for your estimator. In case you added a new sensor model or visualizer , do the same in resolve_sensor_models.h or in resolve_visualizers.h respectively.
    2. In src/ros4crs/ros_estimators/src/component_registry/ add a folder for your model in which you add the files resolve_your_estimator.cpp, resolve_sensor_models.cpp and resolve_visualizers.cpp. These files implement template functions to load (instantiate) your estimator, sensor models and visualizers (i.e. resolve_rocket_discrete_ekf.cpp implements loadCRSEstimator<RocketDiscreteEKF>).
    3. In src/ros4crs/ros_estimators/src/component registry/component_registry.cpp add necessary includes and load your estimator using the template functions implemented previously. These actions are conditional and depend on wether or not the necessary CRS source code is available (i.e. has been compiled).
  4. Create a default configuration file for your estimator in src/ros4crs/ros_estimators/config/your_estimator.yaml .
  5. Create a default launch file for your estimator in src/ros4crs/ros_estimators/launch/your_platform_default.launch .
  6. Don't forget to specify dependencies for your estimator (e.g. necessary models etc.) in src/ros4crs/ros_estimators/package.xml .
  7. Finally, if your estimator depends on a specific model, add CMake instructions to only build your estimator if the dependencies are met. To this end, edit the file src/ros4crs/ros_estimators/CMakeLists.txt .

Launch files, simulation etc...

This is not technically part of this guide but be aware that if you added new sensors you might need to adapt the simulation code to publish measurements for your sensors. Also you might need to add correct topics/remaps to your launch files. Finally, don't forget to actually start the estimator in the main launch file ;)

Experiments: Custom Estimator Config

It is possible to create per-experiment configurations for the used estimator (i.e. to experiment with different noise etc.).

In order to do this, copy the file src/ros4crs/ros_estimators/config/your_estimator.yaml to experiments/your_experiment/your_estimator.yaml .

Example: Rocket-EKF

As an overview over all files involved when adding a new estimator, consider the below git diff. The diff shows changes from before and after adding an EKF for the EmborockETH platform. 

 experiments/rocket_pid/rocket_estimator.yaml
 experiments/rocket_pid/rocket_simulator.yaml
 src/crs/estimators/kalman_estimator/include/kalman_estimator/rocket_discrete_ekf.h
 src/ros4crs/crs_launch/launch/sim_rocket.launch
 src/ros4crs/ros_estimators/CMakeLists.txt
 src/ros4crs/ros_estimators/config/rocket_estimator.yaml
 src/ros4crs/ros_estimators/include/ros_estimators/component_registry/resolve_estimator.h
 src/ros4crs/ros_estimators/include/ros_estimators/data_converter.h
 src/ros4crs/ros_estimators/include/ros_estimators/rocket_estimator/rocket_estimator.h
 src/ros4crs/ros_estimators/launch/rocket_default.launch
 src/ros4crs/ros_estimators/package.xml
 src/ros4crs/ros_estimators/src/component_registry/component_registry.cpp
 src/ros4crs/ros_estimators/src/component_registry/rocket_6_dof/resolve_rocket_discrete_ekf.cpp
 src/ros4crs/ros_estimators/src/component_registry/rocket_6_dof/resolve_sensor_models.cpp
 src/ros4crs/ros_estimators/src/component_registry/rocket_6_dof/resolve_visualizers.cpp
 src/ros4crs/ros_estimators/src/data_converter.cpp
 src/ros4crs/ros_estimators/src/rocket_estimator/rocket_estimator.cpp
 src/ros4crs/ros_simulator/launch/default_rocket.launch
 src/ros4crs/ros_simulator/src/ros_rocket_simulator.cpp