Skip to content

Commit 6eb5959

Browse files
Al-khwarizmi-780Al-khwarizmi-780
authored
Feature/adding measurement models (#22)
* Feature/tracking motion models (#20) * adding CV motion model * fix build error * Add CA motion model * clean up * add CT motion model class * improve process noise covariance * improve process vector setting * adding tests for CV motion model * Add tests for CA motion model * Add tests for CT motion model * improve setting of sigma * Add parameterized tests * move getDimX inside base class of motion model * Adding measurement models * fix class name * fixing templates of measmodel and adding tests * update clang-format-check.yml for clang-format 21.1.0 * Revert "update clang-format-check.yml for clang-format 21.1.0" This reverts commit cd292e2. * Use github action with jidicula/clang-format-action@v21.1.0 * fix clang-format action * add senspos to pos2d measmodel
1 parent 1119fcc commit 6eb5959

15 files changed

Lines changed: 539 additions & 16 deletions

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ jobs:
1616
- name: Run clang-format style check for C/C++/Protobuf programs.
1717
uses: jidicula/clang-format-action@v4.11.0
1818
with:
19-
clang-format-version: '13'
19+
clang-format-version: '21'
2020
check-path: ${{ matrix.path['check'] }}
2121
exclude-regex: ${{ matrix.path['exclude'] }}
2222
fallback-style: 'Microsoft' # optional

‎src/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,15 +27,21 @@ set(LIBRARY_SRC_FILES
2727
set(LIBRARY_HDR_FILES
2828
types.h
2929
util.h
30+
# Kalman filter files.
3031
kalman_filter/kalman_filter.h
3132
kalman_filter/unscented_transform.h
3233
kalman_filter/unscented_kalman_filter.h
3334
kalman_filter/square_root_ukf.h
35+
# Motion model files.
3436
motion_model/motion_model.h
3537
motion_model/ego_motion_model.h
3638
motion_model/cv_motion_model.h
3739
motion_model/ca_motion_model.h
3840
motion_model/ct_motion_model.h
41+
# Measurement model files.
42+
meas_model/meas_model.h
43+
meas_model/range_bearing_meas_model.h
44+
meas_model/bearing_meas_model.h
3945
)
4046

4147
set(LIBRARY_NAME ${PROJECT_NAME})

‎src/meas_model/bearing_meas_model.h‎

Lines changed: 87 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
#ifndef OPENKF_BEARING_MEAS_MODEL_H
2+
#define OPENKF_BEARING_MEAS_MODEL_H
3+
4+
#include "meas_model.h"
5+
#include "types.h"
6+
7+
namespace kf
8+
{
9+
namespace measmodel
10+
{
11+
/// @brief Measurement space dimension for bearing measurement
12+
/// model
13+
/// \vec{z}=[pos_x, pos_y]^T
14+
static constexpr int32_t DIM_Z_BEARING{2};
15+
16+
template <int32_t DIM_X>
17+
class BearingMeasModel
18+
: public MeasModel<BearingMeasModel<DIM_X>, DIM_X, DIM_Z_BEARING>
19+
{
20+
public:
21+
BearingMeasModel(Vector<2> const& sensPos2D,
22+
float32_t const bearingSigma = 1.0F)
23+
: m_sensPos2D{sensPos2D}, m_bearingNoiseSigma{bearingSigma}
24+
{
25+
}
26+
~BearingMeasModel() {}
27+
28+
static constexpr int32_t IDX_X_PX{0}; //< Index for position x in state
29+
static constexpr int32_t IDX_X_PY{1}; //< Index for position y in state
30+
31+
static constexpr int32_t IDX_Z_BEARING{
32+
0}; //< Index for bearing in measurement
33+
34+
/// @brief Measurement model function that maps the state space vector to
35+
/// the measurement space.
36+
/// @param vecX State space vector \vec{x}
37+
/// @return Measurement space vector \vec{z}
38+
Vector<DIM_Z_BEARING> h(Vector<DIM_X> const& vecX) const
39+
{
40+
Vector<DIM_Z_BEARING> vecZ;
41+
float32_t px = vecX(IDX_X_PX) - m_sensPos2D(IDX_X_PX);
42+
float32_t py = vecX(IDX_X_PY) - m_sensPos2D(IDX_X_PY);
43+
vecZ(IDX_Z_BEARING) = std::atan2(py, px);
44+
return vecZ;
45+
}
46+
47+
/// @brief Method that calculates the jacobians of the measurement model.
48+
/// @param vecX State Space vector \vec{x}
49+
/// @return The jacobians of the measurement model.
50+
Matrix<DIM_Z_BEARING, DIM_X> getJacobianHk(Vector<DIM_X> const& vecX) const
51+
{
52+
Matrix<DIM_Z_BEARING, DIM_X> matH;
53+
matH.setZero();
54+
55+
float32_t const px{vecX(IDX_X_PX) - m_sensPos2D(IDX_X_PX)};
56+
float32_t const py{vecX(IDX_X_PY) - m_sensPos2D(IDX_X_PY)};
57+
float32_t const denom{px * px + py * py};
58+
59+
matH(IDX_Z_BEARING, IDX_X_PX) = -py / denom;
60+
matH(IDX_Z_BEARING, IDX_X_PY) = px / denom;
61+
return matH;
62+
}
63+
64+
/// @brief Get the measurement noise covariance R
65+
/// @return The measurement noise covariance R
66+
Matrix<DIM_Z_BEARING, DIM_Z_BEARING> getMeasurementNoiseCov() const
67+
{
68+
float32_t const bearingNoiseSigma2{m_bearingNoiseSigma *
69+
m_bearingNoiseSigma};
70+
Matrix<DIM_Z_BEARING, DIM_Z_BEARING> matR;
71+
matR.setZero();
72+
matR(IDX_Z_BEARING, IDX_Z_BEARING) = bearingNoiseSigma2;
73+
return matR;
74+
}
75+
76+
/// @brief Set bearing noise standard deviation
77+
/// @param sigma Bearing noise standard deviation
78+
void setBearingSigma(float32_t const sigma) { m_bearingNoiseSigma = sigma; }
79+
80+
private:
81+
Vector<2> m_sensPos2D; //< Sensor position in 2D
82+
float32_t m_bearingNoiseSigma; //< Bearing measurement noise vector
83+
};
84+
} // namespace measmodel
85+
86+
} // namespace kf
87+
#endif // OPENKF_BEARING_MEAS_MODEL_H

‎src/meas_model/meas_model.h‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
#ifndef OPENKF_MEAS_MODEL_H
2+
#define OPENKF_MEAS_MODEL_H
3+
4+
#include "types.h"
5+
6+
namespace kf
7+
{
8+
9+
namespace measmodel
10+
{
11+
/// @brief Base class for measurement models used by kalman filters
12+
/// @tparam DIM_X State space vector dimension
13+
/// @tparam DIM_Z Measurement space vector dimension
14+
template <class Derived, int32_t DIM_X, int32_t DIM_Z>
15+
class MeasModel
16+
{
17+
public:
18+
/// @brief Get the measurement space vector dimension
19+
/// @return The measurement space vector dimension
20+
int32_t getDimZ() const { return DIM_Z; }
21+
22+
/// @brief Measurement model function that maps the state space vector to
23+
/// the measurement space.
24+
/// @param vecX State space vector \vec{x}
25+
/// @return Measurement space vector \vec{z}
26+
Vector<DIM_Z> h(Vector<DIM_X> const& vecX) const
27+
{
28+
return static_cast<Derived const*>(this)->h(vecX);
29+
}
30+
31+
/// @brief Method that calculates the jacobians of the measurement model.
32+
/// @param vecX State Space vector \vec{x}
33+
/// @return The jacobians of the measurement model.
34+
Matrix<DIM_Z, DIM_X> getJacobianHk(Vector<DIM_X> const& vecX) const
35+
{
36+
return static_cast<Derived const*>(this)->getJacobianHk(vecX);
37+
}
38+
39+
/// @brief Get the measurement noise covariance R
40+
/// @return The measurement noise covariance R
41+
Matrix<DIM_Z, DIM_Z> getMeasurementNoiseCov() const
42+
{
43+
return static_cast<Derived const*>(this)->getMeasurementNoiseCov();
44+
}
45+
};
46+
} // namespace measmodel
47+
48+
} // namespace kf
49+
50+
#endif // OPENKF_MEAS_MODEL_H

‎src/meas_model/pos2d_meas_model.h‎

Lines changed: 83 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,83 @@
1+
#ifndef OPENKF_POS2D_MEAS_MODEL_H
2+
#define OPENKF_POS2D_MEAS_MODEL_H
3+
4+
#include "meas_model.h"
5+
#include "types.h"
6+
7+
namespace kf
8+
{
9+
namespace measmodel
10+
{
11+
/// @brief Measurement space dimension for 2D position measurement model
12+
/// \vec{z}=[pos_x, pos_y]^T
13+
static constexpr int32_t DIM_Z_POS2D{2};
14+
15+
/// @brief measurement model
16+
/// @tparam DIM_X State space vector dimension \vec{x}=[pos_x, pos_y, ...]^T
17+
template <int32_t DIM_X>
18+
class Pos2dMeasModel
19+
: public MeasModel<Pos2dMeasModel<DIM_X>, DIM_X, DIM_Z_POS2D>
20+
{
21+
public:
22+
Pos2dMeasModel(Vector<2> const& sensPos, float32_t const posSigma = 1.0F)
23+
: m_sensPos{sensPos}, m_posSigma{posSigma}
24+
{
25+
}
26+
~Pos2dMeasModel() {}
27+
28+
static constexpr int32_t IDX_X_PX{0}; //< Index for position x in state
29+
static constexpr int32_t IDX_X_PY{1}; //< Index for position y in state
30+
31+
static constexpr int32_t IDX_Z_PX{0}; //< Index for position x in measurement
32+
static constexpr int32_t IDX_Z_PY{1}; //< Index for position y in measurement
33+
34+
/// @brief Measurement model function that maps the state space vector to
35+
/// the measurement space.
36+
/// @param vecX State space vector \vec{x}
37+
/// @return Measurement space vector \vec{z}
38+
Vector<DIM_Z_POS2D> h(Vector<DIM_X> const& vecX) const
39+
{
40+
Vector<DIM_Z_POS2D> vecZ;
41+
vecZ(IDX_Z_PX) = vecX(IDX_X_PX) - m_sensPos(0);
42+
vecZ(IDX_Z_PY) = vecX(IDX_X_PY) - m_sensPos(1);
43+
return vecZ;
44+
}
45+
46+
/// @brief Method that calculates the jacobians of the measurement model.
47+
/// @param vecX State Space vector \vec{x}
48+
/// @return The jacobians of the measurement model.
49+
Matrix<DIM_Z_POS2D, DIM_X> getJacobianHk(Vector<DIM_X> const& vecX) const
50+
{
51+
// H = [1 0 0 0 0;
52+
// 0 1 0 0 0];
53+
Matrix<DIM_Z_POS2D, DIM_X> matH;
54+
matH.setZero();
55+
matH(IDX_Z_PX, IDX_X_PX) = 1.0f;
56+
matH(IDX_Z_PY, IDX_X_PY) = 1.0f;
57+
return matH;
58+
}
59+
60+
/// @brief Get the measurement noise covariance R
61+
/// @return The measurement noise covariance R
62+
Matrix<DIM_Z_POS2D, DIM_Z_POS2D> getMeasurementNoiseCov() const
63+
{
64+
float32_t const posNoiseSigma2{m_posSigma * m_posSigma};
65+
Matrix<DIM_Z_POS2D, DIM_Z_POS2D> matR;
66+
matR.setZero();
67+
matR(IDX_Z_PX, IDX_Z_PX) = posNoiseSigma2;
68+
matR(IDX_Z_PY, IDX_Z_PY) = posNoiseSigma2;
69+
return matR;
70+
}
71+
72+
/// @brief Set position noise standard deviation
73+
/// @param sigma Position noise standard deviation
74+
void setPosSigma(float32_t const sigma) { m_posSigma = sigma; }
75+
76+
private:
77+
Vector<2> m_sensPos; //< Sensor position in 2D space
78+
float32_t m_posSigma; //< Measurement noise vector
79+
};
80+
} // namespace measmodel
81+
82+
} // namespace kf
83+
#endif // OPENKF_POS2D_MEAS_MODEL_H

‎src/meas_model/range_bearing_meas_model.h‎

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
#ifndef OPENKF_RANGE_BEARING_MEAS_MODEL_H
2+
#define OPENKF_RANGE_BEARING_MEAS_MODEL_H
3+
4+
#include "meas_model.h"
5+
#include "types.h"
6+
7+
namespace kf
8+
{
9+
namespace measmodel
10+
{
11+
/// @brief Measurement space dimension for range-bearing measurement
12+
/// model
13+
/// \vec{z}=[pos_x, pos_y]^T
14+
static constexpr int32_t DIM_Z_BEARING{2};
15+
16+
template <int32_t DIM_X>
17+
class RangeBearingMeasModel
18+
: public MeasModel<RangeBearingMeasModel<DIM_X>, DIM_X, DIM_Z_BEARING>
19+
{
20+
public:
21+
RangeBearingMeasModel(Vector<2> const& sensPos2D,
22+
float32_t const rangeSigma = 1.0F,
23+
float32_t const bearingSigma = 1.0F)
24+
: m_sensPos2D{sensPos2D}, m_rangeSigma{rangeSigma},
25+
m_bearingSigma{bearingSigma}
26+
{
27+
}
28+
~RangeBearingMeasModel() {}
29+
30+
static constexpr int32_t IDX_X_PX{0}; //< Index for position x in state
31+
static constexpr int32_t IDX_X_PY{1}; //< Index for position y in state
32+
33+
static constexpr int32_t IDX_Z_RANGE{0}; //< Index for range in measurement
34+
static constexpr int32_t IDX_Z_BEARING{
35+
1}; //< Index for bearing in measurement
36+
37+
/// @brief Measurement model function that maps the state space vector to
38+
/// the measurement space.
39+
/// @param vecX State space vector \vec{x}
40+
/// @return Measurement space vector \vec{z}
41+
Vector<DIM_Z_BEARING> h(Vector<DIM_X> const& vecX) const
42+
{
43+
Vector<DIM_Z_BEARING> vecZ;
44+
float32_t const px{vecX(IDX_X_PX) - m_sensPos2D(IDX_X_PX)};
45+
float32_t const py{vecX(IDX_X_PY) - m_sensPos2D(IDX_X_PY)};
46+
vecZ(IDX_Z_RANGE) = std::sqrt(px * px + py * py);
47+
vecZ(IDX_Z_BEARING) = std::atan2(py, px);
48+
return vecZ;
49+
}
50+
51+
/// @brief Method that calculates the jacobians of the measurement model.
52+
/// @param vecX State Space vector \vec{x}
53+
/// @return The jacobians of the measurement model.
54+
Matrix<DIM_Z_BEARING, DIM_X> getJacobianHk(Vector<DIM_X> const& vecX) const
55+
{
56+
Matrix<DIM_Z_BEARING, DIM_X> matH;
57+
matH.setZero();
58+
59+
float32_t const px{vecX(IDX_X_PX) - m_sensPos2D(IDX_X_PX)};
60+
float32_t const py{vecX(IDX_X_PY) - m_sensPos2D(IDX_X_PY)};
61+
float32_t const denom{px * px + py * py};
62+
63+
matH(IDX_Z_RANGE, IDX_X_PX) = px / std::sqrt(denom);
64+
matH(IDX_Z_RANGE, IDX_X_PY) = py / std::sqrt(denom);
65+
matH(IDX_Z_BEARING, IDX_X_PX) = -py / denom;
66+
matH(IDX_Z_BEARING, IDX_X_PY) = px / denom;
67+
return matH;
68+
}
69+
70+
/// @brief Get the measurement noise covariance R
71+
/// @return The measurement noise covariance R
72+
Matrix<DIM_Z_BEARING, DIM_Z_BEARING> getMeasurementNoiseCov() const
73+
{
74+
float32_t const rangeSigma2{m_rangeSigma * m_rangeSigma};
75+
float32_t const bearingSigma2{m_bearingSigma * m_bearingSigma};
76+
77+
Matrix<DIM_Z_BEARING, DIM_Z_BEARING> matR;
78+
matR.setZero();
79+
matR(IDX_Z_RANGE, IDX_Z_RANGE) = rangeSigma2;
80+
matR(IDX_Z_BEARING, IDX_Z_BEARING) = bearingSigma2;
81+
return matR;
82+
}
83+
84+
/// @brief Set range noise standard deviation
85+
/// @param rangeSigma Range measurement noise standard deviation
86+
void setRangeNoiseSigma(float32_t const rangeSigma)
87+
{
88+
m_rangeSigma = rangeSigma;
89+
}
90+
91+
/// @brief Set bearing noise standard deviation
92+
/// @param bearingSigma Bearing measurement noise standard deviation
93+
void setBearingNoiseSigma(float32_t const bearingSigma)
94+
{
95+
m_bearingSigma = bearingSigma;
96+
}
97+
98+
private:
99+
Vector<2> m_sensPos2D; //< Sensor position in 2D
100+
float32_t m_rangeSigma; //< Range measurement noise standard deviation
101+
float32_t m_bearingSigma; //< Bearing measurement noise standard deviation
102+
};
103+
} // namespace measmodel
104+
105+
} // namespace kf
106+
#endif // OPENKF_RANGE_BEARING_MEAS_MODEL_H

‎src/motion_model/ca_motion_model.h‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -39,10 +39,6 @@ class CaMotionModel : public MotionModel<CaMotionModel, DIM_X_CA>
3939
Matrix<DIM_X_CA, DIM_X_CA> getJacobianFk(Vector<DIM_X_CA> const& vecX,
4040
float32_t dt = 1.0F) const;
4141

42-
/// @brief Get state dimension
43-
/// @return State dimension
44-
static constexpr int32_t getStateDim() { return DIM_X_CA; }
45-
4642
/// @brief Set process noise standard deviation
4743
/// @param sigma Process noise standard deviation
4844
void setSigma(float32_t const sigma) { m_processNoiseVec[0] = sigma; }

‎src/motion_model/ct_motion_model.h‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -44,10 +44,6 @@ class CtMotionModel : public MotionModel<CtMotionModel, DIM_X_CT>
4444
Matrix<DIM_X_CT, DIM_X_CT> getJacobianFk(Vector<DIM_X_CT> const& vecX,
4545
float32_t dt = 1.0F) const;
4646

47-
/// @brief Get state dimension
48-
/// @return State dimension
49-
static constexpr int32_t getStateDim() { return DIM_X_CT; }
50-
5147
/// @brief Set velocity process noise standard deviation.
5248
/// @param sigmaV Velocity process noise standard deviation
5349
void setSigmaV(float32_t const sigmaV) { m_processNoiseVec[0] = sigmaV; }

‎src/motion_model/cv_motion_model.h‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -42,10 +42,6 @@ class CvMotionModel : public MotionModel<CvMotionModel, DIM_X_CV>
4242
Matrix<DIM_X_CV, DIM_X_CV> getJacobianFk(Vector<DIM_X_CV> const& vecX,
4343
float32_t dt = 1.0F) const;
4444

45-
/// @brief Get state dimension
46-
/// @return State dimension
47-
static constexpr int32_t getStateDim() { return DIM_X_CV; }
48-
4945
/// @brief Set process noise standard deviation
5046
/// @param sigma Process noise standard deviation
5147
void setSigma(float32_t const sigma) { m_processNoiseVec[0] = sigma; }

0 commit comments

Comments
 (0)