Skip to content

Commit b9ea7a1

Browse files
Al-khwarizmi-780Al-khwarizmi-780
committed
Adding measurement models
1 parent a1f48af commit b9ea7a1

5 files changed

Lines changed: 332 additions & 0 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 constant acceleration 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 constant acceleration 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 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

0 commit comments

Comments
 (0)