Merge pull request #4 from Al-khwarizmi-780/feature/refactoring

refactoring and fix linux build

bootstrap-openkf.bat

@@ -1,16 +1,20 @@
 @echo off
 
-echo Creating Build Folder
-md .\cpp\build
+if not exist ".\cpp\build" (
+    echo Creating ./cpp/build Folder
+    md ./cpp/build
+) else (
+	echo ./cpp/build folder already exists
+)
 
 echo generating meta files
-cmake -S .\cpp -B .\cpp\build
+cmake -S ./cpp -B ./cpp/build
 
 echo building ...
-cmake --build .\cpp\build
+cmake --build ./cpp/build
 
 echo installing ...
-cmake --install .\cpp\build --config Debug
+cmake --install ./cpp/build --config Debug
 ::runas /user:Administrator "cmake --install .\cpp\build --config Debug"
 
 pause

cpp/CMakeLists.txt

@@ -30,5 +30,8 @@ enable_language(C CXX)
 
 find_package(Eigen3 3.3 REQUIRED NO_MODULE)
 
-add_subdirectory(kalman_filter)
+include(CTest)
+
+add_subdirectory(third_party/googletest)
+add_subdirectory(openkf)
 add_subdirectory(examples)

cpp/Examples/CMakeLists.txt → cpp/examples/CMakeLists.txt

@@ -1,8 +1,8 @@
-set(EXAMPLE_EXECUTABLE_PREFIX "example_")
-
-add_subdirectory(kf_state_estimation)
-add_subdirectory(ekf_range_sensor)
-add_subdirectory(unscented_transform)
-add_subdirectory(ukf_range_sensor)
-add_subdirectory(test_least_squares)
-add_subdirectory(sr_ukf_linear_function)
+set(EXAMPLE_EXECUTABLE_PREFIX "example_")
+
+add_subdirectory(kf_state_estimation)
+add_subdirectory(ekf_range_sensor)
+add_subdirectory(unscented_transform)
+add_subdirectory(ukf_range_sensor)
+add_subdirectory(test_least_squares)
+add_subdirectory(sr_ukf_linear_function)

cpp/Examples/ekf_range_sensor/CMakeLists.txt → cpp/examples/ekf_range_sensor/CMakeLists.txt

@@ -19,7 +19,12 @@ set(APPLICATION_NAME ${EXAMPLE_EXECUTABLE_PREFIX}_ekf_range_sensor)
 add_executable(${APPLICATION_NAME} ${PROJECT_FILES})
 
 set_target_properties(${APPLICATION_NAME} PROPERTIES LINKER_LANGUAGE CXX)
