Merge pull request #7 from artivis/feature/rts

Add Rauch-Tung-Striebel Smoother

.github/workflows/ci.yml

@@ -57,6 +57,9 @@ jobs:
       - name: Build
         working-directory: ${{runner.workspace}}/build
         run: cmake --build . --config Release
+      - name: Demo
+        working-directory: ${{runner.workspace}}/build
+        run: cmake --build . --target run_demo --config Release
       - name: Test
         working-directory: ${{runner.workspace}}/build
         run: ctest . -C Release
@@ -94,6 +97,9 @@ jobs:
       - name: Build
         working-directory: ${{runner.workspace}}/build
         run: cmake --build . --config Release
+      - name: Demo
+        working-directory: ${{runner.workspace}}/build
+        run: cmake --build . --target run_demo --config Release
       - name: Test
         working-directory: ${{runner.workspace}}/build
         run: ctest . -C Release
@@ -139,6 +145,9 @@ jobs:
       - name: Build
         working-directory: ${{runner.workspace}}/build
         run: cmake --build . --config Release
+      - name: Demo
+        working-directory: ${{runner.workspace}}/build
+        run: cmake --build . --target run_demo --config Release
       - name: Test
         working-directory: ${{runner.workspace}}/build
         run: ctest . -C Release

CMakeLists.txt

@@ -25,7 +25,6 @@ option(BUILD_TESTING "Build all tests." OFF)
 
 # SE2 demo/example produce unstable covariance matrix.
 # Until it is fixed, force disable kalmanif asserts.
-# set(KALMANIF_NO_DEBUG "Disable kalmanif asserts" ON)
 add_compile_definitions(KALMANIF_NO_DEBUG)
 
 ##################
@@ -94,7 +93,6 @@ export(PACKAGE ${PROJECT_NAME})
 
 ## Configuration
 
