diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 42bf1ac..8fcad11 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -6,3 +6,5 @@ add_subdirectory(unscented_transform) add_subdirectory(ukf_range_sensor) add_subdirectory(test_least_squares) add_subdirectory(sr_ukf_linear_function) +add_subdirectory(ego_motion_model_adapter) + diff --git a/src/examples/ego_motion_model_adapter/CMakeLists.txt b/src/examples/ego_motion_model_adapter/CMakeLists.txt new file mode 100644 index 0000000..2ab4b1c --- /dev/null +++ b/src/examples/ego_motion_model_adapter/CMakeLists.txt @@ -0,0 +1,31 @@ +## +## Copyright 2022 Mohanad Youssef (Al-khwarizmi) +## +## Use of this source code is governed by an GPL-3.0 - style +## license that can be found in the LICENSE file or at +## https://opensource.org/licenses/GPL-3.0 +## +## @author Mohanad Youssef +## @file CMakeLists.h +## + +file(GLOB PROJECT_FILES + "${CMAKE_CURRENT_SOURCE_DIR}/*.h" + "${CMAKE_CURRENT_SOURCE_DIR}/*.cpp" +) + +set(APPLICATION_NAME ${EXAMPLE_EXECUTABLE_PREFIX}_ego_motion_model_adapter) + +add_executable(${APPLICATION_NAME} ${PROJECT_FILES}) + +set_target_properties(${APPLICATION_NAME} PROPERTIES LINKER_LANGUAGE CXX) + +target_link_libraries(${APPLICATION_NAME} + PUBLIC + Eigen3::Eigen + OpenKF +) + +target_include_directories(${APPLICATION_NAME} PUBLIC + $ +) diff --git a/src/examples/ego_motion_model_adapter/main.cpp b/src/examples/ego_motion_model_adapter/main.cpp new file mode 100644 index 0000000..02f76f5 --- /dev/null +++ b/src/examples/ego_motion_model_adapter/main.cpp @@ -0,0 +1,176 @@ +/// +/// Copyright 2022 Mohanad Youssef (Al-khwarizmi) +/// +/// Use of this source code is governed by an GPL-3.0 - style +/// license that can be found in the LICENSE file or at +/// https://opensource.org/licenses/GPL-3.0 +/// +/// @author Mohanad Youssef +/// @file main.cpp +/// + +#include +#include + +#include "types.h" +#include "motion_model/ego_motion_model.h" +#include "kalman_filter/kalman_filter.h" + +static constexpr size_t DIM_X{ 5 }; /// \vec{x} = [x, y, vx, vy, yaw]^T +static constexpr size_t DIM_U{ 3 }; /// \vec{u} = [steeringAngle, ds, dyaw]^T +static constexpr size_t DIM_Z{ 2 }; + +using namespace kf; + + +/// @brief This is an adapter example to show case how to convert from the 3-dimension state egomotion model +/// to a higher or lower dimension state vector (e.g. 5-dimension state vector and 3-dimension input vector). +class EgoMotionModelAdapter : public motionmodel::MotionModelExtInput +{ +public: + virtual Vector f(Vector const & vecX, Vector const & vecU, Vector const & /*vecQ = Vector::Zero()*/) const override + { + Vector<3> tmpVecX; // \vec{x} = [x, y, yaw]^T + tmpVecX << vecX[0], vecX[1], vecX[4]; + + Vector<2> tmpVecU; // \vec{u} = [ds, dyaw]^T + tmpVecU << vecU[1], vecU[2]; + + motionmodel::EgoMotionModel const egoMotionModel; + tmpVecX = egoMotionModel.f(tmpVecX, tmpVecU); + + Vector vecXout; + vecXout[0] = tmpVecX[0]; + vecXout[1] = tmpVecX[1]; + vecXout[4] = tmpVecX[2]; + + return vecXout; + } + + virtual Matrix getProcessNoiseCov(Vector const & vecX, Vector const & vecU) const override + { + // input idx -> output index mapping + // 0 -> 0 + // 1 -> 1 + // 2 -> 4 + Vector<3> tmpVecX; + tmpVecX << vecX[0], vecX[1], vecX[4]; + + // input idx -> output index mapping + // 0 -> 1 + // 1 -> 2 + Vector<2> tmpVecU; + tmpVecU << vecU[1], vecU[2]; + + motionmodel::EgoMotionModel const egoMotionModel; + + Matrix<3, 3> matQ = egoMotionModel.getProcessNoiseCov(tmpVecX, tmpVecU); + + // |q00 q01 x x q02| + // |q10 q11 x x q12| |q00 q01 q02| + // Qout = | x x x x x| <- Q = |q10 q11 q12| + // | x x x x x| |q20 q21 q22| + // |q20 q21 x x q22| + + Matrix matQout; + matQout(0, 0) = matQ(0, 0); + matQout(0, 1) = matQ(0, 1); + matQout(0, 4) = matQ(0, 2); + matQout(1, 0) = matQ(1, 0); + matQout(1, 1) = matQ(1, 1); + matQout(1, 4) = matQ(1, 2); + matQout(4, 0) = matQ(2, 0); + matQout(4, 1) = matQ(2, 1); + matQout(4, 4) = matQ(2, 1); + + return matQout; + } + + virtual Matrix getInputNoiseCov(Vector const & vecX, Vector const & vecU) const override + { + Vector<3> tmpVecX; + tmpVecX << vecX[0], vecX[1], vecX[4]; + + Vector<2> tmpVecU; + tmpVecU << vecU[1], vecU[2]; + + motionmodel::EgoMotionModel const egoMotionModel; + + Matrix<3, 3> matU = egoMotionModel.getInputNoiseCov(tmpVecX, tmpVecU); + + Matrix matUout; + matUout(0, 0) = matU(0, 0); + matUout(0, 1) = matU(0, 1); + matUout(0, 4) = matU(0, 2); + matUout(1, 0) = matU(1, 0); + matUout(1, 1) = matU(1, 1); + matUout(1, 4) = matU(1, 2); + matUout(4, 0) = matU(2, 0); + matUout(4, 1) = matU(2, 1); + matUout(4, 4) = matU(2, 1); + + return matUout; + } + + virtual Matrix getJacobianFk(Vector const & vecX, Vector const & vecU) const override + { + Vector<3> tmpVecX; + tmpVecX << vecX[0], vecX[1], vecX[4]; + + Vector<2> tmpVecU; + tmpVecU << vecU[1], vecU[2]; + + motionmodel::EgoMotionModel const egoMotionModel; + + Matrix<3, 3> matFk = egoMotionModel.getJacobianFk(tmpVecX, tmpVecU); + + Matrix matFkout; + matFkout(0, 0) = matFk(0, 0); + matFkout(0, 1) = matFk(0, 1); + matFkout(0, 4) = matFk(0, 2); + matFkout(1, 0) = matFk(1, 0); + matFkout(1, 1) = matFk(1, 1); + matFkout(1, 4) = matFk(1, 2); + matFkout(4, 0) = matFk(2, 0); + matFkout(4, 1) = matFk(2, 1); + matFkout(4, 4) = matFk(2, 1); + + return matFkout; + } + + virtual Matrix getJacobianBk(Vector const & vecX, Vector const & vecU) const override + { + Vector<3> tmpVecX; + tmpVecX << vecX[0], vecX[1], vecX[4]; + + Vector<2> tmpVecU; + tmpVecU << vecU[1], vecU[2]; + + motionmodel::EgoMotionModel const egoMotionModel; + + Matrix<3, 2> matBk = egoMotionModel.getJacobianBk(tmpVecX, tmpVecU); + + Matrix matBkout; + matBkout(0, 1) = matBk(0, 0); + matBkout(0, 2) = matBk(0, 1); + matBkout(1, 1) = matBk(1, 0); + matBkout(1, 2) = matBk(1, 1); + matBkout(4, 1) = matBk(2, 0); + matBkout(4, 2) = matBk(2, 1); + + return matBkout; + } +}; + +int main() +{ + EgoMotionModelAdapter egoMotionModelAdapter; + + Vector vecU; + vecU << 1.0F, 2.0F, 0.01F; + + KalmanFilter kf; + kf.predictEkf(egoMotionModelAdapter, vecU); + + return 0; +} diff --git a/src/openkf/CMakeLists.txt b/src/openkf/CMakeLists.txt index 5a2c7e4..6da824e 100644 --- a/src/openkf/CMakeLists.txt +++ b/src/openkf/CMakeLists.txt @@ -16,6 +16,7 @@ set(LIBRARY_SRC_FILES dummy.cpp + motion_model/ego_motion_model.cpp ) set(LIBRARY_HDR_FILES @@ -25,6 +26,8 @@ set(LIBRARY_HDR_FILES kalman_filter/unscented_transform.h kalman_filter/unscented_kalman_filter.h kalman_filter/square_root_ukf.h + motion_model/motion_model.h + motion_model/ego_motion_model.h ) set(LIBRARY_NAME ${PROJECT_NAME}) diff --git a/src/openkf/kalman_filter/kalman_filter.h b/src/openkf/kalman_filter/kalman_filter.h index b8bb68b..ac193ed 100644 --- a/src/openkf/kalman_filter/kalman_filter.h +++ b/src/openkf/kalman_filter/kalman_filter.h @@ -13,6 +13,7 @@ #define KALMAN_FILTER_LIB_H #include "types.h" +#include "motion_model/motion_model.h" namespace kf { @@ -77,6 +78,32 @@ namespace kf m_matP = matJacobF * m_matP * matJacobF.transpose() + matQ; } + /// + /// @brief predict state with a linear process model. + /// @param motionModel prediction motion model function + /// + void predictEkf(motionmodel::MotionModel const & motionModel) + { + Matrix const matFk{ motionModel.getJacobianFk(m_vecX) }; + Matrix const matQk{ motionModel.getProcessNoiseCov(m_vecX) }; + m_vecX = motionModel.f(m_vecX); + m_matP = matFk * m_matP * matFk.transpose() + matQk; + } + + /// + /// @brief predict state with a linear process model with external input. + /// @param motionModel prediction motion model function + /// @param vecU input vector + /// + template + void predictEkf(motionmodel::MotionModelExtInput const & motionModel, Vector const & vecU) + { + Matrix const matFk{ motionModel.getJacobianFk(m_vecX, vecU) }; + Matrix const matQk{ motionModel.getProcessNoiseCov(m_vecX, vecU) }; + m_vecX = motionModel.f(m_vecX, vecU); + m_matP = matFk * m_matP * matFk.transpose() + matQk; + } + /// /// @brief correct state of with a linear measurement model. /// @param measurementModel measurement model function callback diff --git a/src/openkf/motion_model/ego_motion_model.cpp b/src/openkf/motion_model/ego_motion_model.cpp new file mode 100644 index 0000000..cdbb4e8 --- /dev/null +++ b/src/openkf/motion_model/ego_motion_model.cpp @@ -0,0 +1,199 @@ +#include "ego_motion_model.h" + +namespace kf +{ + namespace motionmodel + { + Vector EgoMotionModel::f(Vector const & vecX, Vector const & vecU, Vector const & vecQ) const + { + Vector vecXout; + float32_t & oPosX{ vecXout[0] }; + float32_t & oPosY{ vecXout[1] }; + float32_t & oYaw { vecXout[2] }; + + float32_t const & iPosX{ vecX[0] }; + float32_t const & iPosY{ vecX[1] }; + float32_t const & iYaw { vecX[2] }; + + float32_t const & qPosX{ vecQ[0] }; + float32_t const & qPosY{ vecQ[1] }; + float32_t const & qYaw { vecQ[2] }; + + float32_t const & deltaDist{ vecU[0] }; + float32_t const & deltaYaw { vecU[1] }; + + float32_t const halfDeltaYaw{ deltaYaw / 2.0F }; + float32_t const iYawPlusHalfDeltaYaw{ iYaw + halfDeltaYaw }; + + oPosX = iPosX + (deltaDist * std::cos(iYawPlusHalfDeltaYaw)) + qPosX; + oPosY = iPosY + (deltaDist * std::sin(iYawPlusHalfDeltaYaw)) + qPosY; + oYaw = iYaw + deltaYaw + qYaw; + + return vecXout; + } + + Matrix EgoMotionModel::getProcessNoiseCov(Vector const & vecX, Vector const & vecU) const + { + Matrix matQk; + + float32_t & q11 = matQk(0, 0); + float32_t & q12 = matQk(0, 1); + float32_t & q13 = matQk(0, 2); + + float32_t & q21 = matQk(1, 0); + float32_t & q22 = matQk(1, 1); + float32_t & q23 = matQk(1, 2); + + float32_t & q31 = matQk(2, 0); + float32_t & q32 = matQk(2, 1); + float32_t & q33 = matQk(2, 2); + + float32_t const & iYaw { vecX[2] }; + + float32_t const & deltaDist{ vecU[0] }; + float32_t const & deltaYaw { vecU[1] }; + + float32_t const deltaDistSquare{ deltaDist * deltaDist }; + float32_t const halfDeltaYaw{ deltaYaw / 2.0F }; + float32_t const iYawPlusHalfDeltaYaw{ iYaw + halfDeltaYaw }; + float32_t const sinTheta{ std::sin(iYawPlusHalfDeltaYaw) }; + float32_t const cosTheta{ std::cos(iYawPlusHalfDeltaYaw) }; + float32_t const sinCosTheta{ sinTheta * cosTheta }; + + q11 = m_qX + (deltaDistSquare * sinTheta * sinTheta * m_qTheta); + q12 = -deltaDistSquare * sinCosTheta * m_qTheta; + q13 = -deltaDist * sinTheta * m_qTheta; + + q21 = -deltaDistSquare * sinCosTheta * m_qTheta; + q22 = m_qY + (deltaDistSquare * cosTheta * cosTheta * m_qTheta); + q23 = deltaDist * cosTheta * m_qTheta; + + q31 = -deltaDist * sinTheta * m_qTheta; + q32 = deltaDist * cosTheta * m_qTheta; + q33 = m_qTheta; + + return matQk; + } + + Matrix EgoMotionModel::getInputNoiseCov(Vector const &vecX, Vector const &vecU) const + { + Matrix matUk; + + float32_t & u11 = matUk(0, 0); + float32_t & u12 = matUk(0, 1); + float32_t & u13 = matUk(0, 2); + + float32_t & u21 = matUk(1, 0); + float32_t & u22 = matUk(1, 1); + float32_t & u23 = matUk(1, 2); + + float32_t & u31 = matUk(2, 0); + float32_t & u32 = matUk(2, 1); + float32_t & u33 = matUk(2, 2); + + float32_t const & iYaw { vecX[2] }; + + float32_t const & deltaDist{ vecU[0] }; + float32_t const & deltaYaw { vecU[1] }; + + float32_t const deltaDistSquare{ deltaDist * deltaDist }; + float32_t const halfDeltaDistSquare{ deltaDist / 2.0F }; + float32_t const quarterDeltaDistSquare{ deltaDistSquare / 4.0F }; + float32_t const halfDeltaYaw{ deltaYaw / 2.0F }; + float32_t const iYawPlusHalfDeltaYaw{ iYaw + halfDeltaYaw }; + float32_t const sinTheta{ std::sin(iYawPlusHalfDeltaYaw) }; + float32_t const cosTheta{ std::cos(iYawPlusHalfDeltaYaw) }; + float32_t const sin2Theta{ sinTheta * sinTheta }; + float32_t const cos2Theta{ cosTheta * cosTheta }; + float32_t const sinCosTheta{ sinTheta * cosTheta }; + + u11 = (m_uDeltaDist * cos2Theta) + (m_uDeltaYaw * quarterDeltaDistSquare * sin2Theta); + u12 = (m_uDeltaDist * sinCosTheta) - (m_uDeltaYaw * quarterDeltaDistSquare * sinCosTheta); + u13 = -m_uDeltaYaw * halfDeltaDistSquare * sinTheta; + + u21 = (m_uDeltaDist * sinCosTheta) - (m_uDeltaYaw * quarterDeltaDistSquare * sinCosTheta); + u22 = (m_uDeltaDist * sin2Theta) + (m_uDeltaYaw * quarterDeltaDistSquare * cos2Theta); + u23 = m_uDeltaYaw * halfDeltaDistSquare * cosTheta; + + u31 = -m_uDeltaYaw * halfDeltaDistSquare * sinTheta; + u32 = m_uDeltaYaw * halfDeltaDistSquare * cosTheta; + u33 = m_uDeltaYaw; + + return matUk; + } + + Matrix EgoMotionModel::getJacobianFk(Vector const & vecX, Vector const & vecU) const + { + Matrix matFk; + + float32_t & df1dx1 = matFk(0, 0); + float32_t & df1dx2 = matFk(0, 1); + float32_t & df1dx3 = matFk(0, 2); + + float32_t & df2dx1 = matFk(1, 0); + float32_t & df2dx2 = matFk(1, 1); + float32_t & df2dx3 = matFk(1, 2); + + float32_t & df3dx1 = matFk(2, 0); + float32_t & df3dx2 = matFk(2, 1); + float32_t & df3dx3 = matFk(2, 2); + + float32_t const & iYaw { vecX[2] }; + + float32_t const & deltaDist{ vecU[0] }; + float32_t const & deltaYaw { vecU[1] }; + + float32_t const halfDeltaYaw{ deltaYaw / 2.0F }; + float32_t const iYawPlusHalfDeltaYaw{ iYaw + halfDeltaYaw }; + + df1dx1 = 1.0F; + df1dx2 = 0.0F; + df1dx3 = -deltaDist * std::sin(iYawPlusHalfDeltaYaw); + + df2dx1 = 0.0F; + df2dx2 = 1.0F; + df2dx3 = deltaDist * std::cos(iYawPlusHalfDeltaYaw); + + df3dx1 = 0.0F; + df3dx2 = 0.0F; + df3dx3 = 1.0F; + + return matFk; + } + + Matrix EgoMotionModel::getJacobianBk(Vector const & vecX, Vector const & vecU) const + { + Matrix matBk; + + float32_t & df1du1 = matBk(0, 0); + float32_t & df1du2 = matBk(0, 1); + + float32_t & df2du1 = matBk(1, 0); + float32_t & df2du2 = matBk(1, 1); + + float32_t & df3du1 = matBk(2, 0); + float32_t & df3du2 = matBk(2, 1); + + float32_t const & iYaw { vecX[2] }; + + float32_t const & deltaDist{ vecU[0] }; + float32_t const & deltaYaw { vecU[1] }; + + float32_t const halfDeltaDist{ deltaDist / 2.0F }; + float32_t const halfDeltaYaw { deltaYaw / 2.0F }; + + float32_t const iYawPlusHalfDeltaYaw{ iYaw + halfDeltaYaw }; + + df1du1 = std::cos(iYawPlusHalfDeltaYaw); + df1du2 = -halfDeltaDist * std::sin(iYawPlusHalfDeltaYaw); + + df2du1 = std::sin(iYawPlusHalfDeltaYaw); + df2du2 = halfDeltaDist * std::cos(iYawPlusHalfDeltaYaw); + + df3du1 = 0.0F; + df3du2 = 1.0F; + + return matBk; + } + } +} \ No newline at end of file diff --git a/src/openkf/motion_model/ego_motion_model.h b/src/openkf/motion_model/ego_motion_model.h new file mode 100644 index 0000000..ec50e1b --- /dev/null +++ b/src/openkf/motion_model/ego_motion_model.h @@ -0,0 +1,120 @@ +#ifndef OPENKF_EGO_MOTION_MODEL_H +#define OPENKF_EGO_MOTION_MODEL_H + +#include "types.h" +#include "motion_model.h" + +namespace kf +{ + namespace motionmodel + { + + /// @brief State space dimension for ego-motion model \vec{x}=[x, y, yaw]^T + static constexpr int32_t DIM_X{ 3 }; + + /// @brief Input space dimension used by ego-motion model \vec{u}=[delta_distance, delta_yaw]^T + static constexpr int32_t DIM_U{ 2 }; + + /// @brief Derived class from base motion model class with external inputs + /// Motion model for ego vehicle which is used for dead reckoning purposes + /// by utilizing odometry external inputs vehicle displacement and change + /// in heading angle and incrementing them to obtain the new state. + class EgoMotionModel : public MotionModelExtInput + { + public: + + EgoMotionModel() + : m_qX(0.0F) + , m_qY(0.0F) + , m_qTheta(0.0F) + , m_uDeltaDist(0.0F) + , m_uDeltaYaw(0.0F) + {} + + /// @brief Prediction motion model function that propagate the previous state to next state in time. + /// @param vecX State space vector \vec{x} + /// @param vecU Input space vector \vec{u} + /// @param vecQ State white gaussian noise vector \vec{q} + /// @return Predicted/ propagated state space vector + virtual Vector f(Vector const & vecX, Vector const & vecU, Vector const & vecQ = Vector::Zero()) const override; + + /// @brief Get the process noise covariance Q + /// @param vecX State space vector \vec{x} + /// @param vecU Input space vector \vec{u} + /// @return The process noise covariance Q + virtual Matrix getProcessNoiseCov(Vector const & vecX, Vector const & vecU) const override; + + /// @brief Get the input noise covariance U + /// @param vecX State space vector \vec{x} + /// @param vecU Input space vector \vec{u} + /// @return The input noise covariance U + virtual Matrix getInputNoiseCov(Vector const & vecX, Vector const & vecU) const override; + + /// @brief Method that calculates the jacobians of the state transition model. + /// @param vecX State Space vector \vec{x} + /// @param vecU Input Space vector \vec{u} + /// @return The jacobians of the state transition model. + virtual Matrix getJacobianFk(Vector const & vecX, Vector const & vecU) const override; + + /// @brief Method that calculates the jacobians of the input transition model. + /// @param vecX State Space vector \vec{x} + /// @param vecU Input Space vector \vec{u} + /// @return The jacobians of the input transition model. + virtual Matrix getJacobianBk(Vector const & vecX, Vector const & vecU) const override; + + /// @brief Setter for noise variance of pose-x state. + /// @param val variance value + void setNoiseX(float32_t const val) + { + m_qX = val; + } + + /// @brief Setter for noise variance of pose-y state. + /// @param val variance value + void setNoiseY(float32_t const val) + { + m_qY = val; + } + + /// @brief Setter for noise variance of pose-yaw state. + /// @param val variance value + void setNoiseTheta(float32_t const val) + { + m_qTheta = val; + } + + /// @brief Setter for noise variance of delta distance input. + /// @param val variance value + void setNoiseDeltaDist(float32_t const val) + { + m_uDeltaDist = val; + } + + /// @brief Setter for noise variance of delta yaw input + /// @param val variance value + void setNoiseDeltaYaw(float32_t const val) + { + m_uDeltaYaw = val; + } + + private: + + /// @brief The noise variance of pose-x state. + float32_t m_qX; + + /// @brief The noise variance of pose-y state. + float32_t m_qY; + + /// @brief The noise variance of pose-yaw state. + float32_t m_qTheta; + + /// @brief The noise variance of delta distance input. + float32_t m_uDeltaDist; + + /// @brief The noise variance of delta yaw input. + float32_t m_uDeltaYaw; + }; + } +} + +#endif // OPENKF_EGO_MOTION_MODEL_H \ No newline at end of file diff --git a/src/openkf/motion_model/motion_model.h b/src/openkf/motion_model/motion_model.h new file mode 100644 index 0000000..30544c8 --- /dev/null +++ b/src/openkf/motion_model/motion_model.h @@ -0,0 +1,76 @@ +#ifndef OPENKF_MOTION_MODEL_H +#define OPENKF_MOTION_MODEL_H + +#include "types.h" + +namespace kf +{ + namespace motionmodel + { + /// @brief Base class for motion models used by kalman filters + /// @tparam DIM_X State space vector dimension + template + class MotionModel + { + public: + + /// @brief Prediction motion model function that propagate the previous state to next state in time. + /// @param vecX State space vector \vec{x} + /// @param vecQ State white gaussian noise vector \vec{q} + /// @return Predicted/ propagated state space vector + virtual Vector f(Vector const & vecX, Vector const & vecQ = Vector::Zero()) const = 0; + + /// @brief Get the process noise covariance Q + /// @param vecX State space vector \vec{x} + /// @return The process noise covariance Q + virtual Matrix getProcessNoiseCov(Vector const & vecX) const = 0; + + /// @brief Method that calculates the jacobians of the state transition model. + /// @param vecX State Space vector \vec{x} + /// @return The jacobians of the state transition model. + virtual Matrix getJacobianFk(Vector const & vecX) const = 0; + }; + + /// @brief Base class for motion models with external inputs used by kalman filters + /// @tparam DIM_X State space vector dimension + /// @tparam DIM_U Input space vector dimension + template + class MotionModelExtInput + { + public: + + /// @brief Prediction motion model function that propagate the previous state to next state in time. + /// @param vecX State space vector \vec{x} + /// @param vecU Input space vector \vec{u} + /// @param vecQ State white gaussian noise vector \vec{q} + /// @return Predicted/ propagated state space vector + virtual Vector f(Vector const & vecX, Vector const & vecU, Vector const & vecQ = Vector::Zero()) const = 0; + + /// @brief Get the process noise covariance Q + /// @param vecX State space vector \vec{x} + /// @param vecU Input space vector \vec{u} + /// @return The process noise covariance Q + virtual Matrix getProcessNoiseCov(Vector const & vecX, Vector const & vecU) const = 0; + + /// @brief Get the input noise covariance U + /// @param vecX State space vector \vec{x} + /// @param vecU Input space vector \vec{u} + /// @return The input noise covariance U + virtual Matrix getInputNoiseCov(Vector const & vecX, Vector const & vecU) const = 0; + + /// @brief Method that calculates the jacobians of the state transition model. + /// @param vecX State Space vector \vec{x} + /// @param vecU Input Space vector \vec{u} + /// @return The jacobians of the state transition model. + virtual Matrix getJacobianFk(Vector const & vecX, Vector const & vecU) const = 0; + + /// @brief Method that calculates the jacobians of the input transition model. + /// @param vecX State Space vector \vec{x} + /// @param vecU Input Space vector \vec{u} + /// @return The jacobians of the input transition model. + virtual Matrix getJacobianBk(Vector const & vecX, Vector const & vecU) const = 0; + }; + } +} + +#endif // OPENKF_MOTION_MODEL_H diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 40d368a..269aea0 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,35 +1 @@ -# cmake_minimum_required(VERSION 3.4) -set(TEST_APP_NAME ${PROJECT_NAME}_Test) - -enable_testing() - -set(BUILD_GMOCK OFF CACHE BOOL "" FORCE) -set(BUILD_GTEST ON CACHE BOOL "" FORCE) - -# find_package(GTest REQUIRED) -# include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR}) - -add_executable(${TEST_APP_NAME} - unit_tests.cpp - kalman_filter_test.cpp - unscented_trasform_test.cpp - unscented_kalman_filter_test.cpp - square_root_ukf_test.cpp -) - -target_link_libraries(${TEST_APP_NAME} PRIVATE - GTest::gtest_main - OpenKF -) - -target_include_directories( ${TEST_APP_NAME} PRIVATE ${GTEST_INCLUDE_DIRS} ) -# target_link_libraries( ${TEST_APP_NAME} tested_library ${GTEST_BOTH_LIBRARIES} ) - -if (MSVC) - # https://stackoverflow.com/a/18635749 - set_property(TARGET ${TEST_APP_NAME} PROPERTY - MSVC_RUNTIME_LIBRARY "MultiThreaded$<$:Debug>") -endif(MSVC) - -include(GoogleTest) -gtest_discover_tests(${TEST_APP_NAME}) +add_subdirectory(unit) diff --git a/tests/unit/CMakeLists.txt b/tests/unit/CMakeLists.txt new file mode 100644 index 0000000..222dbc6 --- /dev/null +++ b/tests/unit/CMakeLists.txt @@ -0,0 +1,36 @@ +# cmake_minimum_required(VERSION 3.4) +set(TEST_APP_NAME ${PROJECT_NAME}_Test) + +enable_testing() + +set(BUILD_GMOCK OFF CACHE BOOL "" FORCE) +set(BUILD_GTEST ON CACHE BOOL "" FORCE) + +# find_package(GTest REQUIRED) +# include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR}) + +add_executable(${TEST_APP_NAME} + unit_tests.cpp + kalman_filter/kalman_filter_test.cpp + kalman_filter/unscented_trasform_test.cpp + kalman_filter/unscented_kalman_filter_test.cpp + kalman_filter/square_root_ukf_test.cpp + motion_model/ego_motion_model_test.cpp +) + +target_link_libraries(${TEST_APP_NAME} PRIVATE + GTest::gtest_main + OpenKF +) + +target_include_directories( ${TEST_APP_NAME} PRIVATE ${GTEST_INCLUDE_DIRS} ) +# target_link_libraries( ${TEST_APP_NAME} tested_library ${GTEST_BOTH_LIBRARIES} ) + +if (MSVC) + # https://stackoverflow.com/a/18635749 + set_property(TARGET ${TEST_APP_NAME} PROPERTY + MSVC_RUNTIME_LIBRARY "MultiThreaded$<$:Debug>") +endif(MSVC) + +include(GoogleTest) +gtest_discover_tests(${TEST_APP_NAME}) diff --git a/tests/kalman_filter_test.cpp b/tests/unit/kalman_filter/kalman_filter_test.cpp similarity index 100% rename from tests/kalman_filter_test.cpp rename to tests/unit/kalman_filter/kalman_filter_test.cpp diff --git a/tests/square_root_ukf_test.cpp b/tests/unit/kalman_filter/square_root_ukf_test.cpp similarity index 100% rename from tests/square_root_ukf_test.cpp rename to tests/unit/kalman_filter/square_root_ukf_test.cpp diff --git a/tests/unscented_kalman_filter_test.cpp b/tests/unit/kalman_filter/unscented_kalman_filter_test.cpp similarity index 100% rename from tests/unscented_kalman_filter_test.cpp rename to tests/unit/kalman_filter/unscented_kalman_filter_test.cpp diff --git a/tests/unscented_trasform_test.cpp b/tests/unit/kalman_filter/unscented_trasform_test.cpp similarity index 100% rename from tests/unscented_trasform_test.cpp rename to tests/unit/kalman_filter/unscented_trasform_test.cpp diff --git a/tests/unit/motion_model/ego_motion_model_test.cpp b/tests/unit/motion_model/ego_motion_model_test.cpp new file mode 100644 index 0000000..28e1ec2 --- /dev/null +++ b/tests/unit/motion_model/ego_motion_model_test.cpp @@ -0,0 +1,263 @@ +#include +#include "motion_model/ego_motion_model.h" +#include "kalman_filter/kalman_filter.h" +#include "types.h" + +using namespace kf; + +class EgoMotionModelTest : public testing::Test +{ +public: + virtual void SetUp() override {} + virtual void TearDown() override {} + + static constexpr int32_t DIM_X{ 3 }; + static constexpr int32_t DIM_U{ 2 }; + static constexpr int32_t DIM_Z{ 1 }; +}; + +TEST_F(EgoMotionModelTest, test_egoMotionModel_Covariances) +{ + kf::Vector vecX; + vecX << 1.0F, 2.0F, 0.015F; + + kf::Vector vecU; + vecU << 0.5F, 0.005F; + + kf::motionmodel::EgoMotionModel egoMotionModel; + + float32_t const qX{ 0.2F }; + float32_t const qY{ 0.3F }; + float32_t const qTheta{ 0.003F }; + + float32_t const uDeltaDist{ 0.02F }; + float32_t const uDeltaYaw{ 0.0002F }; + + egoMotionModel.setNoiseX(qX); + egoMotionModel.setNoiseY(qY); + egoMotionModel.setNoiseTheta(qTheta); + egoMotionModel.setNoiseDeltaDist(uDeltaDist); + egoMotionModel.setNoiseDeltaYaw(uDeltaYaw); + + kf::Matrix const matQk{ egoMotionModel.getProcessNoiseCov(vecX, vecU) }; + kf::Matrix const matUk{ egoMotionModel.getInputNoiseCov(vecX, vecU) }; + + kf::Matrix const matFk{ egoMotionModel.getJacobianFk(vecX, vecU) }; + kf::Matrix const matBk{ egoMotionModel.getJacobianBk(vecX, vecU) }; + + kf::Matrix matQ; + matQ << qX, 0.0F, 0.0F, + 0.0F, qY, 0.0F, + 0.0F, 0.0F, qTheta; + + kf::Matrix matU; + matU << uDeltaDist, 0.0F, + 0.0F, uDeltaYaw; + + kf::Matrix const matQkExp{ matFk * matQ * matFk.transpose() }; + kf::Matrix const matUkExp{ matBk * matU * matBk.transpose() }; + + EXPECT_NEAR(matQk(0,0), matQkExp(0,0), 0.0001F); + EXPECT_NEAR(matQk(0,1), matQkExp(0,1), 0.0001F); + EXPECT_NEAR(matQk(0,2), matQkExp(0,2), 0.0001F); + EXPECT_NEAR(matQk(1,0), matQkExp(1,0), 0.0001F); + EXPECT_NEAR(matQk(1,1), matQkExp(1,1), 0.0001F); + EXPECT_NEAR(matQk(1,2), matQkExp(1,2), 0.0001F); + EXPECT_NEAR(matQk(2,0), matQkExp(2,0), 0.0001F); + EXPECT_NEAR(matQk(2,1), matQkExp(2,1), 0.0001F); + EXPECT_NEAR(matQk(2,2), matQkExp(2,2), 0.0001F); + + EXPECT_NEAR(matUk(0,0), matUkExp(0,0), 0.0001F); + EXPECT_NEAR(matUk(0,1), matUkExp(0,1), 0.0001F); + EXPECT_NEAR(matUk(0,2), matUkExp(0,2), 0.0001F); + EXPECT_NEAR(matUk(1,0), matUkExp(1,0), 0.0001F); + EXPECT_NEAR(matUk(1,1), matUkExp(1,1), 0.0001F); + EXPECT_NEAR(matUk(1,2), matUkExp(1,2), 0.0001F); + EXPECT_NEAR(matUk(2,0), matUkExp(2,0), 0.0001F); + EXPECT_NEAR(matUk(2,1), matUkExp(2,1), 0.0001F); + EXPECT_NEAR(matUk(2,2), matUkExp(2,2), 0.0001F); +} + +TEST_F(EgoMotionModelTest, test_egoMotionModel_ForwardReverseMoves) +{ + kf::motionmodel::EgoMotionModel egoMotionModel; + kf::Vector vecX; + kf::Vector vecU; + kf::Vector vecXn; + + // moving forward + vecX << 0.0F, 0.0F, 0.0F; + vecU << 0.5F, 0.0F; + + vecXn = egoMotionModel.f(vecX, vecU); + + EXPECT_NEAR(vecXn[0], vecX[0]+0.5F, 0.0001F); + EXPECT_NEAR(vecXn[1], vecX[1], 0.0001F); + EXPECT_NEAR(vecXn[2], vecX[2], 0.0001F); + + // moving backward/reverse + vecX << 0.0F, 0.0F, 0.0F; + vecU << -0.5F, 0.0F; + + vecXn = egoMotionModel.f(vecX, vecU); + + EXPECT_NEAR(vecXn[0], vecX[0]-0.5F, 0.0001F); + EXPECT_NEAR(vecXn[1], vecX[1], 0.0001F); + EXPECT_NEAR(vecXn[2], vecX[2], 0.0001F); + + // moving forward + oriented 90 degrees + vecX << 0.0F, 0.0F, M_PI / 2.0F; + vecU << 0.5F, 0.0F; + + vecXn = egoMotionModel.f(vecX, vecU); + + EXPECT_NEAR(vecXn[0], vecX[0], 0.0001F); + EXPECT_NEAR(vecXn[1], vecX[1] + 0.5F, 0.0001F); + EXPECT_NEAR(vecXn[2], vecX[2], 0.0001F); + + // moving backward + oriented 90 degrees + vecX << 0.0F, 0.0F, M_PI / 2.0F; + vecU << -0.5F, 0.0F; + + vecXn = egoMotionModel.f(vecX, vecU); + + EXPECT_NEAR(vecXn[0], vecX[0], 0.0001F); + EXPECT_NEAR(vecXn[1], vecX[1] - 0.5F, 0.0001F); + EXPECT_NEAR(vecXn[2], vecX[2], 0.0001F); + + // moving forward + oriented -90 degrees + vecX << 0.0F, 0.0F, -M_PI / 2.0F; + vecU << 0.5F, 0.0F; + + vecXn = egoMotionModel.f(vecX, vecU); + + EXPECT_NEAR(vecXn[0], vecX[0], 0.0001F); + EXPECT_NEAR(vecXn[1], vecX[1] - 0.5F, 0.0001F); + EXPECT_NEAR(vecXn[2], vecX[2], 0.0001F); + + // moving backward + oriented -90 degrees + vecX << 0.0F, 0.0F, -M_PI / 2.0F; + vecU << -0.5F, 0.0F; + + vecXn = egoMotionModel.f(vecX, vecU); + + EXPECT_NEAR(vecXn[0], vecX[0], 0.0001F); + EXPECT_NEAR(vecXn[1], vecX[1] + 0.5F, 0.0001F); + EXPECT_NEAR(vecXn[2], vecX[2], 0.0001F); + + // moving forward + oriented 45 degrees + vecX << 0.0F, 0.0F, M_PI / 4.0F; + vecU << 0.5F, 0.0F; + + vecXn = egoMotionModel.f(vecX, vecU); + + EXPECT_NEAR(vecXn[0], vecX[0] + vecU[0] * std::cos(vecX[2] + (vecU[1] / 2.0F)), 0.0001F); + EXPECT_NEAR(vecXn[1], vecX[1] + vecU[0] * std::sin(vecX[2] + (vecU[1] / 2.0F)), 0.0001F); + EXPECT_NEAR(vecXn[2], vecX[2] + vecU[1], 0.0001F); + + // moving forward + oriented 45 degrees + vecX << 0.0F, 0.0F, -M_PI / 4.0F; + vecU << 0.5F, 0.0F; + + vecXn = egoMotionModel.f(vecX, vecU); + + EXPECT_NEAR(vecXn[0], vecX[0] + vecU[0] * std::cos(vecX[2] + (vecU[1] / 2.0F)), 0.0001F); + EXPECT_NEAR(vecXn[1], vecX[1] + vecU[0] * std::sin(vecX[2] + (vecU[1] / 2.0F)), 0.0001F); + EXPECT_NEAR(vecXn[2], vecX[2] + vecU[1], 0.0001F); +} + +TEST_F(EgoMotionModelTest, test_egoMotionModel_EKF_ForwardReverseMoves) +{ + kf::KalmanFilter kalmanFilter; + kf::motionmodel::EgoMotionModel egoMotionModel; + kf::Vector vecX; + kf::Vector vecU; + + // moving forward + vecX << 0.0F, 0.0F, 0.0F; + vecU << 0.5F, 0.0F; + + kalmanFilter.vecX() = vecX; + kalmanFilter.predictEkf(egoMotionModel, vecU); + + EXPECT_NEAR(kalmanFilter.vecX()[0], vecX[0]+0.5F, 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[1], vecX[1], 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[2], vecX[2], 0.0001F); + + // moving backward/reverse + vecX << 0.0F, 0.0F, 0.0F; + vecU << -0.5F, 0.0F; + + kalmanFilter.vecX() = vecX; + kalmanFilter.predictEkf(egoMotionModel, vecU); + + EXPECT_NEAR(kalmanFilter.vecX()[0], vecX[0]-0.5F, 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[1], vecX[1], 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[2], vecX[2], 0.0001F); + + // moving forward + oriented 90 degrees + vecX << 0.0F, 0.0F, M_PI / 2.0F; + vecU << 0.5F, 0.0F; + + kalmanFilter.vecX() = vecX; + kalmanFilter.predictEkf(egoMotionModel, vecU); + + EXPECT_NEAR(kalmanFilter.vecX()[0], vecX[0], 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[1], vecX[1] + 0.5F, 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[2], vecX[2], 0.0001F); + + // moving backward + oriented 90 degrees + vecX << 0.0F, 0.0F, M_PI / 2.0F; + vecU << -0.5F, 0.0F; + + kalmanFilter.vecX() = vecX; + kalmanFilter.predictEkf(egoMotionModel, vecU); + + EXPECT_NEAR(kalmanFilter.vecX()[0], vecX[0], 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[1], vecX[1] - 0.5F, 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[2], vecX[2], 0.0001F); + + // moving forward + oriented -90 degrees + vecX << 0.0F, 0.0F, -M_PI / 2.0F; + vecU << 0.5F, 0.0F; + + kalmanFilter.vecX() = vecX; + kalmanFilter.predictEkf(egoMotionModel, vecU); + + EXPECT_NEAR(kalmanFilter.vecX()[0], vecX[0], 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[1], vecX[1] - 0.5F, 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[2], vecX[2], 0.0001F); + + // moving backward + oriented -90 degrees + vecX << 0.0F, 0.0F, -M_PI / 2.0F; + vecU << -0.5F, 0.0F; + + kalmanFilter.vecX() = vecX; + kalmanFilter.predictEkf(egoMotionModel, vecU); + + EXPECT_NEAR(kalmanFilter.vecX()[0], vecX[0], 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[1], vecX[1] + 0.5F, 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[2], vecX[2], 0.0001F); + + // moving forward + oriented 45 degrees + vecX << 0.0F, 0.0F, M_PI / 4.0F; + vecU << 0.5F, 0.0F; + + kalmanFilter.vecX() = vecX; + kalmanFilter.predictEkf(egoMotionModel, vecU); + + EXPECT_NEAR(kalmanFilter.vecX()[0], vecX[0] + vecU[0] * std::cos(vecX[2] + (vecU[1] / 2.0F)), 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[1], vecX[1] + vecU[0] * std::sin(vecX[2] + (vecU[1] / 2.0F)), 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[2], vecX[2] + vecU[1], 0.0001F); + + // moving forward + oriented 45 degrees + vecX << 0.0F, 0.0F, -M_PI / 4.0F; + vecU << 0.5F, 0.0F; + + kalmanFilter.vecX() = vecX; + kalmanFilter.predictEkf(egoMotionModel, vecU); + + EXPECT_NEAR(kalmanFilter.vecX()[0], vecX[0] + vecU[0] * std::cos(vecX[2] + (vecU[1] / 2.0F)), 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[1], vecX[1] + vecU[0] * std::sin(vecX[2] + (vecU[1] / 2.0F)), 0.0001F); + EXPECT_NEAR(kalmanFilter.vecX()[2], vecX[2] + vecU[1], 0.0001F); +} diff --git a/tests/unit_tests.cpp b/tests/unit/unit_tests.cpp similarity index 100% rename from tests/unit_tests.cpp rename to tests/unit/unit_tests.cpp diff --git a/tests/util_test.cpp b/tests/unit/util_test.cpp similarity index 100% rename from tests/util_test.cpp rename to tests/unit/util_test.cpp