code refactor

.github/workflows/ci_ukf.yml

@@ -0,0 +1,33 @@
+name: CI_UKF
+
+on:
+  push:
+    branches: [ main ]
+  pull_request:
+    branches: [ main ]
+
+jobs:
+  build:
+
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v2
+
+      - name: Install Eigen
+        run: |
+          sudo apt-get update
+          sudo apt-get install libeigen3-dev
+  
+      - name: Configure and build
+        run: |
+          mkdir build
+          cd build
+          cmake ..
+          make
+  
+      - name: Run tests
+        run: |
+          cd build -DBUILD_TEST_UKF=ON
+          ctest

CMakeLists.txt

@@ -1,10 +1,13 @@
 # Author: Gabriele Nava
 # Date: Ott. 2023
-
 cmake_minimum_required(VERSION 3.5)
 
 set (CMAKE_CXX_STANDARD 11)
 
+# Compile example and test
+option(BUILD_EXAMPLE_UKF "Build an example of usage of the UKF code." TRUE)
+option(BUILD_TEST_UKF "Build a simple test to inspect library integrity." FALSE)
+
 # Use the variable PROJECT_NAME for changing the target name
 get_filename_component(ProjectId ${CMAKE_CURRENT_SOURCE_DIR} NAME)
 string(REPLACE " " "_" ProjectId ${ProjectId})
@@ -15,16 +18,14 @@ project(${PROJECT_NAME})
 find_package(Eigen3 REQUIRED)
 include_directories(${EIGEN3_INCLUDE_DIR})
 
-# Configure the library
-add_library(${PROJECT_NAME}_lib src/UnscentedKF.cpp src/SystemModel.cpp)
-target_include_directories(${PROJECT_NAME}_lib PUBLIC include)
-
-# Configure the executable
-set(SRCS src/testUKF.cpp)
-add_executable(${PROJECT_NAME} ${SRCS})
-
-target_include_directories(${PROJECT_NAME} PUBLIC include)
-target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_lib)
-
-# Copy the example data in the build folder
-configure_file("${CMAKE_SOURCE_DIR}/data/example_data.txt" "${CMAKE_BINARY_DIR}/example_data.txt" COPYONLY)
+# Configure and install the library
+add_subdirectory(lib)
+
+# Compile the UKF example and test
+if(BUILD_EXAMPLE_UKF)
+    add_subdirectory(example)
+endif()
+if(BUILD_TEST_UKF)
+    enable_testing()
+    add_subdirectory(test)
+endif()

README.md

@@ -1,24 +1,42 @@
 # Eigen_UKF
 
-A C++ implementation of the Unscented Kalman Filter (UKF) using Eigen. For the implementation I took inspiration from https://github.com/CoffeeKumazaki/kalman_filters.
+A C++ implementation of the Unscented Kalman Filter (UKF) using Eigen. The UKF uses a deterministic sampling technique known as the unscented transformation to calculate statistics around the mean. This technique does not require differentiability of the models. 
 
-The UKF uses a deterministic sampling technique known as the unscented transformation to calculate statistics around the mean. This technique does not require differentiability of the models.
+See also https://groups.seas.harvard.edu/courses/cs281/papers/unscented.pdf for more details.
 