-target_link_libraries(${APPLICATION_NAME} PUBLIC Eigen3::Eigen)
+
+target_link_libraries(${APPLICATION_NAME}
+    PUBLIC
+        Eigen3::Eigen
+        OpenKF
+)
 
 target_include_directories(${APPLICATION_NAME} PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../../>

cpp/Examples/ekf_range_sensor/main.cpp → cpp/examples/ekf_range_sensor/main.cpp

@@ -1,75 +1,74 @@
-///
-/// 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 <mohanad.magdy.hammad@gmail.com>
-/// @file main.cpp
-///
-
-#include <iostream>
-#include <stdint.h>
-
-#include "kalman_filter/types.h"
-#include "kalman_filter/kalman_filter.h"
-
-#include "kalman_filter/unscented_transform.h"
-
-static constexpr size_t DIM_X{ 2 };
-static constexpr size_t DIM_Z{ 2 };
-
-static kf::KalmanFilter<DIM_X, DIM_Z> kalmanfilter;
-
-kf::Vector<DIM_Z> covertCartesian2Polar(const kf::Vector<DIM_X> & cartesian);
-kf::Matrix<DIM_Z, DIM_Z> calculateJacobianMatrix(const kf::Vector<DIM_X> & vecX);
-void executeCorrectionStep();
-
-int main(int argc, char ** argv)
-{
-    executeCorrectionStep();
-
-    return 0;
-}
-
-kf::Vector<DIM_Z> covertCartesian2Polar(const kf::Vector<DIM_X> & cartesian)
-{
-    const kf::Vector<DIM_Z> polar{
-        std::sqrt(cartesian[0] * cartesian[0] + cartesian[1] * cartesian[1]),
-        std::atan2(cartesian[1], cartesian[0])
-    };
-    return polar;
-}
-
-kf::Matrix<DIM_Z, DIM_Z> calculateJacobianMatrix(const kf::Vector<DIM_X> & vecX)
-{
-    const kf::float32_t valX2PlusY2{ (vecX[0] * vecX[0]) + (vecX[1] * vecX[1]) };
-    const kf::float32_t valSqrtX2PlusY2{ std::sqrt(valX2PlusY2) };
-
-    kf::Matrix<DIM_Z, DIM_Z> matHj;
-    matHj <<
-        (vecX[0] / valSqrtX2PlusY2), (vecX[1] / valSqrtX2PlusY2),
-        (-vecX[1] / valX2PlusY2), (vecX[0] / valX2PlusY2);
-
-    return matHj;
-}
-
-void executeCorrectionStep()
-{
-    kalmanfilter.vecX() << 10.0F, 5.0F;
-    kalmanfilter.matP() << 0.3F, 0.0F, 0.0F, 0.3F;
-
-    const kf::Vector<DIM_X> measPosCart{ 10.4F, 5.2F };
-    const kf::Vector<DIM_Z> vecZ{ covertCartesian2Polar(measPosCart) };
-
-    kf::Matrix<DIM_Z, DIM_Z> matR;
-    matR << 0.1F, 0.0F, 0.0F, 0.0008F;
-
-    kf::Matrix<DIM_Z, DIM_X> matHj{ calculateJacobianMatrix(kalmanfilter.vecX()) }; // jacobian matrix Hj
-
-    kalmanfilter.correctEkf(covertCartesian2Polar, vecZ, matR, matHj);
-
-    std::cout << "\ncorrected state vector = \n" << kalmanfilter.vecX() << "\n";
-    std::cout << "\ncorrected state covariance = \n" << kalmanfilter.matP() << "\n";
-}
+///
+/// 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 <mohanad.magdy.hammad@gmail.com>
+/// @file main.cpp
+///
+
+#include <iostream>
+#include <stdint.h>
+
+#include "types.h"
+#include "kalman_filter/kalman_filter.h"
+#include "kalman_filter/unscented_transform.h"
+
+static constexpr size_t DIM_X{ 2 };
+static constexpr size_t DIM_Z{ 2 };
+
+static kf::KalmanFilter<DIM_X, DIM_Z> kalmanfilter;
+
+kf::Vector<DIM_Z> covertCartesian2Polar(const kf::Vector<DIM_X> & cartesian);
+kf::Matrix<DIM_Z, DIM_Z> calculateJacobianMatrix(const kf::Vector<DIM_X> & vecX);
+void executeCorrectionStep();
+
+int main(int argc, char ** argv)
+{
+    executeCorrectionStep();
+
+    return 0;
+}
+
+kf::Vector<DIM_Z> covertCartesian2Polar(const kf::Vector<DIM_X> & cartesian)
+{
+    const kf::Vector<DIM_Z> polar{
+        std::sqrt(cartesian[0] * cartesian[0] + cartesian[1] * cartesian[1]),
+        std::atan2(cartesian[1], cartesian[0])
+    };
+    return polar;
+}
+
+kf::Matrix<DIM_Z, DIM_Z> calculateJacobianMatrix(const kf::Vector<DIM_X> & vecX)
+{
+    const kf::float32_t valX2PlusY2{ (vecX[0] * vecX[0]) + (vecX[1] * vecX[1]) };
+    const kf::float32_t valSqrtX2PlusY2{ std::sqrt(valX2PlusY2) };
+
+    kf::Matrix<DIM_Z, DIM_Z> matHj;
+    matHj <<
+        (vecX[0] / valSqrtX2PlusY2), (vecX[1] / valSqrtX2PlusY2),
+        (-vecX[1] / valX2PlusY2), (vecX[0] / valX2PlusY2);
+
+    return matHj;
+}
+
+void executeCorrectionStep()
+{
+    kalmanfilter.vecX() << 10.0F, 5.0F;
+    kalmanfilter.matP() << 0.3F, 0.0F, 0.0F, 0.3F;
+
+    const kf::Vector<DIM_X> measPosCart{ 10.4F, 5.2F };
+    const kf::Vector<DIM_Z> vecZ{ covertCartesian2Polar(measPosCart) };
+
+    kf::Matrix<DIM_Z, DIM_Z> matR;
+    matR << 0.1F, 0.0F, 0.0F, 0.0008F;
+
+    kf::Matrix<DIM_Z, DIM_X> matHj{ calculateJacobianMatrix(kalmanfilter.vecX()) }; // jacobian matrix Hj
+
+    kalmanfilter.correctEkf(covertCartesian2Polar, vecZ, matR, matHj);
+
+    std::cout << "\ncorrected state vector = \n" << kalmanfilter.vecX() << "\n";
+    std::cout << "\ncorrected state covariance = \n" << kalmanfilter.matP() << "\n";
+}

cpp/Examples/kf_state_estimation/CMakeLists.txt → cpp/examples/kf_state_estimation/CMakeLists.txt

@@ -19,7 +19,12 @@ set(APPLICATION_NAME ${EXAMPLE_EXECUTABLE_PREFIX}_kf_state_estimation)
 add_executable(${APPLICATION_NAME} ${PROJECT_FILES})
 
 set_target_properties(${APPLICATION_NAME} PROPERTIES LINKER_LANGUAGE CXX)
-target_link_libraries(${APPLICATION_NAME} PUBLIC Eigen3::Eigen)
+
+target_link_libraries(${APPLICATION_NAME}
+    PUBLIC
+        Eigen3::Eigen
+        OpenKF
+)
 
 target_include_directories(${APPLICATION_NAME} PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../../>

cpp/Examples/kf_state_estimation/main.cpp → cpp/examples/kf_state_estimation/main.cpp

@@ -1,70 +1,70 @@
-///
-/// 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 <mohanad.magdy.hammad@gmail.com>
-/// @file main.cpp
-///
-
-#include <iostream>
-#include <stdint.h>
-
-#include "kalman_filter/types.h"
-#include "kalman_filter/kalman_filter.h"
-
-static constexpr size_t DIM_X{ 2 };
-static constexpr size_t DIM_Z{ 1 };
-static constexpr kf::float32_t T{ 1.0F };
-static constexpr kf::float32_t Q11{ 0.1F }, Q22{ 0.1F };
-
-static kf::KalmanFilter<DIM_X, DIM_Z> kalmanfilter;
-
-void executePredictionStep();
-void executeCorrectionStep();
-
-int main(int argc, char ** argv)
-{
-    executePredictionStep();
-    executeCorrectionStep();
-
-    return 0;
-}
-
-void executePredictionStep()
-{
-    kalmanfilter.vecX() << 0.0F, 2.0F;
-    kalmanfilter.matP() << 0.1F, 0.0F, 0.0F, 0.1F;
-
-    kf::Matrix<DIM_X, DIM_X> F; // state transition matrix
-    F << 1.0F, T, 0.0F, 1.0F;
-
-    kf::Matrix<DIM_X, DIM_X> Q; // process noise covariance
-    Q(0, 0) = (Q11 * T) + (Q22 * (std::pow(T, 3) / 3.0F));
-    Q(0, 1) = Q(1, 0) = Q22 * (std::pow(T, 2) / 2.0F);
-    Q(1, 1) = Q22 * T;
-
-    kalmanfilter.predictLKF(F, Q); // execute prediction step
-
-    std::cout << "\npredicted state vector = \n" << kalmanfilter.vecX() << "\n";
-    std::cout << "\npredicted state covariance = \n" << kalmanfilter.matP() << "\n";
-}
-
-void executeCorrectionStep()
-{
-    kf::Vector<DIM_Z> vecZ;
-    vecZ << 2.25F;
-
-    kf::Matrix<DIM_Z, DIM_Z> matR;
-    matR << 0.01F;
-
-    kf::Matrix<DIM_Z, DIM_X> matH;
-    matH << 1.0F, 0.0F;
-
-    kalmanfilter.correctLKF(vecZ, matR, matH);
-
-    std::cout << "\ncorrected state vector = \n" << kalmanfilter.vecX() << "\n";
-    std::cout << "\ncorrected state covariance = \n" << kalmanfilter.matP() << "\n";
-}
+///
+/// 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 <mohanad.magdy.hammad@gmail.com>
+/// @file main.cpp
+///
+
+#include <iostream>
+#include <stdint.h>
+
+#include "types.h"
+#include "kalman_filter/kalman_filter.h"
+
+static constexpr size_t DIM_X{ 2 };
+static constexpr size_t DIM_Z{ 1 };
+static constexpr kf::float32_t T{ 1.0F };
+static constexpr kf::float32_t Q11{ 0.1F }, Q22{ 0.1F };
+
+static kf::KalmanFilter<DIM_X, DIM_Z> kalmanfilter;
+
+void executePredictionStep();
+void executeCorrectionStep();
+
+int main(int argc, char ** argv)
+{
+    executePredictionStep();
+    executeCorrectionStep();
+
+    return 0;
+}
+
+void executePredictionStep()
+{
+    kalmanfilter.vecX() << 0.0F, 2.0F;
+    kalmanfilter.matP() << 0.1F, 0.0F, 0.0F, 0.1F;
+
+    kf::Matrix<DIM_X, DIM_X> F; // state transition matrix
+    F << 1.0F, T, 0.0F, 1.0F;
+
+    kf::Matrix<DIM_X, DIM_X> Q; // process noise covariance
+    Q(0, 0) = (Q11 * T) + (Q22 * (std::pow(T, 3.0F) / 3.0F));
+    Q(0, 1) = Q(1, 0) = Q22 * (std::pow(T, 2.0F) / 2.0F);
+    Q(1, 1) = Q22 * T;
+
+    kalmanfilter.predictLKF(F, Q); // execute prediction step
+
+    std::cout << "\npredicted state vector = \n" << kalmanfilter.vecX() << "\n";
+    std::cout << "\npredicted state covariance = \n" << kalmanfilter.matP() << "\n";
+}
+
+void executeCorrectionStep()
+{
+    kf::Vector<DIM_Z> vecZ;
+    vecZ << 2.25F;
+
+    kf::Matrix<DIM_Z, DIM_Z> matR;
+    matR << 0.01F;
+
+    kf::Matrix<DIM_Z, DIM_X> matH;
+    matH << 1.0F, 0.0F;
+
+    kalmanfilter.correctLKF(vecZ, matR, matH);
+
+    std::cout << "\ncorrected state vector = \n" << kalmanfilter.vecX() << "\n";
+    std::cout << "\ncorrected state covariance = \n" << kalmanfilter.matP() << "\n";
+}

cpp/Examples/sr_ukf_linear_function/CMakeLists.txt → cpp/examples/sr_ukf_linear_function/CMakeLists.txt

@@ -19,7 +19,12 @@ set(APPLICATION_NAME ${EXAMPLE_EXECUTABLE_PREFIX}_sr_ukf_linear_function)
 add_executable(${APPLICATION_NAME} ${PROJECT_FILES})
 
 set_target_properties(${APPLICATION_NAME} PROPERTIES LINKER_LANGUAGE CXX)
-target_link_libraries(${APPLICATION_NAME} PUBLIC Eigen3::Eigen)
+
+target_link_libraries(${APPLICATION_NAME}
+    PUBLIC
+        Eigen3::Eigen
+        OpenKF
+)
 
 target_include_directories(${APPLICATION_NAME} PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../../>