-#
 configure_package_config_file(
   "${PROJECT_SOURCE_DIR}/cmake/${PROJECT_NAME}Config.cmake.in"
   "${generated_dir}/${PROJECT_NAME}Config.cmake"

README.md

@@ -13,12 +13,15 @@
 targeted at robotics applications.
 It is developed as a header-only C++17 library based on [manif][manif-repo].
 
-At the moment, it implements the filters:
+At the moment, it implements:
 
 - Extended Kalman Filter (EKF)
 - Square Root Extended Kalman Filter (SEKF)
 - Invariant Extended Kalman Filter (IEKF)
 - Unscented Kalman Filter on manifolds (UKFM)
+- Rauch-Tung-Striebel Smoother*
+
+(*the RTS Smoother is compatible with all filters - ERTS / SERTS / IERTS/ URTS-M)
 
 Together with a few system and measurement models mostly for demo purpose.
 Other filters/models can and will be added, contributions are welcome.
@@ -46,7 +49,7 @@ Checkout the installation guide [over here](CONTRIBUTING.md#development-environm
 
 ### Note
 
-Both the `IEKF` and `UKFM` filters are implemented in there **'right invariant'** flavor.
+Both the `IEKF` and `UKFM` filters are implemented in their **'right invariant'** flavor.
 However they are able to handle both 'right' *and* 'left' measurements.
 
 <!-- ## Documentation -->

docs/publications.md

@@ -58,9 +58,44 @@ Both `kalmanif` and **manif** often refer to the following [paper][jsola18]:
   doi={10.1109/ICRA40945.2020.9197489}}
 ```
 
+`kalmanif` implements the [Invariant Rauch-Tung-Stribel Smoother][laan20] (IERTS) described in:
+
+```latex
+@article{Laan-RAL-20,
+  author={van der Laan, Niels and Cohen, Mitchell and Arsenault, Jonathan and Forbes, James Richard},
+  journal={IEEE Robotics and Automation Letters},
+  title={The Invariant Rauch-Tung-Striebel Smoother},
+  year={2020},
+  volume={5},
+  number={4},
+  pages={5067-5074},
+  doi={10.1109/LRA.2020.3005132}}
+```
+
+`kalmanif` implements the 'Unscented Rauch-Tung-Stribel Smoother on Manifold' (URTS-M) based on:
+
+- the aforementioned [Unscented Kalman Filter on Manifolds][brossard20]
+- the aforementioned [Invariant Rauch-Tung-Stribel Smoother][laan20]
+- and [Bayesian Filtering and Smoothing][sarkka13] Sec. 9.3
+
+```latex
+@book{sarkka_2013,
+  place={Cambridge},
+  series={Institute of Mathematical Statistics Textbooks},
+  title={Bayesian Filtering and Smoothing},
+  DOI={10.1017/CBO9781139344203},
+  publisher={Cambridge University Press},
+  author={S{\"a}rkk{\"a}, Simo},
+  collection={Institute of Mathematical Statistics Textbooks},
+  number={3},
+  year={2013}}
+```
+
 [//]: # (URLs)
 
 [jsola18]: http://arxiv.org/abs/1812.01537
 [deray20]: https://joss.theoj.org/papers/10.21105/joss.01371
 [brossard20]: https://arxiv.org/pdf/2002.00878.pdf
 [Barrau17]: https://arxiv.org/pdf/1410.1465.pdf
+[laan20]: https://ras.papercept.net/proceedings/IROS20/2526.pdf
+[sarkka13]: https://www.cambridge.org/core/books/bayesian-filtering-and-smoothing/C372FB31C5D9A100F8476C1B23721A67

examples/CMakeLists.txt

@@ -36,6 +36,7 @@ set(CXX_17_EXAMPLE_TARGETS
   demo_se_2_3
 )
 
+set(run_all_demo_cmds)
 foreach(target ${CXX_17_EXAMPLE_TARGETS})
   # Link to kalmanif
   target_link_libraries(${target} ${PROJECT_NAME})
@@ -45,10 +46,6 @@ foreach(target ${CXX_17_EXAMPLE_TARGETS})
     target_compile_options(${target} PRIVATE
       -Werror=all
       -Werror=extra
-      -Wno-unknown-pragmas
-      -Wno-sign-compare
-      -Wno-unused-parameter
-      -Wno-missing-field-initializers
     )
   endif()
 
@@ -67,8 +64,13 @@ foreach(target ${CXX_17_EXAMPLE_TARGETS})
     # Link to sciplot
     target_link_sciplot(${target})
   endif()
+
+  list(APPEND run_demo_cmds COMMAND $<TARGET_FILE:${target}> -q)
 endforeach()
 
+add_custom_target(run_demo ${run_demo_cmds} COMMENT "Runs all demo")
+add_dependencies(run_demo ${CXX_17_EXAMPLE_TARGETS})
+
 # Set required C++17 flag
 set_property(TARGET ${CXX_17_EXAMPLE_TARGETS} PROPERTY CXX_STANDARD 17)
 set_property(TARGET ${CXX_17_EXAMPLE_TARGETS} PROPERTY CXX_STANDARD_REQUIRED ON)

examples/demo_se2.cpp

@@ -120,18 +120,26 @@ using MeasurementModel = Landmark2DMeasurementModel<State>;
 using Landmark = MeasurementModel::Landmark;
 using Measurement = MeasurementModel::Measurement;
 
+// Filters
 using EKF = ExtendedKalmanFilter<State>;
 using SEKF = SquareRootExtendedKalmanFilter<State>;
 using IEKF = InvariantExtendedKalmanFilter<State>;
 using UKFM = UnscentedKalmanFilterManifolds<State>;
 
+// Smoothers
+using ERTS = RauchTungStriebelSmoother<EKF>;
+using SERTS = RauchTungStriebelSmoother<SEKF>;
+using IERTS = RauchTungStriebelSmoother<IEKF>;
+using URTSM = RauchTungStriebelSmoother<UKFM>;
+
 int main (int argc, char* argv[]) {
 
   KALMANIF_DEMO_PROCESS_INPUT(argc, argv);
   KALMANIF_DEMO_PRETTY_PRINT();
 
   // START CONFIGURATION
 
+  constexpr double eot = 30;                  // s
   constexpr double dt = 0.01;                 // s
   double sqrtdt = std::sqrt(dt);
 
@@ -141,6 +149,8 @@ int main (int argc, char* argv[]) {
 
   constexpr int gps_freq = 10;                // Hz
   constexpr int landmark_freq = 50;           // Hz
+  (void)landmark_freq;
+  (void)gps_freq;
 
   State X_simulation = State::Identity(),
         X_unfiltered = State::Identity(); // propagation only, for comparison purposes
@@ -192,6 +202,8 @@ int main (int argc, char* argv[]) {
   Eigen::Vector3d X_init_coeffs = state_cov_init.cwiseSqrt() * n;
   State X_init(X_init_coeffs(0), X_init_coeffs(1), X_init_coeffs(2));
 
+  // X_unfiltered = X_init;
+
   EKF ekf;
   ekf.setState(X_init);
   ekf.setCovariance(state_cov_init);
@@ -202,11 +214,22 @@ int main (int argc, char* argv[]) {
 
   UKFM ukfm(X_init, state_cov_init);
 
+  ERTS erts(X_init, state_cov_init);
+
+  SERTS serts(X_init, state_cov_init);
+
+  IERTS ierts(X_init, state_cov_init);
+
+  URTSM urtsm(X_init, state_cov_init);
+
   // Store some data for plots
   DemoDataCollector<State> collector;
+  collector.reserve(
+    eot/dt, "UNFI", "EKF", "SEKF", "IEKF", "UKFM", "ERTS", "SERTS", "IERTS", "URTSM"
+  );
 
   // Make T steps. Measure up to K landmarks each time.
-  for (double t = 0; t < 30; t += dt) {
+  for (double t = 0; t < eot; t += dt) {
     //// I. Simulation
 
     /// simulate noise
@@ -221,7 +244,7 @@ int main (int argc, char* argv[]) {
     X_simulation = system_model(X_simulation, u_simu);
 
     /// then we measure all landmarks - - - - - - - - - - - - - - - - - - - -
-    for (int i = 0; i < measurement_models.size(); ++i) {
+    for (std::size_t i = 0; i < measurement_models.size(); ++i) {
       auto measurement_model = measurement_models[i];
 
       y = measurement_model(X_simulation);            // landmark measurement, before adding noise
@@ -245,33 +268,48 @@ int main (int argc, char* argv[]) {
 
     ukfm.propagate(system_model, u_est);
 
+    erts.propagate(system_model, u_est);
+
+    serts.propagate(system_model, u_est);
+
+    ierts.propagate(system_model, u_est, dt);
+
+    urtsm.propagate(system_model, u_est);
+
     X_unfiltered = system_model(X_unfiltered, u_unfilt);
 
     /// Then we correct using the measurements of each lmk
 
-    if (int(t*100) % int(100./landmark_freq) == 0) {
-      for (int i = 0; i < measurement_models.size(); ++i) {
+    // if (int(t*100) % int(100./landmark_freq) == 0) {
+      // for (std::size_t i = 0; i < measurement_models.size(); ++i) {
 
-        // landmark
-        auto measurement_model = measurement_models[i];
+      //   // landmark
+      //   auto measurement_model = measurement_models[i];
 
-        // measurement
-        y = measurements[i];
+      //   // measurement
+      //   y = measurements[i];
 
-        // filter update
-        ekf.update(measurement_model, y);
+      //   // filter update
+        // ekf.update(measurement_model, y);
 
-        sekf.update(measurement_model, y);
+        // sekf.update(measurement_model, y);
 
-        iekf.update(measurement_model, y);
+        // iekf.update(measurement_model, y);
 
-        ukfm.update(measurement_model, y);
+        // ukfm.update(measurement_model, y);
 
-      }
-    }
+        // erts.update(measurement_model, y);
+
+        // serts.update(measurement_model, y);
 
-    // GPS measurement update
-    if (int(t*100) % int(100./gps_freq) == 0) {
+        // ierts.update(measurement_model, y);
+
+        // urtsm.update(measurement_model, y);
+      // }
+    // }
+
+    // // GPS measurement update
+    // if (int(t*100) % int(100./gps_freq) == 0) {
 
       // gps measurement model
       auto gps_measurement_model = DummyGPSMeasurementModel<State>(R_gps);
@@ -290,7 +328,15 @@ int main (int argc, char* argv[]) {
       iekf.update(gps_measurement_model, y_gps);
 
       ukfm.update(gps_measurement_model, y_gps);
-    }
+
+      erts.update(gps_measurement_model, y_gps);
+
+      serts.update(gps_measurement_model, y_gps);
+
+      ierts.update(gps_measurement_model, y_gps);
+
+      urtsm.update(gps_measurement_model, y_gps);
+    // }
 
     //// III. Next iteration
 
@@ -300,32 +346,49 @@ int main (int argc, char* argv[]) {
 
     //// IV. Results
 
-    auto X_e = ekf.getState();
-    auto X_s = sekf.getState();
-    auto X_i = iekf.getState();
-    auto X_u = ukfm.getState();
-
-    collector.collect("EKF",  X_simulation, X_e, ekf.getCovariance(), t);
-    collector.collect("SEKF", X_simulation, X_s, sekf.getCovariance(), t);
-    collector.collect("IEKF", X_simulation, X_i, iekf.getCovariance(), t);
-    collector.collect("UKFM", X_simulation, X_u, ukfm.getCovariance(), t);
-    collector.collect("UNFI", X_simulation, X_unfiltered, StateCovariance::Zero(), t);
-
-    std::cout << "X simulated      : " << X_simulation.log()      << "\n"
-              << "X estimated EKF  : " << X_e.log()
-              << " : |d|=" << (X_simulation - X_e).weightedNorm() << "\n"
-              << "X estimated SEKF : " << X_s.log()
-              << " : |d|=" << (X_simulation - X_s).weightedNorm() << "\n"
-              << "X estimated IEKF : " << X_i.log()
-              << " : |d|=" << (X_simulation - X_i).weightedNorm() << "\n"
-              << "X estimated UKFM : " << X_u.log()
-              << " : |d|=" << (X_simulation - X_u).weightedNorm() << "\n"
-              << "X unfilterd      : " << X_unfiltered.log()
-              << " : |d|=" << (X_simulation - X_unfiltered).weightedNorm() << "\n"
-              << "----------------------------------"                      << "\n";
+    collector.collect(X_simulation, t);
+
+    collector.collect("UNFI", X_unfiltered, StateCovariance::Zero(), t);
+
+    collector.collect("EKF",  ekf.getState(), ekf.getCovariance(), t);
+    collector.collect("SEKF", sekf.getState(), sekf.getCovariance(), t);
+    collector.collect("IEKF", iekf.getState(), iekf.getCovariance(), t);
+    collector.collect("UKFM", ukfm.getState(), ukfm.getCovariance(), t);
   }
 
-  // END OF TEMPORAL LOOP. DONE.
+  // END OF TEMPORAL LOOP, forward pass
+
+  // Batch backward pass - smoothing
+  {
+    erts.smooth();
+    const auto& Xs_erts = erts.getStates();
+    const auto& Ps_erts = erts.getCovariances();
+
+    serts.smooth();
+    const auto& Xs_serts = serts.getStates();
+    const auto& Ps_serts = serts.getCovariances();
+
+    ierts.smooth();
+    const auto& Xs_ierts = ierts.getStates();
+    const auto& Ps_ierts = ierts.getCovariances();
+
+    urtsm.smooth();
+    const auto& Xs_urtsm = urtsm.getStates();
+    const auto& Ps_urtsm = urtsm.getCovariances();
+
+    double t=0;
+    for (std::size_t i=0; i<Xs_erts.size(); ++i, t+=dt) {
+      collector.collect("ERTS", Xs_erts[i], Ps_erts[i], t);
+      collector.collect("SERTS", Xs_serts[i], Ps_serts[i], t);
+      collector.collect("IERTS", Xs_ierts[i], Ps_ierts[i], t);
+      collector.collect("URTSM", Xs_urtsm[i], Ps_urtsm[i], t);
+    }
+  }
+
+  // print the trajectory
+  if (!quiet) {
+    KALMANIF_DEMO_PRINT_TRAJECTORY(collector);
+  }
 
   // Generate some metrics and print them
   DemoDataProcessor<State>().process(collector).print();