Skip to content

Commit 7c4ea5b

Browse files
Al-khwarizmi-780Al-khwarizmi-780
authored
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
1 parent 1119fcc commit 7c4ea5b

10 files changed

Lines changed: 339 additions & 15 deletions

File tree

‎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: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
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_Z_BEARING>
19+
{
20+
public:
21+
BearingMeasModel(Vector<2> const& sensPos2D, float32_t const measNoise)
22+
: m_sensPos2D{sensPos2D}, m_measNoiseSigma{measNoise}
23+
{
24+
}
25+
~BearingMeasModel() {}
26+
27+
static constexpr int32_t IDX_X_PX{0}; //< Index for position x in state
28+
static constexpr int32_t IDX_X_PY{1}; //< Index for position y in state
29+
30+
static constexpr int32_t IDX_Z_BEARING{
31+
0}; //< Index for bearing in measurement
32+
33+
/// @brief Measurement model function that maps the state space vector to
34+
/// the measurement space.
35+
/// @param vecX State space vector \vec{x}
36+
/// @return Measurement space vector \vec{z}
37+
Vector<DIM_Z_BEARING> h(Vector<DIM_X> const& vecX) const
38+
{
39+
Vector<DIM_Z_BEARING> vecZ;
40+
float32_t px = vecX(IDX_X_PX) - m_sensPos2D(IDX_X_PX);
41+
float32_t py = vecX(IDX_X_PY) - m_sensPos2D(IDX_X_PY);
42+
vecZ(IDX_Z_BEARING) = std::atan2(py, px);
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_BEARING, DIM_X> getJacobianHk(Vector<DIM_X> const& vecX) const
50+
{
51+
Matrix<DIM_Z_BEARING, DIM_X> matH;
52+
matH.setZero();
53+
54+
float32_t const px{vecX(IDX_X_PX) - m_sensPos2D(IDX_X_PX)};
55+
float32_t const py{vecX(IDX_X_PY) - m_sensPos2D(IDX_X_PY)};
56+
float32_t const denom{px * px + py * py};
57+
58+
matH(IDX_Z_BEARING, IDX_X_PX) = -py / denom;
59+
matH(IDX_Z_BEARING, IDX_X_PY) = px / denom;
60+
return matH;
61+
}
62+
63+
/// @brief Get the measurement noise covariance R
64+
/// @return The measurement noise covariance R
65+
Matrix<DIM_Z_BEARING, DIM_Z_BEARING> getMeasurementNoiseCov() const
66+
{
67+
float32_t const measNoiseSigma2{m_measNoiseSigma * m_measNoiseSigma};
68+
Matrix<DIM_Z_BEARING, DIM_Z_BEARING> matR;
69+
matR.setZero();
70+
matR(IDX_Z_BEARING, IDX_Z_BEARING) = measNoiseSigma2;
71+
return matR;
72+
}
73+
74+
/// @brief Set measurement noise standard deviation
75+
/// @param sigma Measurement noise standard deviation
76+
void setMeasurementNoiseSigma(float32_t const sigma)
77+
{
78+
m_measNoiseSigma = sigma;
79+
}
80+
81+
private:
82+
Vector<2> m_sensPos2D; //< Sensor position in 2D
83+
float32_t m_measNoiseSigma; //< Measurement noise vector
84+
};
85+
} // namespace measmodel
86+
87+
} // namespace kf
88+
#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: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
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 : public MeasModel<Pos2dMeasModel, DIM_X, DIM_Z_POS2D>
19+
{
20+
public:
21+
Pos2dMeasModel(float32_t const measNoise) : m_measNoiseSigma{measNoise} {}
22+
~Pos2dMeasModel() {}
23+
24+
static constexpr int32_t IDX_X_PX{0}; //< Index for position x in state
25+
static constexpr int32_t IDX_X_PY{1}; //< Index for position y in state
26+
27+
static constexpr int32_t IDX_Z_PX{0}; //< Index for position x in measurement
28+
static constexpr int32_t IDX_Z_PY{1}; //< Index for position y in measurement
29+
30+
/// @brief Measurement model function that maps the state space vector to
31+
/// the measurement space.
32+
/// @param vecX State space vector \vec{x}
33+
/// @return Measurement space vector \vec{z}
34+
Vector<DIM_Z_POS2D> h(Vector<DIM_X> const& vecX) const
35+
{
36+
Vector<DIM_Z_POS2D> vecZ;
37+
vecZ(IDX_Z_PX) = vecX(IDX_X_PX);
38+
vecZ(IDX_Z_PY) = vecX(IDX_X_PY);
39+
return vecZ;
40+
}
41+
42+
/// @brief Method that calculates the jacobians of the measurement model.
43+
/// @param vecX State Space vector \vec{x}
44+
/// @return The jacobians of the measurement model.
45+
Matrix<DIM_Z_POS2D, DIM_X> getJacobianHk(Vector<DIM_X> const& vecX) const
46+
{
47+
// H = [1 0 0 0 0;
48+
// 0 1 0 0 0];
49+
Matrix<DIM_Z_POS2D, DIM_X> matH;
50+
matH.setZero();
51+
matH(IDX_Z_PX, IDX_X_PX) = 1.0f;
52+
matH(IDX_Z_PY, IDX_X_PY) = 1.0f;
53+
return matH;
54+
}
55+
56+
/// @brief Get the measurement noise covariance R
57+
/// @return The measurement noise covariance R
58+
Matrix<DIM_Z_POS2D, DIM_Z_POS2D> getMeasurementNoiseCov() const
59+
{
60+
float32_t const measNoiseSigma2{m_measNoiseSigma * m_measNoiseSigma};
61+
Matrix<DIM_Z_POS2D, DIM_Z_POS2D> matR;
62+
matR.setZero();
63+
matR(IDX_Z_PX, IDX_Z_PX) = measNoiseSigma2;
64+
matR(IDX_Z_PY, IDX_Z_PY) = measNoiseSigma2;
65+
return matR;
66+
}
67+
68+
/// @brief Set measurement noise standard deviation
69+
/// @param sigma Measurement noise standard deviation
70+
void setMeasurementNoiseSigma(float32_t const sigma)
71+
{
72+
m_measNoiseSigma = sigma;
73+
}
74+
75+
private:
76+
float32_t m_measNoiseSigma; //< Measurement noise vector
77+
};
78+
} // namespace measmodel
79+
80+
} // namespace kf
81+
#endif // OPENKF_POS2D_MEAS_MODEL_H