-## Installation
+For the implementation I took inspiration from https://github.com/CoffeeKumazaki/kalman_filters.
+
+## Installation and usage
+
+**Tested only on Ubuntu 20.04 LTS**
 
 ### Dependencies
 
 - [Eigen3](https://eigen.tuxfamily.org/index.php?title=Main_Page) library.
 
-### Tested only on Ubuntu 20.04 LTS
+### Compilation
 
 Clone the repo on your PC. Then, enter the folder where you downloaded the repo, open a terminal and run:
 
 ```
 mkdir build && cd build
-cmake ../
-make
+cmake .. -DCMAKE_INSTALL_PREFIX="/path/to/desired/install/dir"
+make install
 ```
 
-The executable of the c++ code will be available in the `build/` folder.
+The UKF library will be available in the `install/lib` folder. The library is composed of two classes:
+
+- [SystemModel](lib/src/SystemModel.cpp): template class used to construct the state and observation models. Create a child class from this parent class, then edit it to add your models.
+- [UnscentedKF](lib/src/UnscentedKF.cpp): the class implementing the UKF algorithm.
+
+### Example
+
+An example of usage of the library is available in the [example](example) folder. The filter is used to estimate the thrust provided by a jet engine and its rate of change, given the measured thrust data.
+
+### Test
+
+A simple test to verify library integrity is provided in the [test](test) folder. The test uses [Catch2](https://github.com/catchorg/Catch2.git) library and can be run with the `ctest` command from the `build` directory.
+
+## Maintainer
 
+Gabriele Nava, @gabrielenava

example/CMakeLists.txt

@@ -0,0 +1,12 @@
+# Author: Gabriele Nava
+# Date: Ott. 2023
+
+# Configure the executable
+set(SRCS src/ThrustEstimationUKF.cpp src/JetSystemModel.cpp)
+add_executable(${PROJECT_NAME}_example ${SRCS})
+
+target_include_directories(${PROJECT_NAME}_example PUBLIC include)
+target_link_libraries(${PROJECT_NAME}_example ${PROJECT_NAME})
+
+# Copy the example data in the build folder
+configure_file("${CMAKE_SOURCE_DIR}/example/data/jetData.txt" "${CMAKE_BINARY_DIR}/example/jetData.txt" COPYONLY)

example/include/JetSystemModel.h

@@ -0,0 +1,29 @@
+#ifndef JET_SYSTEM_MODEL_H
+#define JET_SYSTEM_MODEL_H
+
+#include "SystemModel.h"
+
+class JetSystemModel : public SystemModel
+{
+public:
+  // overwrite constructor for the child class
+  JetSystemModel(int _nMeasures, double _dt);
+
+  // initialize variables of the child class
+  double dt = 0.01;
+  int nMeasures = 1;
+
+  // state model parameters
+  static Eigen::Matrix2d A;
+  static Eigen::Vector2d B;
+
+  // measurement model parameters
+  static Eigen::Vector2d h;
+
+  // add additional methods and override existing methods
+  void configureModelParameters();
+  Eigen::VectorXd dynamicsModel(const Eigen::VectorXd &x, const Eigen::VectorXd &u) override;
+  Eigen::VectorXd observationModel(const Eigen::VectorXd &x) override;
+};
+
+#endif /* end of include guard JET_SYSTEM_MODEL_H */

example/src/JetSystemModel.cpp

@@ -0,0 +1,35 @@
+/**
+ * @file JetSystemModel.cpp
+ * @author Gabriele Nava
+ * @date 2023
+ */
+#include <JetSystemModel.h>
+
+// definition of the new constructor; initialize model parameters
+JetSystemModel::JetSystemModel(int _nMeasures, double _dt) : nMeasures(_nMeasures), dt(_dt) {}
+Eigen::Matrix2d JetSystemModel::A = Eigen::Matrix2d::Zero();
+Eigen::Vector2d JetSystemModel::B = Eigen::Vector2d::Zero();
+Eigen::Vector2d JetSystemModel::h = Eigen::Vector2d::Zero();
+
+// definition of the child class methods
+void JetSystemModel::configureModelParameters()
+{
+  // for jet engines, implemented a simple kinematic model
+  this->A << 1.0, dt, 0.0, 1.0;
+  this->B << std::pow(dt, 2) / 2, dt;
+
+  // measurements are the observed thrusts
+  this->h << 1.0, 0.0;
+}
+
+// overwrite the dynamicsModel class
+Eigen::VectorXd JetSystemModel::dynamicsModel(const Eigen::VectorXd &x, const Eigen::VectorXd &u)
+{
+  return (this->A * x + this->B * u);
+}
+
+// overwrite the observationModel class
+Eigen::VectorXd JetSystemModel::observationModel(const Eigen::VectorXd &x)
+{
+  return this->h.transpose() * x;
+}

src/testUKF.cpp → example/src/ThrustEstimationUKF.cpp

@@ -1,26 +1,33 @@
 /**
- * @file main.cpp
+ * @file ThrustEstimationUKF.cpp
  * @author Gabriele Nava
  * @date 2023
  */
 #include <memory>
 #include <iostream>
 #include <fstream>
 #include <UnscentedKF.h>
-#include <SystemModel.h>
+#include <JetSystemModel.h>
+
+/*
+  This example illustrates how to use the SystemModel and UnscentedKF, the two classes composing Eigen_UKF code.
+  In this case, the UKF is used to estimate the thrust provided by a jet engine and its rate of change.
+*/
 
 int main(int argc, char const *argv[])
 {
+	std::cout << "Loading data..." << std::endl;
+
 	// open the file containing the example data
-	std::ifstream inputFile("example_data.txt");
+	std::ifstream inputFile("jetData.txt");
 
 	if (!inputFile.is_open())
 	{
-		std::cerr << "Failed to open the file." << std::endl;
+		std::cerr << "Failed to open the jet data file." << std::endl;
 		return 1;
 	}
 
-	// store the thrusts and time data
+	// create variables to store the thrusts and time data
 	std::vector<double> thrustData;
 	std::vector<double> timeData;
 	std::string line;
@@ -30,7 +37,8 @@ int main(int argc, char const *argv[])
 
 	while (std::getline(inputFile, line))
 	{
-		std::istringstream iss(line); // treat line as an input
+		// treat line as an input string
+		std::istringstream iss(line);
 		std::string thrustStr, timeStr;
 
 		if (std::getline(iss, thrustStr, ',') && std::getline(iss, timeStr, ','))
@@ -46,15 +54,22 @@ int main(int argc, char const *argv[])
 	// close the file
 	inputFile.close();
 
+	std::cout << "Data loaded!" << std::endl;
+	std::cout << "Running Estimation..." << std::endl;
+
 	// step time
 	double dt = 0.01;
 
 	// system initialization
-	Eigen::VectorXd u(1);
-	Eigen::VectorXd x0(2);
-	Eigen::MatrixXd Q(2, 2);
-	Eigen::MatrixXd R(1, 1);
-	Eigen::MatrixXd P(2, 2);
+	int nState = 2;
+	int nInput = 1;
+	int nMeasures = 1;
+
+	Eigen::VectorXd u(nInput);
+	Eigen::VectorXd x0(nState);
+	Eigen::MatrixXd Q(nState, nState);
+	Eigen::MatrixXd R(nMeasures, nMeasures);
+	Eigen::MatrixXd P(nState, nState);
 
 	// set initial conditions
 	x0.setZero();			   // initial state
@@ -63,16 +78,17 @@ int main(int argc, char const *argv[])
 	P << 1.0, 0.0, 0.0, 1.0;   // initial covariance matrix
 	u << 0.0;				   // exogenous input
 
-	// initialize a pointer to an instance of the SystemModel class
-	std::shared_ptr<SystemModel> model = std::make_shared<SystemModel>(R, Q, dt);
+	// initialize a shared pointer to an instance of the JetSystemModel class
+	std::shared_ptr<JetSystemModel> model = std::make_shared<JetSystemModel>(nMeasures, dt);
+	model->configureModelParameters();
 
 	// create an instance of the UKF class
-	UnscentedKF ukf(model, P);
+	UnscentedKF ukf(model, P, Q, R);
 	ukf.init(x0);
 
 	// iterate on the data and get the estimated state
 	Eigen::VectorXd y(1);
-	std::vector<Eigen::VectorXd> res;
+	std::vector<Eigen::VectorXd> x_est;
 
 	for (int k = 0; k < timeData.size(); k++)
 	{
@@ -82,7 +98,7 @@ int main(int argc, char const *argv[])
 		// call to the UKF
 		ukf.predict(u, dt);
 		ukf.update(y);
-		res.push_back(ukf.get_state());
+		x_est.push_back(ukf.get_state());
 	}
 
 	std::cout << "Estimation done!" << std::endl;
@@ -97,7 +113,7 @@ int main(int argc, char const *argv[])
 	}
 
 	// iterate through the vector and write each Eigen vector as a row
-	for (const Eigen::VectorXd &row : res)
+	for (const Eigen::VectorXd &row : x_est)
 	{
 		for (int i = 0; i < row.size(); ++i)
 		{

lib/CMakeLists.txt

@@ -0,0 +1,7 @@
+# Author: Gabriele Nava
+# Date: Ott. 2023
+
+# Configure and install the library
+add_library(${PROJECT_NAME} src/UnscentedKF.cpp src/SystemModel.cpp)
+target_include_directories(${PROJECT_NAME} PUBLIC include)
+install(TARGETS ${PROJECT_NAME} DESTINATION lib)