diff --git a/.github/workflows/clang-format-check.yml b/.github/workflows/clang-format-check.yml index a69528f..cf2f065 100644 --- a/.github/workflows/clang-format-check.yml +++ b/.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 \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index fb34e3b..da9c1c1 100644 --- a/src/CMakeLists.txt +++ b/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}) diff --git a/src/meas_model/bearing_meas_model.h b/src/meas_model/bearing_meas_model.h new file mode 100644 index 0000000..fde394a --- /dev/null +++ b/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 +class BearingMeasModel + : public MeasModel, 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 h(Vector const& vecX) const + { + Vector 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 getJacobianHk(Vector const& vecX) const + { + Matrix 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 getMeasurementNoiseCov() const + { + float32_t const bearingNoiseSigma2{m_bearingNoiseSigma * + m_bearingNoiseSigma}; + Matrix 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 diff --git a/src/meas_model/meas_model.h b/src/meas_model/meas_model.h new file mode 100644 index 0000000..988f00f --- /dev/null +++ b/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 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 h(Vector const& vecX) const + { + return static_cast(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 getJacobianHk(Vector const& vecX) const + { + return static_cast(this)->getJacobianHk(vecX); + } + + /// @brief Get the measurement noise covariance R + /// @return The measurement noise covariance R + Matrix getMeasurementNoiseCov() const + { + return static_cast(this)->getMeasurementNoiseCov(); + } +}; +} // namespace measmodel + +} // namespace kf + +#endif // OPENKF_MEAS_MODEL_H diff --git a/src/meas_model/pos2d_meas_model.h b/src/meas_model/pos2d_meas_model.h new file mode 100644 index 0000000..fcf5253 --- /dev/null +++ b/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 +class Pos2dMeasModel + : public MeasModel, 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 h(Vector const& vecX) const + { + Vector 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 getJacobianHk(Vector const& vecX) const + { + // H = [1 0 0 0 0; + // 0 1 0 0 0]; + Matrix 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 getMeasurementNoiseCov() const + { + float32_t const posNoiseSigma2{m_posSigma * m_posSigma}; + Matrix 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 diff --git a/src/meas_model/range_bearing_meas_model.h b/src/meas_model/range_bearing_meas_model.h new file mode 100644 index 0000000..0c22365 --- /dev/null +++ b/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 +class RangeBearingMeasModel + : public MeasModel, 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 h(Vector const& vecX) const + { + Vector 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 getJacobianHk(Vector const& vecX) const + { + Matrix 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 getMeasurementNoiseCov() const + { + float32_t const rangeSigma2{m_rangeSigma * m_rangeSigma}; + float32_t const bearingSigma2{m_bearingSigma * m_bearingSigma}; + + Matrix 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 diff --git a/src/motion_model/ca_motion_model.h b/src/motion_model/ca_motion_model.h index 3fe9dd0..f154d73 100644 --- a/src/motion_model/ca_motion_model.h +++ b/src/motion_model/ca_motion_model.h @@ -39,10 +39,6 @@ class CaMotionModel : public MotionModel Matrix getJacobianFk(Vector 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; } diff --git a/src/motion_model/ct_motion_model.h b/src/motion_model/ct_motion_model.h index 35c3443..6dd105b 100644 --- a/src/motion_model/ct_motion_model.h +++ b/src/motion_model/ct_motion_model.h @@ -44,10 +44,6 @@ class CtMotionModel : public MotionModel Matrix getJacobianFk(Vector 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; } diff --git a/src/motion_model/cv_motion_model.h b/src/motion_model/cv_motion_model.h index 871ecbd..c043a1a 100644 --- a/src/motion_model/cv_motion_model.h +++ b/src/motion_model/cv_motion_model.h @@ -42,10 +42,6 @@ class CvMotionModel : public MotionModel Matrix getJacobianFk(Vector 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; } diff --git a/src/motion_model/motion_model.h b/src/motion_model/motion_model.h index d54a36b..716bf29 100644 --- a/src/motion_model/motion_model.h +++ b/src/motion_model/motion_model.h @@ -14,6 +14,10 @@ template class MotionModel { public: + /// @brief Get the state space vector dimension + /// @return The state space vector dimension + int32_t getDimX() const { return DIM_X; } + /// @brief Prediction motion model function that propagate the previous state /// to next state in time. /// @param vecX State space vector \vec{x} diff --git a/tests/unit/CMakeLists.txt b/tests/unit/CMakeLists.txt index af5c980..5d2f41e 100644 --- a/tests/unit/CMakeLists.txt +++ b/tests/unit/CMakeLists.txt @@ -20,6 +20,9 @@ add_executable(${TEST_APP_NAME} motion_model/ca_motion_model_test.cpp motion_model/ct_motion_model_test.cpp motion_model/param_motion_model_test.cpp + meas_model/pos2d_meas_model_test.cpp + meas_model/bearing_meas_model_test.cpp + meas_model/range_bearing_meas_model_test.cpp ) target_link_libraries(${TEST_APP_NAME} PRIVATE diff --git a/tests/unit/meas_model/bearing_meas_model_test.cpp b/tests/unit/meas_model/bearing_meas_model_test.cpp new file mode 100644 index 0000000..02dea0e --- /dev/null +++ b/tests/unit/meas_model/bearing_meas_model_test.cpp @@ -0,0 +1,59 @@ +#include "meas_model/bearing_meas_model.h" +#include "motion_model/cv_motion_model.h" +#include "types.h" +#include + +#define _USE_MATH_DEFINES +#include + +using namespace kf; +using namespace kf::motionmodel; +using namespace kf::measmodel; + +class BearingMeasModelTest : public testing::Test +{ + protected: + virtual void SetUp() override { m_initialState << 10.0, 20.0, 5.0, -2.0; } + virtual void TearDown() override {} + + static constexpr int32_t DIM_X{motionmodel::DIM_X_CV}; + static constexpr int32_t DIM_Z{measmodel::DIM_Z_BEARING}; + + Vector m_initialState; + Vector<2> m_sensPos2D{0.0F, 0.0F}; + measmodel::BearingMeasModel m_bearingMeasModel{m_sensPos2D, 0.2F}; +}; + +TEST_F(BearingMeasModelTest, test_MeasurementFunction) +{ + Vector const vecZMeas{m_bearingMeasModel.h(m_initialState)}; + + const float32_t expectedBearing{ + std::atan2(20.0F - 0.0F, 10.0F - 0.0F)}; // atan2(py, px) + + EXPECT_DOUBLE_EQ(vecZMeas(0), expectedBearing); // bearing +} + +TEST_F(BearingMeasModelTest, test_JacobianHk) +{ + Matrix const matH{ + m_bearingMeasModel.getJacobianHk(m_initialState)}; + + const float32_t px{10.0F - 0.0F}; + const float32_t py{20.0F - 0.0F}; + const float32_t denom{px * px + py * py}; + + EXPECT_DOUBLE_EQ(matH(0, 0), -py / denom); // dbearing/dpos_x + EXPECT_DOUBLE_EQ(matH(0, 1), px / denom); // dbearing/dpos_y + EXPECT_DOUBLE_EQ(matH(0, 2), 0.0); // dbearing/dvel_x + EXPECT_DOUBLE_EQ(matH(0, 3), 0.0); // dbearing/dvel_y +} + +TEST_F(BearingMeasModelTest, test_MeasurementNoiseCov) +{ + Matrix const matR{m_bearingMeasModel.getMeasurementNoiseCov()}; + + const float32_t sigma2{0.2F * 0.2F}; + + EXPECT_DOUBLE_EQ(matR(0, 0), sigma2); // var_bearing +} diff --git a/tests/unit/meas_model/pos2d_meas_model_test.cpp b/tests/unit/meas_model/pos2d_meas_model_test.cpp new file mode 100644 index 0000000..24115a8 --- /dev/null +++ b/tests/unit/meas_model/pos2d_meas_model_test.cpp @@ -0,0 +1,66 @@ +#include "meas_model/pos2d_meas_model.h" +#include "motion_model/cv_motion_model.h" +#include "types.h" +#include + +#define _USE_MATH_DEFINES +#include + +using namespace kf; +using namespace kf::motionmodel; +using namespace kf::measmodel; + +class Pos2DMeasModelTest : public testing::Test +{ + protected: + virtual void SetUp() override + { + m_pos2DMeasModel.setPosSigma(0.3F); + m_initialState << 15.0, -10.0, 3.0, 4.0; + } + virtual void TearDown() override {} + + static constexpr int32_t DIM_X{motionmodel::DIM_X_CV}; + static constexpr int32_t DIM_Z{measmodel::DIM_Z_POS2D}; + + Vector m_initialState; + Vector<2> const m_sensPos{1.0F, 2.0F}; + measmodel::Pos2dMeasModel m_pos2DMeasModel{m_sensPos}; +}; + +TEST_F(Pos2DMeasModelTest, test_MeasurementFunction) +{ + Vector const vecZMeas{m_pos2DMeasModel.h(m_initialState)}; + + EXPECT_DOUBLE_EQ(vecZMeas(0), 14.0); // pos_x + EXPECT_DOUBLE_EQ(vecZMeas(1), -12.0); // pos_y +} + +TEST_F(Pos2DMeasModelTest, test_JacobianHk) +{ + Matrix const matH{ + m_pos2DMeasModel.getJacobianHk(m_initialState)}; + + EXPECT_DOUBLE_EQ(matH(0, 0), 1.0); // dpos_x/dpos_x + EXPECT_DOUBLE_EQ(matH(0, 1), 0.0); // dpos_x/dpos_y + EXPECT_DOUBLE_EQ(matH(0, 2), 0.0); // dpos_x/dvel_x + EXPECT_DOUBLE_EQ(matH(0, 3), 0.0); // dpos_x/dvel_y + + EXPECT_DOUBLE_EQ(matH(1, 0), 0.0); // dpos_y/dpos_x + EXPECT_DOUBLE_EQ(matH(1, 1), 1.0); // dpos_y/dpos_y + EXPECT_DOUBLE_EQ(matH(1, 2), 0.0); // dpos_y/dvel_x + EXPECT_DOUBLE_EQ(matH(1, 3), 0.0); // dpos_y/dvel_y +} + +TEST_F(Pos2DMeasModelTest, test_MeasurementNoiseCov) +{ + Matrix const matR{m_pos2DMeasModel.getMeasurementNoiseCov()}; + + const float32_t sigma2{0.3F * 0.3F}; + + EXPECT_DOUBLE_EQ(matR(0, 0), sigma2); // var_pos_x + EXPECT_DOUBLE_EQ(matR(0, 1), 0.0); // cov_pos_xy + + EXPECT_DOUBLE_EQ(matR(1, 0), 0.0); // cov_pos_yx + EXPECT_DOUBLE_EQ(matR(1, 1), sigma2); // var_pos_y +} diff --git a/tests/unit/meas_model/range_bearing_meas_model_test.cpp b/tests/unit/meas_model/range_bearing_meas_model_test.cpp new file mode 100644 index 0000000..585df28 --- /dev/null +++ b/tests/unit/meas_model/range_bearing_meas_model_test.cpp @@ -0,0 +1,71 @@ +#include "meas_model/range_bearing_meas_model.h" +#include "motion_model/cv_motion_model.h" +#include "types.h" +#include + +#define _USE_MATH_DEFINES +#include + +using namespace kf; +using namespace kf::motionmodel; +using namespace kf::measmodel; + +class RangeBearingMeasModelTest : public testing::Test +{ + protected: + virtual void SetUp() override { m_initialState << 10.0, 20.0, 5.0, -2.0; } + virtual void TearDown() override {} + + static constexpr int32_t DIM_X{motionmodel::DIM_X_CV}; + static constexpr int32_t DIM_Z{measmodel::DIM_Z_BEARING}; + + Vector m_initialState; + Vector<2> const sensPos2D{0.0F, 0.0F}; + measmodel::RangeBearingMeasModel m_rangeBearingMeasModel{sensPos2D, + 0.3F, 0.2F}; +}; + +TEST_F(RangeBearingMeasModelTest, test_MeasurementFunction) +{ + Vector const vecZMeas{m_rangeBearingMeasModel.h(m_initialState)}; + + const float32_t expectedRange{ + std::sqrt(10.0F * 10.0F + 20.0F * 20.0F)}; // sqrt(px^2 + py^2) + const float32_t expectedBearing{ + std::atan2(20.0F - 0.0F, 10.0F - 0.0F)}; // atan2(py, px) + + EXPECT_DOUBLE_EQ(vecZMeas(0), expectedRange); // range + EXPECT_DOUBLE_EQ(vecZMeas(1), expectedBearing); // bearing +} + +TEST_F(RangeBearingMeasModelTest, test_JacobianHk) +{ + Matrix const matH{ + m_rangeBearingMeasModel.getJacobianHk(m_initialState)}; + + const float32_t px{10.0F - 0.0F}; + const float32_t py{20.0F - 0.0F}; + const float32_t denom{px * px + py * py}; + + EXPECT_DOUBLE_EQ(matH(0, 0), px / std::sqrt(denom)); // drange/dpos_x + EXPECT_DOUBLE_EQ(matH(0, 1), py / std::sqrt(denom)); // drange/dpos_y + EXPECT_DOUBLE_EQ(matH(0, 2), 0.0); // drange/dvel_x + EXPECT_DOUBLE_EQ(matH(0, 3), 0.0); // drange/dvel_y + + EXPECT_DOUBLE_EQ(matH(1, 0), -py / denom); // dbearing/dpos_x + EXPECT_DOUBLE_EQ(matH(1, 1), px / denom); // dbearing/dpos_y + EXPECT_DOUBLE_EQ(matH(1, 2), 0.0); // dbearing/dvel_x + EXPECT_DOUBLE_EQ(matH(1, 3), 0.0); // dbearing/dvel_y +} + +TEST_F(RangeBearingMeasModelTest, test_MeasurementNoiseCov) +{ + Matrix const matR{ + m_rangeBearingMeasModel.getMeasurementNoiseCov()}; + + const float32_t rangeSigma2{0.3F * 0.3F}; + const float32_t bearingSigma2{0.2F * 0.2F}; + + EXPECT_DOUBLE_EQ(matR(0, 0), rangeSigma2); // var_range + EXPECT_DOUBLE_EQ(matR(1, 1), bearingSigma2); // var_bearing +} diff --git a/tests/unit/motion_model/param_motion_model_test.cpp b/tests/unit/motion_model/param_motion_model_test.cpp index e287365..f950398 100644 --- a/tests/unit/motion_model/param_motion_model_test.cpp +++ b/tests/unit/motion_model/param_motion_model_test.cpp @@ -26,9 +26,9 @@ TEST(MotionModelFactoryTest, CreateAndTestAllModels) m_ctState << 0.0F, 0.0F, 10.0F, static_cast(M_PI) / 4.0F, static_cast(M_PI) / 12.0F; - EXPECT_EQ(cv->getStateDim(), 4); - EXPECT_EQ(ca->getStateDim(), 6); - EXPECT_EQ(ctr->getStateDim(), 5); + EXPECT_EQ(cv->getDimX(), 4); + EXPECT_EQ(ca->getDimX(), 6); + EXPECT_EQ(ctr->getDimX(), 5); // Test that prediction doesn't crash for small dt EXPECT_NO_THROW(cv->f(m_cvState, 0.1F));