‎src/meas_model/range_bearing_meas_model.h‎

Lines changed: 107 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,107 @@
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_Z_BEARING>
19+
{
20+
public:
21+
RangeBearingMeasModel(Vector<2> const& sensPos2D, float32_t const rangeSigma,
22+
float32_t const bearingSigma)
23+
: m_sensPos2D{sensPos2D}, m_rangeNoiseSigma{rangeSigma},
24+
m_bearingNoiseSigma{bearingSigma}
25+
{
26+
}
27+
~BearingMeasModel() {}
28+
29+
static constexpr int32_t IDX_X_PX{0}; //< Index for position x in state
30+
static constexpr int32_t IDX_X_PY{1}; //< Index for position y in state
31+
32+
static constexpr int32_t IDX_Z_RANGE{0}; //< Index for range in measurement
33+
static constexpr int32_t IDX_Z_BEARING{
34+
1}; //< Index for bearing in measurement
35+
36+
/// @brief Measurement model function that maps the state space vector to
37+
/// the measurement space.
38+
/// @param vecX State space vector \vec{x}
39+
/// @return Measurement space vector \vec{z}
40+
Vector<DIM_Z_BEARING> h(Vector<DIM_X> const& vecX) const
41+
{
42+
Vector<DIM_Z_BEARING> vecZ;
43+
float32_t const px{vecX(IDX_X_PX) - m_sensPos2D(IDX_X_PX)};
44+
float32_t const py{vecX(IDX_X_PY) - m_sensPos2D(IDX_X_PY)};
45+
vecZ(IDX_Z_RANGE) = std::sqrt(px * px + py * py);
46+
vecZ(IDX_Z_BEARING) = std::atan2(py, px);
47+
return vecZ;
48+
}
49+
50+
/// @brief Method that calculates the jacobians of the measurement model.
51+
/// @param vecX State Space vector \vec{x}
52+
/// @return The jacobians of the measurement model.
53+
Matrix<DIM_Z_BEARING, DIM_X> getJacobianHk(Vector<DIM_X> const& vecX) const
54+
{
55+
Matrix<DIM_Z_BEARING, DIM_X> matH;
56+
matH.setZero();
57+
58+
float32_t const px{vecX(IDX_X_PX) - m_sensPos2D(IDX_X_PX)};
59+
float32_t const py{vecX(IDX_X_PY) - m_sensPos2D(IDX_X_PY)};
60+
float32_t const denom{px * px + py * py};
61+
62+
matH(IDX_Z_RANGE, IDX_X_PX) = px / std::sqrt(denom);
63+
matH(IDX_Z_RANGE, IDX_X_PY) = py / std::sqrt(denom);
64+
matH(IDX_Z_BEARING, IDX_X_PX) = -py / denom;
65+
matH(IDX_Z_BEARING, IDX_X_PY) = px / denom;
66+
return matH;
67+
}
68+
69+
/// @brief Get the measurement noise covariance R
70+
/// @return The measurement noise covariance R
71+
Matrix<DIM_Z_BEARING, DIM_Z_BEARING> getMeasurementNoiseCov() const
72+
{
73+
float32_t const rangeNoiseSigma2{m_rangeNoiseSigma * m_rangeNoiseSigma};
74+
float32_t const bearingNoiseSigma2{m_bearingNoiseSigma *
75+
m_bearingNoiseSigma};
76+
77+
Matrix<DIM_Z_BEARING, DIM_Z_BEARING> matR;
78+
matR.setZero();
79+
matR(IDX_Z_RANGE, IDX_Z_RANGE) = rangeNoiseSigma2;
80+
matR(IDX_Z_BEARING, IDX_Z_BEARING) = bearingNoiseSigma2;
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_rangeNoiseSigma = 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_bearingNoiseSigma = bearingSigma;
96+
}
97+
98+
private:
99+
Vector<2> m_sensPos2D; //< Sensor position in 2D
100+
float32_t m_rangeNoiseSigma; //< Range measurement noise standard deviation
101+
float32_t
102+
m_bearingNoiseSigma; //< Bearing measurement noise standard deviation
103+
};
104+
} // namespace measmodel
105+
106+
} // namespace kf
107+
#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; }

‎src/motion_model/motion_model.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,10 @@ template <class Derived, int32_t DIM_X>
1414
class MotionModel
1515
{
1616
public:
17+
/// @brief Get the state space vector dimension
18+
/// @return The state space vector dimension
19+
int32_t getDimX() const { return DIM_X; }
20+
1721
/// @brief Prediction motion model function that propagate the previous state
1822
/// to next state in time.
1923
/// @param vecX State space vector \vec{x}

0 commit comments

Comments
 (0)