Feature/adding measurement models

.github/workflows/clang-format-check.yml

@@ -16,7 +16,7 @@ jobs:
     - name: Run clang-format style check for C/C++/Protobuf programs.
       uses: jidicula/clang-format-action@v4.11.0
       with:
-        clang-format-version: '13'
+        clang-format-version: '21'
         check-path: ${{ matrix.path['check'] }}
         exclude-regex: ${{ matrix.path['exclude'] }}
         fallback-style: 'Microsoft' # optional

src/CMakeLists.txt

@@ -27,15 +27,21 @@ set(LIBRARY_SRC_FILES
 set(LIBRARY_HDR_FILES
     types.h
     util.h
+    # Kalman filter files.
     kalman_filter/kalman_filter.h
     kalman_filter/unscented_transform.h
     kalman_filter/unscented_kalman_filter.h
     kalman_filter/square_root_ukf.h
+    # Motion model files.
     motion_model/motion_model.h
     motion_model/ego_motion_model.h
     motion_model/cv_motion_model.h
     motion_model/ca_motion_model.h
     motion_model/ct_motion_model.h
+    # Measurement model files.
+    meas_model/meas_model.h
+    meas_model/range_bearing_meas_model.h
+    meas_model/bearing_meas_model.h
 )
 
 set(LIBRARY_NAME ${PROJECT_NAME})

src/meas_model/bearing_meas_model.h

@@ -0,0 +1,87 @@
+#ifndef OPENKF_BEARING_MEAS_MODEL_H
+#define OPENKF_BEARING_MEAS_MODEL_H
+
+#include "meas_model.h"
+#include "types.h"
+
+namespace kf
+{
+namespace measmodel
+{
+/// @brief Measurement space dimension for bearing measurement
+/// model
+/// \vec{z}=[pos_x, pos_y]^T
+static constexpr int32_t DIM_Z_BEARING{2};
+
+template <int32_t DIM_X>
+class BearingMeasModel
+    : public MeasModel<BearingMeasModel<DIM_X>, DIM_X, DIM_Z_BEARING>
+{
+ public:
+  BearingMeasModel(Vector<2> const& sensPos2D,
+                   float32_t const bearingSigma = 1.0F)
+      : m_sensPos2D{sensPos2D}, m_bearingNoiseSigma{bearingSigma}
+  {
+  }
+  ~BearingMeasModel() {}
+
+  static constexpr int32_t IDX_X_PX{0};  //< Index for position x in state
+  static constexpr int32_t IDX_X_PY{1};  //< Index for position y in state
+
+  static constexpr int32_t IDX_Z_BEARING{
+      0};  //< Index for bearing in measurement
+
+  /// @brief Measurement model function that maps the state space vector to
+  /// the measurement space.
+  /// @param vecX State space vector \vec{x}
+  /// @return Measurement space vector \vec{z}
+  Vector<DIM_Z_BEARING> h(Vector<DIM_X> const& vecX) const
+  {
+    Vector<DIM_Z_BEARING> vecZ;
+    float32_t px = vecX(IDX_X_PX) - m_sensPos2D(IDX_X_PX);
+    float32_t py = vecX(IDX_X_PY) - m_sensPos2D(IDX_X_PY);
+    vecZ(IDX_Z_BEARING) = std::atan2(py, px);
+    return vecZ;
+  }
+
+  /// @brief Method that calculates the jacobians of the measurement model.
+  /// @param vecX State Space vector \vec{x}
+  /// @return The jacobians of the measurement model.
+  Matrix<DIM_Z_BEARING, DIM_X> getJacobianHk(Vector<DIM_X> const& vecX) const
+  {
+    Matrix<DIM_Z_BEARING, DIM_X> matH;
+    matH.setZero();
+
+    float32_t const px{vecX(IDX_X_PX) - m_sensPos2D(IDX_X_PX)};
+    float32_t const py{vecX(IDX_X_PY) - m_sensPos2D(IDX_X_PY)};
+    float32_t const denom{px * px + py * py};
+
+    matH(IDX_Z_BEARING, IDX_X_PX) = -py / denom;
+    matH(IDX_Z_BEARING, IDX_X_PY) = px / denom;
+    return matH;
+  }
+
+  /// @brief Get the measurement noise covariance R
+  /// @return The measurement noise covariance R
+  Matrix<DIM_Z_BEARING, DIM_Z_BEARING> getMeasurementNoiseCov() const
+  {
+    float32_t const bearingNoiseSigma2{m_bearingNoiseSigma *
+                                       m_bearingNoiseSigma};
+    Matrix<DIM_Z_BEARING, DIM_Z_BEARING> matR;
+    matR.setZero();
+    matR(IDX_Z_BEARING, IDX_Z_BEARING) = bearingNoiseSigma2;
+    return matR;
+  }
+
+  /// @brief Set bearing noise standard deviation
+  /// @param sigma Bearing noise standard deviation
+  void setBearingSigma(float32_t const sigma) { m_bearingNoiseSigma = sigma; }
+
+ private:
+  Vector<2> m_sensPos2D;          //< Sensor position in 2D
+  float32_t m_bearingNoiseSigma;  //< Bearing measurement noise vector
+};
+}  // namespace measmodel
+
+}  // namespace kf
+#endif  // OPENKF_BEARING_MEAS_MODEL_H

src/meas_model/meas_model.h

@@ -0,0 +1,50 @@
+#ifndef OPENKF_MEAS_MODEL_H
+#define OPENKF_MEAS_MODEL_H
+
+#include "types.h"
+
+namespace kf
+{
+
+namespace measmodel
+{
+/// @brief Base class for measurement models used by kalman filters
+/// @tparam DIM_X State space vector dimension
+/// @tparam DIM_Z Measurement space vector dimension
+template <class Derived, int32_t DIM_X, int32_t DIM_Z>
+class MeasModel
+{
+ public:
+  /// @brief Get the measurement space vector dimension
+  /// @return The measurement space vector dimension
+  int32_t getDimZ() const { return DIM_Z; }
+
+  /// @brief Measurement model function that maps the state space vector to
+  /// the measurement space.
+  /// @param vecX State space vector \vec{x}
+  /// @return Measurement space vector \vec{z}
+  Vector<DIM_Z> h(Vector<DIM_X> const& vecX) const
+  {
+    return static_cast<Derived const*>(this)->h(vecX);
+  }
+
+  /// @brief Method that calculates the jacobians of the measurement model.
+  /// @param vecX State Space vector \vec{x}
+  /// @return The jacobians of the measurement model.
+  Matrix<DIM_Z, DIM_X> getJacobianHk(Vector<DIM_X> const& vecX) const
+  {
+    return static_cast<Derived const*>(this)->getJacobianHk(vecX);
+  }
+
+  /// @brief Get the measurement noise covariance R
+  /// @return The measurement noise covariance R
+  Matrix<DIM_Z, DIM_Z> getMeasurementNoiseCov() const
+  {
+    return static_cast<Derived const*>(this)->getMeasurementNoiseCov();
+  }
+};
+}  // namespace measmodel
+
+}  // namespace kf
+
+#endif  // OPENKF_MEAS_MODEL_H

src/meas_model/pos2d_meas_model.h

@@ -0,0 +1,83 @@
+#ifndef OPENKF_POS2D_MEAS_MODEL_H
+#define OPENKF_POS2D_MEAS_MODEL_H
+
+#include "meas_model.h"
+#include "types.h"
+
+namespace kf
+{
+namespace measmodel
+{
+/// @brief Measurement space dimension for 2D position measurement model
+/// \vec{z}=[pos_x, pos_y]^T
+static constexpr int32_t DIM_Z_POS2D{2};
+
+/// @brief  measurement model
+/// @tparam DIM_X State space vector dimension \vec{x}=[pos_x, pos_y, ...]^T
+template <int32_t DIM_X>
+class Pos2dMeasModel
+    : public MeasModel<Pos2dMeasModel<DIM_X>, DIM_X, DIM_Z_POS2D>
+{
+ public:
+  Pos2dMeasModel(Vector<2> const& sensPos, float32_t const posSigma = 1.0F)
+      : m_sensPos{sensPos}, m_posSigma{posSigma}
+  {
+  }
+  ~Pos2dMeasModel() {}
+
+  static constexpr int32_t IDX_X_PX{0};  //< Index for position x in state
+  static constexpr int32_t IDX_X_PY{1};  //< Index for position y in state
+
+  static constexpr int32_t IDX_Z_PX{0};  //< Index for position x in measurement
+  static constexpr int32_t IDX_Z_PY{1};  //< Index for position y in measurement
+
+  /// @brief Measurement model function that maps the state space vector to
+  /// the measurement space.
+  /// @param vecX State space vector \vec{x}
+  /// @return Measurement space vector \vec{z}
+  Vector<DIM_Z_POS2D> h(Vector<DIM_X> const& vecX) const
+  {
+    Vector<DIM_Z_POS2D> vecZ;
+    vecZ(IDX_Z_PX) = vecX(IDX_X_PX) - m_sensPos(0);
+    vecZ(IDX_Z_PY) = vecX(IDX_X_PY) - m_sensPos(1);
+    return vecZ;
+  }
+
+  /// @brief Method that calculates the jacobians of the measurement model.
+  /// @param vecX State Space vector \vec{x}
+  /// @return The jacobians of the measurement model.
+  Matrix<DIM_Z_POS2D, DIM_X> getJacobianHk(Vector<DIM_X> const& vecX) const
+  {
+    // H = [1 0 0 0 0;
+    //      0 1 0 0 0];
+    Matrix<DIM_Z_POS2D, DIM_X> matH;
+    matH.setZero();
+    matH(IDX_Z_PX, IDX_X_PX) = 1.0f;
+    matH(IDX_Z_PY, IDX_X_PY) = 1.0f;
+    return matH;
+  }
+
+  /// @brief Get the measurement noise covariance R
+  /// @return The measurement noise covariance R
+  Matrix<DIM_Z_POS2D, DIM_Z_POS2D> getMeasurementNoiseCov() const
+  {
+    float32_t const posNoiseSigma2{m_posSigma * m_posSigma};
+    Matrix<DIM_Z_POS2D, DIM_Z_POS2D> matR;
+    matR.setZero();
+    matR(IDX_Z_PX, IDX_Z_PX) = posNoiseSigma2;
+    matR(IDX_Z_PY, IDX_Z_PY) = posNoiseSigma2;
+    return matR;
+  }
+
+  /// @brief Set position noise standard deviation
+  /// @param sigma Position noise standard deviation
+  void setPosSigma(float32_t const sigma) { m_posSigma = sigma; }
+
+ private:
+  Vector<2> m_sensPos;   //< Sensor position in 2D space
+  float32_t m_posSigma;  //< Measurement noise vector
+};
+}  // namespace measmodel
+
+}  // namespace kf
+#endif  // OPENKF_POS2D_MEAS_MODEL_H

src/meas_model/range_bearing_meas_model.h

@@ -0,0 +1,106 @@
+#ifndef OPENKF_RANGE_BEARING_MEAS_MODEL_H
+#define OPENKF_RANGE_BEARING_MEAS_MODEL_H
+
+#include "meas_model.h"
+#include "types.h"
+
+namespace kf
+{
+namespace measmodel
+{
+/// @brief Measurement space dimension for range-bearing measurement
+/// model
+/// \vec{z}=[pos_x, pos_y]^T
+static constexpr int32_t DIM_Z_BEARING{2};
+
+template <int32_t DIM_X>
+class RangeBearingMeasModel
+    : public MeasModel<RangeBearingMeasModel<DIM_X>, DIM_X, DIM_Z_BEARING>
+{
+ public:
+  RangeBearingMeasModel(Vector<2> const& sensPos2D,
+                        float32_t const rangeSigma = 1.0F,
+                        float32_t const bearingSigma = 1.0F)
+      : m_sensPos2D{sensPos2D}, m_rangeSigma{rangeSigma},
+        m_bearingSigma{bearingSigma}
+  {
+  }
+  ~RangeBearingMeasModel() {}
+
+  static constexpr int32_t IDX_X_PX{0};  //< Index for position x in state
+  static constexpr int32_t IDX_X_PY{1};  //< Index for position y in state
+
+  static constexpr int32_t IDX_Z_RANGE{0};  //< Index for range in measurement
+  static constexpr int32_t IDX_Z_BEARING{
+      1};  //< Index for bearing in measurement
+
+  /// @brief Measurement model function that maps the state space vector to
+  /// the measurement space.
+  /// @param vecX State space vector \vec{x}
+  /// @return Measurement space vector \vec{z}
+  Vector<DIM_Z_BEARING> h(Vector<DIM_X> const& vecX) const
+  {
+    Vector<DIM_Z_BEARING> vecZ;
+    float32_t const px{vecX(IDX_X_PX) - m_sensPos2D(IDX_X_PX)};
+    float32_t const py{vecX(IDX_X_PY) - m_sensPos2D(IDX_X_PY)};
+    vecZ(IDX_Z_RANGE) = std::sqrt(px * px + py * py);
+    vecZ(IDX_Z_BEARING) = std::atan2(py, px);
+    return vecZ;
+  }
+
+  /// @brief Method that calculates the jacobians of the measurement model.
+  /// @param vecX State Space vector \vec{x}
+  /// @return The jacobians of the measurement model.
+  Matrix<DIM_Z_BEARING, DIM_X> getJacobianHk(Vector<DIM_X> const& vecX) const
+  {
+    Matrix<DIM_Z_BEARING, DIM_X> matH;
+    matH.setZero();
+
+    float32_t const px{vecX(IDX_X_PX) - m_sensPos2D(IDX_X_PX)};
+    float32_t const py{vecX(IDX_X_PY) - m_sensPos2D(IDX_X_PY)};
+    float32_t const denom{px * px + py * py};
+
+    matH(IDX_Z_RANGE, IDX_X_PX) = px / std::sqrt(denom);
+    matH(IDX_Z_RANGE, IDX_X_PY) = py / std::sqrt(denom);
+    matH(IDX_Z_BEARING, IDX_X_PX) = -py / denom;
+    matH(IDX_Z_BEARING, IDX_X_PY) = px / denom;
+    return matH;
+  }
+
+  /// @brief Get the measurement noise covariance R
+  /// @return The measurement noise covariance R
+  Matrix<DIM_Z_BEARING, DIM_Z_BEARING> getMeasurementNoiseCov() const
+  {
+    float32_t const rangeSigma2{m_rangeSigma * m_rangeSigma};
+    float32_t const bearingSigma2{m_bearingSigma * m_bearingSigma};
+
+    Matrix<DIM_Z_BEARING, DIM_Z_BEARING> matR;
+    matR.setZero();
+    matR(IDX_Z_RANGE, IDX_Z_RANGE) = rangeSigma2;
+    matR(IDX_Z_BEARING, IDX_Z_BEARING) = bearingSigma2;
+    return matR;
+  }
+
+  /// @brief Set range noise standard deviation
+  /// @param rangeSigma Range measurement noise standard deviation
+  void setRangeNoiseSigma(float32_t const rangeSigma)
+  {
+    m_rangeSigma = rangeSigma;
+  }
+
+  /// @brief Set bearing noise standard deviation
+  /// @param bearingSigma Bearing measurement noise standard deviation
+  void setBearingNoiseSigma(float32_t const bearingSigma)
+  {
+    m_bearingSigma = bearingSigma;
+  }
+
+ private:
+  Vector<2> m_sensPos2D;     //< Sensor position in 2D
+  float32_t m_rangeSigma;    //< Range measurement noise standard deviation
+  float32_t m_bearingSigma;  //< Bearing measurement noise standard deviation
+};
+}  // namespace measmodel
+
+}  // namespace kf
+#endif  // OPENKF_RANGE_BEARING_MEAS_MODEL_H

src/motion_model/ca_motion_model.h

@@ -39,10 +39,6 @@ class CaMotionModel : public MotionModel<CaMotionModel, DIM_X_CA>
   Matrix<DIM_X_CA, DIM_X_CA> getJacobianFk(Vector<DIM_X_CA> const& vecX,
                                            float32_t dt = 1.0F) const;
 
-  /// @brief Get state dimension
-  /// @return State dimension
-  static constexpr int32_t getStateDim() { return DIM_X_CA; }
-
   /// @brief Set process noise standard deviation
   /// @param sigma Process noise standard deviation
   void setSigma(float32_t const sigma) { m_processNoiseVec[0] = sigma; }

src/motion_model/ct_motion_model.h

@@ -44,10 +44,6 @@ class CtMotionModel : public MotionModel<CtMotionModel, DIM_X_CT>
   Matrix<DIM_X_CT, DIM_X_CT> getJacobianFk(Vector<DIM_X_CT> const& vecX,
                                            float32_t dt = 1.0F) const;
 
-  /// @brief Get state dimension
-  /// @return State dimension
-  static constexpr int32_t getStateDim() { return DIM_X_CT; }
-
   /// @brief Set velocity process noise standard deviation.
   /// @param sigmaV Velocity process noise standard deviation
   void setSigmaV(float32_t const sigmaV) { m_processNoiseVec[0] = sigmaV; }

src/motion_model/cv_motion_model.h

@@ -42,10 +42,6 @@ class CvMotionModel : public MotionModel<CvMotionModel, DIM_X_CV>
   Matrix<DIM_X_CV, DIM_X_CV> getJacobianFk(Vector<DIM_X_CV> const& vecX,
                                            float32_t dt = 1.0F) const;
 
-  /// @brief Get state dimension
-  /// @return State dimension
-  static constexpr int32_t getStateDim() { return DIM_X_CV; }
-
   /// @brief Set process noise standard deviation
   /// @param sigma Process noise standard deviation
   void setSigma(float32_t const sigma) { m_processNoiseVec[0] = sigma; }