Feature/better imm filter

.clang-format

@@ -23,7 +23,7 @@ AlwaysBreakAfterDefinitionReturnType: None
 AlwaysBreakAfterReturnType: None
 AlwaysBreakBeforeMultilineStrings: false
 AlwaysBreakTemplateDeclarations: MultiLine
-BinPackArguments: true
+BinPackArguments: false
 BinPackParameters: true
 BraceWrapping:
   AfterCaseLabel:  false
@@ -47,8 +47,8 @@ BreakBeforeBraces: Stroustrup
 BreakBeforeInheritanceComma: false
 BreakInheritanceList: BeforeColon
 BreakBeforeTernaryOperators: true
-BreakConstructorInitializersBeforeComma: false
-BreakConstructorInitializers: BeforeColon
+BreakConstructorInitializersBeforeComma: true
+BreakConstructorInitializers: BeforeComma
 BreakAfterJavaFieldAnnotations: false
 BreakStringLiterals: true
 ColumnLimit:     160
@@ -67,7 +67,7 @@ ForEachMacros:
   - foreach
   - Q_FOREACH
   - BOOST_FOREACH
-IncludeBlocks:   Preserve
+IncludeBlocks:   Regroup
 IncludeCategories:
   - Regex:           '^"(llvm|llvm-c|clang|clang-c)/'
     Priority:        2

.github/workflows/run-unit-tests.yml

@@ -52,6 +52,15 @@ jobs:
         uses: ros-tooling/[email protected]
         with:
           required-ros-distributions: ${{ matrix.ros_distribution }}
+      - name: Use gcc 13 and g++ 13
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y software-properties-common
+          sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test
+          sudo apt-get update
+          sudo apt-get install -y gcc-13 g++-13
+          sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-13 100 --slave /usr/bin/g++ g++ /usr/bin/g++-13
+          sudo update-alternatives --config gcc
       - name: Build and test ROS 2 packages
         uses: ros-tooling/[email protected]
         id: action_ros_ci_step

.gitignore

@@ -1 +1,4 @@
-.vscode
+.vscode
+build/*
+install/*
+log/*

README.md

@@ -1,8 +1,9 @@
 # Vortex VKF
 ## Introduction
-This is a C++ library for state estimation.
+This is a C++ library for state estimation. VKF stands for Visual Kalman Filter which is a stupid and misleading name
 
 ## Packages
-### vortex-filtering
-Currently, it has a pure C++ implementation of the Unscented Kalman Filter (UKF) and the Extended Kalman Filter (EKF) as well as model
-[More info](vortex-filtering/README.md)
+
+- [vortex-filtering](vortex-filtering/README.md)
+
+

vortex-filtering/CMakeLists.txt

@@ -13,6 +13,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(
     -Wall -Wextra -Wpedantic 
     -fopenmp # For parallel processing with Eigen
+    -fconcepts-diagnostics-depth=2 # For better concepts error messages
+    -Warray-bounds # For better array bounds error messages
   )
 endif()
 
@@ -32,7 +34,11 @@ find_package(Boost REQUIRED
     filesystem
 ) # for gnuplot
 
-# = Specify dependencies
+
+# === Include directories ===
+include_directories(include)
+
+# = Specify the dependencies
 set(lib_deps
   Eigen3
   OpenMP
@@ -84,8 +90,8 @@ install(
   DESTINATION include
 )
 
-# === Build tests ===
 if(BUILD_TESTING)
+  # add_compile_definitions(GNUPLOT_ENABLE=1)
   add_subdirectory(test)
 endif()
 

vortex-filtering/README.md

@@ -6,4 +6,72 @@ Contains the models used in the filters. The models are implemented as classes t
 Contains the filters. The filters are implemented as classes that inherit from the `KalmanFilterBase` class. The filters are implemented in the `filters` namespace. [More info](include/vortex_filtering/filters/README.md)
 
 ## Class Diagram
-![Class Diagram](../docs/vkf_class_diagram.md)
+
+```mermaid
+classDiagram
+
+    DynamicModel <|-- DynamicModelLTV
+    SensorModel <|-- SensorModelLTV
+    DynamicModelLTV <|-- ConstantVelocity
+    DynamicModelLTV <|-- ConstantAcceleration
+    DynamicModelLTV <|-- CoordinatedTurn
+
+    EKF -- DynamicModelLTV
+    EKF -- SensorModelLTV
+
+    UKF -- DynamicModel
+    UKF -- SensorModel
+
+
+
+    class EKF{
+        +predict()
+        +update()
+        +step()
+    }
+
+    class UKF{
+        +predict()
+        +update()
+        +step()
+        -get_sigma_points()
+        -propagate_sigma_points_f()
+        -propagate_sigma_points_h()
+        -estimate_gaussian()
+    }
+
+    class DynamicModel{
+        +virtual f_d() Vec_x
+        +virtual Q_d() Mat_vv
+        +sample_f_d() Vec_x
+    }
+
+    class SensorModel{
+        +virtual h() Vec_z
+        +virtual R() Mat_ww
+        +sample_h() Vec_z
+    }
+
+    class DynamicModelLTV {
+        +overide f_d() Vec_x
+        +virtual A_d() Mat_xx
+        +virtual Q_d() Mat_vv
+        +vurtual G_d() Mat_xv
+        +pred_from_est() Gauss_x
+        +pred_from_state() Gauss_x
+    }
+
+    class SensorModelLTV {
+        +override h(x) Vec_z
+        +virtual R(x) Mat_ww
+        +virtual C(x) Mat_zx
+        +virtual H(x) Mat_zw
+        +pred_from_est(x_est) Gauss_z
+        +pred_from_state(x) Gauss_z
+    }
+
+    class ConstantVelocity
+    class CoordinatedTurn
+    class ConstantAcceleration
+
+```