diff --git a/bootstrap-openkf.bat b/bootstrap-openkf.bat
index ccf2a4f..a5399dc 100644
--- a/bootstrap-openkf.bat
+++ b/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
diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt
index 894feb9..55c6424 100644
--- a/cpp/CMakeLists.txt
+++ b/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)
diff --git a/cpp/Examples/CMakeLists.txt b/cpp/examples/CMakeLists.txt
similarity index 97%
rename from cpp/Examples/CMakeLists.txt
rename to cpp/examples/CMakeLists.txt
index 322ea8f..42bf1ac 100644
--- a/cpp/Examples/CMakeLists.txt
+++ b/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)
diff --git a/cpp/Examples/ekf_range_sensor/CMakeLists.txt b/cpp/examples/ekf_range_sensor/CMakeLists.txt
similarity index 88%
rename from cpp/Examples/ekf_range_sensor/CMakeLists.txt
rename to cpp/examples/ekf_range_sensor/CMakeLists.txt
index 09ae273..6236de0 100644
--- a/cpp/Examples/ekf_range_sensor/CMakeLists.txt
+++ b/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
$
diff --git a/cpp/Examples/ekf_range_sensor/main.cpp b/cpp/examples/ekf_range_sensor/main.cpp
similarity index 95%
rename from cpp/Examples/ekf_range_sensor/main.cpp
rename to cpp/examples/ekf_range_sensor/main.cpp
index 36d5e73..bfa21e8 100644
--- a/cpp/Examples/ekf_range_sensor/main.cpp
+++ b/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
-/// @file main.cpp
-///
-
-#include
-#include
-
-#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 kalmanfilter;
-
-kf::Vector covertCartesian2Polar(const kf::Vector & cartesian);
-kf::Matrix calculateJacobianMatrix(const kf::Vector & vecX);
-void executeCorrectionStep();
-
-int main(int argc, char ** argv)
-{
- executeCorrectionStep();
-
- return 0;
-}
-
-kf::Vector covertCartesian2Polar(const kf::Vector & cartesian)
-{
- const kf::Vector polar{
- std::sqrt(cartesian[0] * cartesian[0] + cartesian[1] * cartesian[1]),
- std::atan2(cartesian[1], cartesian[0])
- };
- return polar;
-}
-
-kf::Matrix calculateJacobianMatrix(const kf::Vector & vecX)
-{
- const kf::float32_t valX2PlusY2{ (vecX[0] * vecX[0]) + (vecX[1] * vecX[1]) };
- const kf::float32_t valSqrtX2PlusY2{ std::sqrt(valX2PlusY2) };
-
- kf::Matrix 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 measPosCart{ 10.4F, 5.2F };
- const kf::Vector vecZ{ covertCartesian2Polar(measPosCart) };
-
- kf::Matrix matR;
- matR << 0.1F, 0.0F, 0.0F, 0.0008F;
-
- kf::Matrix 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
+/// @file main.cpp
+///
+
+#include
+#include
+
+#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 kalmanfilter;
+
+kf::Vector covertCartesian2Polar(const kf::Vector & cartesian);
+kf::Matrix calculateJacobianMatrix(const kf::Vector & vecX);
+void executeCorrectionStep();
+
+int main(int argc, char ** argv)
+{
+ executeCorrectionStep();
+
+ return 0;
+}
+
+kf::Vector covertCartesian2Polar(const kf::Vector & cartesian)
+{
+ const kf::Vector polar{
+ std::sqrt(cartesian[0] * cartesian[0] + cartesian[1] * cartesian[1]),
+ std::atan2(cartesian[1], cartesian[0])
+ };
+ return polar;
+}
+
+kf::Matrix calculateJacobianMatrix(const kf::Vector & vecX)
+{
+ const kf::float32_t valX2PlusY2{ (vecX[0] * vecX[0]) + (vecX[1] * vecX[1]) };
+ const kf::float32_t valSqrtX2PlusY2{ std::sqrt(valX2PlusY2) };
+
+ kf::Matrix 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 measPosCart{ 10.4F, 5.2F };
+ const kf::Vector vecZ{ covertCartesian2Polar(measPosCart) };
+
+ kf::Matrix matR;
+ matR << 0.1F, 0.0F, 0.0F, 0.0008F;
+
+ kf::Matrix 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";
+}
diff --git a/cpp/Examples/kf_state_estimation/CMakeLists.txt b/cpp/examples/kf_state_estimation/CMakeLists.txt
similarity index 88%
rename from cpp/Examples/kf_state_estimation/CMakeLists.txt
rename to cpp/examples/kf_state_estimation/CMakeLists.txt
index d40497a..e2c1f4b 100644
--- a/cpp/Examples/kf_state_estimation/CMakeLists.txt
+++ b/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
$
diff --git a/cpp/Examples/kf_state_estimation/main.cpp b/cpp/examples/kf_state_estimation/main.cpp
similarity index 88%
rename from cpp/Examples/kf_state_estimation/main.cpp
rename to cpp/examples/kf_state_estimation/main.cpp
index 942aaf3..a7098d1 100644
--- a/cpp/Examples/kf_state_estimation/main.cpp
+++ b/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
-/// @file main.cpp
-///
-
-#include
-#include
-
-#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 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 F; // state transition matrix
- F << 1.0F, T, 0.0F, 1.0F;
-
- kf::Matrix 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 vecZ;
- vecZ << 2.25F;
-
- kf::Matrix matR;
- matR << 0.01F;
-
- kf::Matrix 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
+/// @file main.cpp
+///
+
+#include
+#include
+
+#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 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 F; // state transition matrix
+ F << 1.0F, T, 0.0F, 1.0F;
+
+ kf::Matrix 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 vecZ;
+ vecZ << 2.25F;
+
+ kf::Matrix matR;
+ matR << 0.01F;
+
+ kf::Matrix 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";
+}
diff --git a/cpp/Examples/sr_ukf_linear_function/CMakeLists.txt b/cpp/examples/sr_ukf_linear_function/CMakeLists.txt
similarity index 88%
rename from cpp/Examples/sr_ukf_linear_function/CMakeLists.txt
rename to cpp/examples/sr_ukf_linear_function/CMakeLists.txt
index 66db641..abc4e58 100644
--- a/cpp/Examples/sr_ukf_linear_function/CMakeLists.txt
+++ b/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
$
diff --git a/cpp/Examples/sr_ukf_linear_function/main.cpp b/cpp/examples/sr_ukf_linear_function/main.cpp
similarity index 94%
rename from cpp/Examples/sr_ukf_linear_function/main.cpp
rename to cpp/examples/sr_ukf_linear_function/main.cpp
index 53b0708..0157d4a 100644
--- a/cpp/Examples/sr_ukf_linear_function/main.cpp
+++ b/cpp/examples/sr_ukf_linear_function/main.cpp
@@ -1,97 +1,97 @@
-///
-/// 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 "kalman_filter/types.h"
-#include "kalman_filter/square_root_ukf.h"
-
-static constexpr size_t DIM_X{ 2 };
-static constexpr size_t DIM_Z{ 2 };
-
-void runExample1();
-
-kf::Vector funcF(const kf::Vector & x)
-{
- return x;
-}
-
-int main(int argc, char ** argv)
-{
- // example 1
- runExample1();
-
- return 0;
-}
-
-void runExample1()
-{
- std::cout << " Start of Example 1: ===========================" << std::endl;
-
- // initializations
- //x0 = np.array([1.0, 2.0])
- //P0 = np.array([[1.0, 0.5], [0.5, 1.0]])
- //Q = np.array([[0.5, 0.0], [0.0, 0.5]])
-
- //z = np.array([1.2, 1.8])
- //R = np.array([[0.3, 0.0], [0.0, 0.3]])
-
- kf::Vector x;
- x << 1.0F, 2.0F;
-
- kf::Matrix P;
- P << 1.0F, 0.5F,
- 0.5F, 1.0F;
-
- kf::Matrix Q;
- Q << 0.5F, 0.0F,
- 0.0F, 0.5F;
-
- kf::Vector z;
- z << 1.2F, 1.8F;
-
- kf::Matrix R;
- R << 0.3F, 0.0F,
- 0.0F, 0.3F;
-
- kf::SquareRootUKF srUkf;
- srUkf.initialize(x, P, Q, R);
-
- srUkf.predictSRUKF(funcF);
-
- std::cout << "x = \n" << srUkf.vecX() << std::endl;
- std::cout << "P = \n" << srUkf.matP() << std::endl;
-
- // Expectation from the python results:
- // =====================================
- //x1 =
- // [1. 2.]
- //P1 =
- // [[1.5 0.5]
- // [0.5 1.5]]
-
- srUkf.correctSRUKF(funcF, z);
-
- std::cout << "x = \n" << srUkf.vecX() << std::endl;
- std::cout << "P = \n" << srUkf.matP() << std::endl;
-
- // Expectations from the python results:
- // ======================================
- // x =
- // [1.15385 1.84615]
- // P =
- // [[ 0.24582 0.01505 ]
- // [ 0.01505 0.24582 ]]
-
- std::cout << " End of Example 1: ===========================" << std::endl;
-}
-
+///
+/// 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 "kalman_filter/square_root_ukf.h"
+
+static constexpr size_t DIM_X{ 2 };
+static constexpr size_t DIM_Z{ 2 };
+
+void runExample1();
+
+kf::Vector funcF(const kf::Vector & x)
+{
+ return x;
+}
+
+int main(int argc, char ** argv)
+{
+ // example 1
+ runExample1();
+
+ return 0;
+}
+
+void runExample1()
+{
+ std::cout << " Start of Example 1: ===========================" << std::endl;
+
+ // initializations
+ //x0 = np.array([1.0, 2.0])
+ //P0 = np.array([[1.0, 0.5], [0.5, 1.0]])
+ //Q = np.array([[0.5, 0.0], [0.0, 0.5]])
+
+ //z = np.array([1.2, 1.8])
+ //R = np.array([[0.3, 0.0], [0.0, 0.3]])
+
+ kf::Vector x;
+ x << 1.0F, 2.0F;
+
+ kf::Matrix P;
+ P << 1.0F, 0.5F,
+ 0.5F, 1.0F;
+
+ kf::Matrix Q;
+ Q << 0.5F, 0.0F,
+ 0.0F, 0.5F;
+
+ kf::Vector z;
+ z << 1.2F, 1.8F;
+
+ kf::Matrix R;
+ R << 0.3F, 0.0F,
+ 0.0F, 0.3F;
+
+ kf::SquareRootUKF srUkf;
+ srUkf.initialize(x, P, Q, R);
+
+ srUkf.predictSRUKF(funcF);
+
+ std::cout << "x = \n" << srUkf.vecX() << std::endl;
+ std::cout << "P = \n" << srUkf.matP() << std::endl;
+
+ // Expectation from the python results:
+ // =====================================
+ //x1 =
+ // [1. 2.]
+ //P1 =
+ // [[1.5 0.5]
+ // [0.5 1.5]]
+
+ srUkf.correctSRUKF(funcF, z);
+
+ std::cout << "x = \n" << srUkf.vecX() << std::endl;
+ std::cout << "P = \n" << srUkf.matP() << std::endl;
+
+ // Expectations from the python results:
+ // ======================================
+ // x =
+ // [1.15385 1.84615]
+ // P =
+ // [[ 0.24582 0.01505 ]
+ // [ 0.01505 0.24582 ]]
+
+ std::cout << " End of Example 1: ===========================" << std::endl;
+}
+
diff --git a/cpp/Examples/test_least_squares/CMakeLists.txt b/cpp/examples/test_least_squares/CMakeLists.txt
similarity index 88%
rename from cpp/Examples/test_least_squares/CMakeLists.txt
rename to cpp/examples/test_least_squares/CMakeLists.txt
index d60593d..2f429c7 100644
--- a/cpp/Examples/test_least_squares/CMakeLists.txt
+++ b/cpp/examples/test_least_squares/CMakeLists.txt
@@ -19,7 +19,12 @@ set(APPLICATION_NAME ${EXAMPLE_EXECUTABLE_PREFIX}_test_least_squares)
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
$
diff --git a/cpp/Examples/test_least_squares/main.cpp b/cpp/examples/test_least_squares/main.cpp
similarity index 93%
rename from cpp/Examples/test_least_squares/main.cpp
rename to cpp/examples/test_least_squares/main.cpp
index 8c305fe..8c81103 100644
--- a/cpp/Examples/test_least_squares/main.cpp
+++ b/cpp/examples/test_least_squares/main.cpp
@@ -1,112 +1,112 @@
-///
-/// 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 "kalman_filter/types.h"
-#include "kalman_filter/util.h"
-
-void runExample1();
-void runExample2();
-void runExample3();
-void runExample4();
-
-int main(int argc, char ** argv)
-{
- runExample1();
- runExample2();
- runExample3();
- runExample4();
-
- return 0;
-}
-
-void runExample1()
-{
- std::cout << " Start of Example 1: ===========================" << std::endl;
-
- kf::Matrix<3, 3> A;
- A << 3.0, 2.0, 1.0,
- 2.0, 3.0, 4.0,
- 1.0, 4.0, 3.0;
-
- kf::Matrix<2, 3> B;
- B << 5.0, 6.0, 7.0,
- 8.0, 9.0, 10.0;
-
- kf::util::JointRows<3, 2, 3> jmat(A, B);
- auto AB = jmat.jointMatrix();
-
- std::cout << "Joint Rows: AB = \n" << AB << std::endl;
-
- std::cout << " End of Example 1: ===========================" << std::endl;
-}
-
-void runExample2()
-{
- std::cout << " Start of Example 2: ===========================" << std::endl;
-
- kf::Matrix<3, 3> A;
- A << 3.0, 2.0, 1.0,
- 2.0, 3.0, 4.0,
- 1.0, 4.0, 3.0;
-
- kf::Matrix<3, 2> B;
- B << 5.0, 6.0,
- 7.0, 8.0,
- 9.0, 10.0;
-
- kf::util::JointCols<3, 3, 2> jmat(A, B);
- auto AB = jmat.jointMatrix();
-
- std::cout << "Joint Columns: AB = \n" << AB << std::endl;
-
- std::cout << " End of Example 2: ===========================" << std::endl;
-}
-
-void runExample3()
-{
- std::cout << " Start of Example 2: ===========================" << std::endl;
-
- kf::Matrix<3, 3> A;
- A << 1.0, -2.0, 1.0,
- 0.0, 1.0, 6.0,
- 0.0, 0.0, 1.0;
-
- kf::Matrix<3, 1> b;
- b << 4.0, -1.0, 2.0;
-
- auto x = kf::util::backwardSubstitute<3, 1>(A, b);
-
- std::cout << "Backward Substitution: x = \n" << x << std::endl;
-
- std::cout << " End of Example 2: ===========================" << std::endl;
-}
-
-void runExample4()
-{
- std::cout << " Start of Example 2: ===========================" << std::endl;
-
- kf::Matrix<3, 3> A;
- A << 1.0, 0.0, 0.0,
- -2.0, 1.0, 0.0,
- 1.0, 6.0, 1.0;
-
- kf::Matrix<3, 1> b;
- b << 4.0, -1.0, 2.0;
-
- auto x = kf::util::forwardSubstitute<3, 1>(A, b);
-
- std::cout << "Forward Substitution: x = \n" << x << std::endl;
-
- std::cout << " End of Example 2: ===========================" << std::endl;
-}
+///
+/// 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 "util.h"
+
+void runExample1();
+void runExample2();
+void runExample3();
+void runExample4();
+
+int main(int argc, char ** argv)
+{
+ runExample1();
+ runExample2();
+ runExample3();
+ runExample4();
+
+ return 0;
+}
+
+void runExample1()
+{
+ std::cout << " Start of Example 1: ===========================" << std::endl;
+
+ kf::Matrix<3, 3> A;
+ A << 3.0, 2.0, 1.0,
+ 2.0, 3.0, 4.0,
+ 1.0, 4.0, 3.0;
+
+ kf::Matrix<2, 3> B;
+ B << 5.0, 6.0, 7.0,
+ 8.0, 9.0, 10.0;
+
+ kf::util::JointRows<3, 2, 3> jmat(A, B);
+ auto AB = jmat.jointMatrix();
+
+ std::cout << "Joint Rows: AB = \n" << AB << std::endl;
+
+ std::cout << " End of Example 1: ===========================" << std::endl;
+}
+
+void runExample2()
+{
+ std::cout << " Start of Example 2: ===========================" << std::endl;
+
+ kf::Matrix<3, 3> A;
+ A << 3.0, 2.0, 1.0,
+ 2.0, 3.0, 4.0,
+ 1.0, 4.0, 3.0;
+
+ kf::Matrix<3, 2> B;
+ B << 5.0, 6.0,
+ 7.0, 8.0,
+ 9.0, 10.0;
+
+ kf::util::JointCols<3, 3, 2> jmat(A, B);
+ auto AB = jmat.jointMatrix();
+
+ std::cout << "Joint Columns: AB = \n" << AB << std::endl;
+
+ std::cout << " End of Example 2: ===========================" << std::endl;
+}
+
+void runExample3()
+{
+ std::cout << " Start of Example 2: ===========================" << std::endl;
+
+ kf::Matrix<3, 3> A;
+ A << 1.0, -2.0, 1.0,
+ 0.0, 1.0, 6.0,
+ 0.0, 0.0, 1.0;
+
+ kf::Matrix<3, 1> b;
+ b << 4.0, -1.0, 2.0;
+
+ auto x = kf::util::backwardSubstitute<3, 1>(A, b);
+
+ std::cout << "Backward Substitution: x = \n" << x << std::endl;
+
+ std::cout << " End of Example 2: ===========================" << std::endl;
+}
+
+void runExample4()
+{
+ std::cout << " Start of Example 2: ===========================" << std::endl;
+
+ kf::Matrix<3, 3> A;
+ A << 1.0, 0.0, 0.0,
+ -2.0, 1.0, 0.0,
+ 1.0, 6.0, 1.0;
+
+ kf::Matrix<3, 1> b;
+ b << 4.0, -1.0, 2.0;
+
+ auto x = kf::util::forwardSubstitute<3, 1>(A, b);
+
+ std::cout << "Forward Substitution: x = \n" << x << std::endl;
+
+ std::cout << " End of Example 2: ===========================" << std::endl;
+}
diff --git a/cpp/Examples/ukf_range_sensor/CMakeLists.txt b/cpp/examples/ukf_range_sensor/CMakeLists.txt
similarity index 88%
rename from cpp/Examples/ukf_range_sensor/CMakeLists.txt
rename to cpp/examples/ukf_range_sensor/CMakeLists.txt
index b971370..b1d0037 100644
--- a/cpp/Examples/ukf_range_sensor/CMakeLists.txt
+++ b/cpp/examples/ukf_range_sensor/CMakeLists.txt
@@ -19,7 +19,12 @@ set(APPLICATION_NAME ${EXAMPLE_EXECUTABLE_PREFIX}_ukf_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
$
diff --git a/cpp/Examples/ukf_range_sensor/main.cpp b/cpp/examples/ukf_range_sensor/main.cpp
similarity index 94%
rename from cpp/Examples/ukf_range_sensor/main.cpp
rename to cpp/examples/ukf_range_sensor/main.cpp
index 1e6514d..1f19d50 100644
--- a/cpp/Examples/ukf_range_sensor/main.cpp
+++ b/cpp/examples/ukf_range_sensor/main.cpp
@@ -1,120 +1,120 @@
-///
-/// 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 "kalman_filter/types.h"
-#include "kalman_filter/unscented_kalman_filter.h"
-
-static constexpr size_t DIM_X{ 4 };
-static constexpr size_t DIM_V{ 4 };
-static constexpr size_t DIM_Z{ 2 };
-static constexpr size_t DIM_N{ 2 };
-
-void runExample1();
-
-kf::Vector funcF(const kf::Vector & x, const kf::Vector & v)
-{
- kf::Vector y;
- y[0] = x[0] + x[2] + v[0];
- y[1] = x[1] + x[3] + v[1];
- y[2] = x[2] + v[2];
- y[3] = x[3] + v[3];
- return y;
-}
-
-kf::Vector funcH(const kf::Vector & x, const kf::Vector & n)
-{
- kf::Vector y;
-
- kf::float32_t px{ x[0] + n[0] };
- kf::float32_t py{ x[1] + n[1] };
-
- y[0] = std::sqrt((px * px) + (py * py));
- y[1] = std::atan(py / (px + std::numeric_limits::epsilon()));
- return y;
-}
-
-int main(int argc, char ** argv)
-{
- // example 1
- runExample1();
-
- return 0;
-}
-
-void runExample1()
-{
- std::cout << " Start of Example 1: ===========================" << std::endl;
-
- kf::Vector x;
- x << 2.0F, 1.0F, 0.0F, 0.0F;
-
- kf::Matrix P;
- P << 0.01F, 0.0F, 0.0F, 0.0F,
- 0.0F, 0.01F, 0.0F, 0.0F,
- 0.0F, 0.0F, 0.05F, 0.0F,
- 0.0F, 0.0F, 0.0F, 0.05F;
-
- kf::Matrix Q;
- Q << 0.05F, 0.0F, 0.0F, 0.0F,
- 0.0F, 0.05F, 0.0F, 0.0F,
- 0.0F, 0.0F, 0.1F, 0.0F,
- 0.0F, 0.0F, 0.0F, 0.1F;
-
- kf::Matrix R;
- R << 0.01F, 0.0F, 0.0F, 0.01F;
-
- kf::Vector z;
- z << 2.5, 0.05;
-
- kf::UnscentedKalmanFilter ukf;
-
- ukf.vecX() = x;
- ukf.matP() = P;
-
- ukf.setCovarianceQ(Q);
- ukf.setCovarianceR(R);
-
- ukf.predictUKF(funcF);
-
- std::cout << "x = \n" << ukf.vecX() << std::endl;
- std::cout << "P = \n" << ukf.matP() << std::endl;
-
- // Expectation from the python results:
- // =====================================
- // x =
- // [2.0 1.0 0.0 0.0]
- // P =
- // [[0.11 0.00 0.05 0.00]
- // [0.00 0.11 0.00 0.05]
- // [0.05 0.00 0.15 0.00]
- // [0.00 0.05 0.00 0.15]]
-
- ukf.correctUKF(funcH, z);
-
- std::cout << "x = \n" << ukf.vecX() << std::endl;
- std::cout << "P = \n" << ukf.matP() << std::endl;
-
- // Expectations from the python results:
- // ======================================
- // x =
- // [ 2.554 0.356 0.252 -0.293]
- // P =
- // [[ 0.01 -0.001 0.005 -0. ]
- // [-0.001 0.01 - 0. 0.005 ]
- // [ 0.005 - 0. 0.129 - 0. ]
- // [-0. 0.005 - 0. 0.129]]
-
- std::cout << " End of Example 1: ===========================" << std::endl;
-}
-
+///
+/// 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 "kalman_filter/unscented_kalman_filter.h"
+
+static constexpr size_t DIM_X{ 4 };
+static constexpr size_t DIM_V{ 4 };
+static constexpr size_t DIM_Z{ 2 };
+static constexpr size_t DIM_N{ 2 };
+
+void runExample1();
+
+kf::Vector funcF(const kf::Vector & x, const kf::Vector & v)
+{
+ kf::Vector y;
+ y[0] = x[0] + x[2] + v[0];
+ y[1] = x[1] + x[3] + v[1];
+ y[2] = x[2] + v[2];
+ y[3] = x[3] + v[3];
+ return y;
+}
+
+kf::Vector funcH(const kf::Vector & x, const kf::Vector & n)
+{
+ kf::Vector y;
+
+ kf::float32_t px{ x[0] + n[0] };
+ kf::float32_t py{ x[1] + n[1] };
+
+ y[0] = std::sqrt((px * px) + (py * py));
+ y[1] = std::atan(py / (px + std::numeric_limits::epsilon()));
+ return y;
+}
+
+int main(int argc, char ** argv)
+{
+ // example 1
+ runExample1();
+
+ return 0;
+}
+
+void runExample1()
+{
+ std::cout << " Start of Example 1: ===========================" << std::endl;
+
+ kf::Vector x;
+ x << 2.0F, 1.0F, 0.0F, 0.0F;
+
+ kf::Matrix P;
+ P << 0.01F, 0.0F, 0.0F, 0.0F,
+ 0.0F, 0.01F, 0.0F, 0.0F,
+ 0.0F, 0.0F, 0.05F, 0.0F,
+ 0.0F, 0.0F, 0.0F, 0.05F;
+
+ kf::Matrix Q;
+ Q << 0.05F, 0.0F, 0.0F, 0.0F,
+ 0.0F, 0.05F, 0.0F, 0.0F,
+ 0.0F, 0.0F, 0.1F, 0.0F,
+ 0.0F, 0.0F, 0.0F, 0.1F;
+
+ kf::Matrix R;
+ R << 0.01F, 0.0F, 0.0F, 0.01F;
+
+ kf::Vector z;
+ z << 2.5F, 0.05F;
+
+ kf::UnscentedKalmanFilter ukf;
+
+ ukf.vecX() = x;
+ ukf.matP() = P;
+
+ ukf.setCovarianceQ(Q);
+ ukf.setCovarianceR(R);
+
+ ukf.predictUKF(funcF);
+
+ std::cout << "x = \n" << ukf.vecX() << std::endl;
+ std::cout << "P = \n" << ukf.matP() << std::endl;
+
+ // Expectation from the python results:
+ // =====================================
+ // x =
+ // [2.0 1.0 0.0 0.0]
+ // P =
+ // [[0.11 0.00 0.05 0.00]
+ // [0.00 0.11 0.00 0.05]
+ // [0.05 0.00 0.15 0.00]
+ // [0.00 0.05 0.00 0.15]]
+
+ ukf.correctUKF(funcH, z);
+
+ std::cout << "x = \n" << ukf.vecX() << std::endl;
+ std::cout << "P = \n" << ukf.matP() << std::endl;
+
+ // Expectations from the python results:
+ // ======================================
+ // x =
+ // [ 2.554 0.356 0.252 -0.293]
+ // P =
+ // [[ 0.01 -0.001 0.005 -0. ]
+ // [-0.001 0.01 - 0. 0.005 ]
+ // [ 0.005 - 0. 0.129 - 0. ]
+ // [-0. 0.005 - 0. 0.129]]
+
+ std::cout << " End of Example 1: ===========================" << std::endl;
+}
+
diff --git a/cpp/Examples/unscented_transform/CMakeLists.txt b/cpp/examples/unscented_transform/CMakeLists.txt
similarity index 88%
rename from cpp/Examples/unscented_transform/CMakeLists.txt
rename to cpp/examples/unscented_transform/CMakeLists.txt
index fad0660..458aef1 100644
--- a/cpp/Examples/unscented_transform/CMakeLists.txt
+++ b/cpp/examples/unscented_transform/CMakeLists.txt
@@ -19,7 +19,12 @@ set(APPLICATION_NAME ${EXAMPLE_EXECUTABLE_PREFIX}_unscented_transform)
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
$
diff --git a/cpp/Examples/unscented_transform/main.cpp b/cpp/examples/unscented_transform/main.cpp
similarity index 94%
rename from cpp/Examples/unscented_transform/main.cpp
rename to cpp/examples/unscented_transform/main.cpp
index 2f3b9ae..8c18d64 100644
--- a/cpp/Examples/unscented_transform/main.cpp
+++ b/cpp/examples/unscented_transform/main.cpp
@@ -1,100 +1,99 @@
-///
-/// 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 "kalman_filter/types.h"
-#include "kalman_filter/kalman_filter.h"
-
-#include "kalman_filter/unscented_transform.h"
-
-static constexpr size_t DIM_1{ 1 };
-static constexpr size_t DIM_2{ 2 };
-
-void runExample1();
-void runExample2();
-
-kf::Vector function1(const kf::Vector & x)
-{
- kf::Vector y;
- y[0] = x[0] * x[0];
- return y;
-}
-
-kf::Vector function2(const kf::Vector & x)
-{
- kf::Vector y;
- y[0] = x[0] * x[0];
- y[1] = x[1] * x[1];
- return y;
-}
-
-int main(int argc, char ** argv)
-{
- // example 1
- runExample1();
-
- // example 2
- runExample2();
-
- return 0;
-}
-
-void runExample1()
-{
- std::cout << " Start of Example 1: ===========================" << std::endl;
-
- kf::Vector x;
- x << 0.0F;
-
- kf::Matrix P;
- P << 0.5F;
-
- kf::UnscentedTransform UT;
- UT.compute(x, P, 0.0F);
-
- kf::Vector vecY;
- kf::Matrix matPyy;
-
- UT.transform(function1, vecY, matPyy);
-
- UT.showSummary();
- std::cout << "vecY: \n" << vecY << "\n";
- std::cout << "matPyy: \n" << matPyy << "\n";
-
- std::cout << " End of Example 1: ===========================" << std::endl;
-}
-
-void runExample2()
-{
- std::cout << " Start of Example 2: ===========================" << std::endl;
-
- kf::Vector x;
- x << 2.0F, 1.0F;
-
- kf::Matrix P;
- P << 0.1F, 0.0F, 0.0F, 0.1F;
-
- kf::UnscentedTransform UT;
- UT.compute(x, P, 0.0F);
-
- kf::Vector vecY;
- kf::Matrix matPyy;
-
- UT.transform(function2, vecY, matPyy);
-
- UT.showSummary();
- std::cout << "vecY: \n" << vecY << "\n";
- std::cout << "matPyy: \n" << matPyy << "\n";
-
- std::cout << " End of Example 2: ===========================" << std::endl;
-}
+///
+/// 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 "kalman_filter/kalman_filter.h"
+#include "kalman_filter/unscented_transform.h"
+
+static constexpr size_t DIM_1{ 1 };
+static constexpr size_t DIM_2{ 2 };
+
+void runExample1();
+void runExample2();
+
+kf::Vector function1(const kf::Vector & x)
+{
+ kf::Vector y;
+ y[0] = x[0] * x[0];
+ return y;
+}
+
+kf::Vector function2(const kf::Vector & x)
+{
+ kf::Vector y;
+ y[0] = x[0] * x[0];
+ y[1] = x[1] * x[1];
+ return y;
+}
+
+int main(int argc, char ** argv)
+{
+ // example 1
+ runExample1();
+
+ // example 2
+ runExample2();
+
+ return 0;
+}
+
+void runExample1()
+{
+ std::cout << " Start of Example 1: ===========================" << std::endl;
+
+ kf::Vector x;
+ x << 0.0F;
+
+ kf::Matrix P;
+ P << 0.5F;
+
+ kf::UnscentedTransform UT;
+ UT.compute(x, P, 0.0F);
+
+ kf::Vector vecY;
+ kf::Matrix matPyy;
+
+ UT.transform(function1, vecY, matPyy);
+
+ UT.showSummary();
+ std::cout << "vecY: \n" << vecY << "\n";
+ std::cout << "matPyy: \n" << matPyy << "\n";
+
+ std::cout << " End of Example 1: ===========================" << std::endl;
+}
+
+void runExample2()
+{
+ std::cout << " Start of Example 2: ===========================" << std::endl;
+
+ kf::Vector x;
+ x << 2.0F, 1.0F;
+
+ kf::Matrix P;
+ P << 0.1F, 0.0F, 0.0F, 0.1F;
+
+ kf::UnscentedTransform UT;
+ UT.compute(x, P, 0.0F);
+
+ kf::Vector vecY;
+ kf::Matrix matPyy;
+
+ UT.transform(function2, vecY, matPyy);
+
+ UT.showSummary();
+ std::cout << "vecY: \n" << vecY << "\n";
+ std::cout << "matPyy: \n" << matPyy << "\n";
+
+ std::cout << " End of Example 2: ===========================" << std::endl;
+}
diff --git a/cpp/kalman_filter/CMakeLists.txt b/cpp/openkf/CMakeLists.txt
similarity index 80%
rename from cpp/kalman_filter/CMakeLists.txt
rename to cpp/openkf/CMakeLists.txt
index 79272f4..f1484a1 100644
--- a/cpp/kalman_filter/CMakeLists.txt
+++ b/cpp/openkf/CMakeLists.txt
@@ -1,81 +1,90 @@
-##
-## 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 LIBRARY_FILES
-# "${CMAKE_CURRENT_SOURCE_DIR}/*.h"
-# "${CMAKE_CURRENT_SOURCE_DIR}/*.cpp"
-# )
-
-set(LIBRARY_SRC_FILES
- dummy.cpp
-)
-
-set(LIBRARY_HDR_FILES
- types.h
- util.h
- kalman_filter.h
- unscented_transform.h
- unscented_kalman_filter.h
- square_root_ukf.h
-)
-
-set(LIBRARY_NAME ${PROJECT_NAME})
-
-add_library(${LIBRARY_NAME} ${LIBRARY_SRC_FILES} ${LIBRARY_HDR_FILES})
-
-set_target_properties(${LIBRARY_NAME} PROPERTIES LINKER_LANGUAGE CXX)
-target_link_libraries(${LIBRARY_NAME} PUBLIC Eigen3::Eigen)
-
-target_include_directories(${LIBRARY_NAME} PUBLIC
- $
- $
-)
-
-include(CMakePackageConfigHelpers)
-
-configure_package_config_file(
- ${CMAKE_CURRENT_SOURCE_DIR}/conf/Config.cmake.in
- ${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}Config.cmake
- INSTALL_DESTINATION ${CONFIG_INSTALL_DIR}
- PATH_VARS INCLUDE_FOLDER
-)
-
-write_basic_package_version_file(
- ${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}ConfigVersion.cmake
- VERSION 1.0.0
- COMPATIBILITY SameMajorVersion
-)
-
-install(
- FILES ${LIBRARY_HDR_FILES}
- DESTINATION ${INCLUDE_INSTALL_DIR}
-)
-
-install(
- FILES ${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}Config.cmake
- ${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}ConfigVersion.cmake
- DESTINATION ${CONFIG_INSTALL_DIR}
-)
-
-install(
- TARGETS ${LIBRARY_NAME}
- EXPORT "${TARGETS_EXPORT_NAME}"
- LIBRARY DESTINATION "${LIBRARY_INSTALL_DIR}"
- ARCHIVE DESTINATION "${LIBRARY_INSTALL_DIR}"
- INCLUDES DESTINATION "${INCLUDE_FOLDER}"
-)
-
-# Config
-# * /lib/cmake/OpenKF/OpenKFTargets.cmake
-install(
- EXPORT ${TARGETS_EXPORT_NAME}
- DESTINATION ${CONFIG_INSTALL_DIR}
-)
\ No newline at end of file
+##
+## 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 LIBRARY_FILES
+# "${CMAKE_CURRENT_SOURCE_DIR}/*.h"
+# "${CMAKE_CURRENT_SOURCE_DIR}/*.cpp"
+# )
+
+set(LIBRARY_SRC_FILES
+ dummy.cpp
+)
+
+set(LIBRARY_HDR_FILES
+ types.h
+ util.h
+ kalman_filter/kalman_filter.h
+ kalman_filter/unscented_transform.h
+ kalman_filter/unscented_kalman_filter.h
+ kalman_filter/square_root_ukf.h
+)
+
+set(LIBRARY_NAME ${PROJECT_NAME})
+
+add_library(${LIBRARY_NAME} ${LIBRARY_SRC_FILES} ${LIBRARY_HDR_FILES})
+
+set_target_properties(${LIBRARY_NAME} PROPERTIES LINKER_LANGUAGE CXX)
+target_link_libraries(${LIBRARY_NAME} PUBLIC Eigen3::Eigen)
+
+if (MSVC)
+ # https://stackoverflow.com/a/18635749
+ add_compile_options(-MTd)
+ set_property(TARGET ${LIBRARY_NAME} PROPERTY
+ MSVC_RUNTIME_LIBRARY "MultiThreaded$<$:Debug>")
+endif(MSVC)
+
+target_include_directories(${LIBRARY_NAME} PUBLIC
+ $
+ $
+)
+
+include(CMakePackageConfigHelpers)
+
+configure_package_config_file(
+ ${CMAKE_CURRENT_SOURCE_DIR}/conf/Config.cmake.in
+ ${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}Config.cmake
+ INSTALL_DESTINATION ${CONFIG_INSTALL_DIR}
+ PATH_VARS INCLUDE_FOLDER
+)
+
+write_basic_package_version_file(
+ ${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}ConfigVersion.cmake
+ VERSION 1.0.0
+ COMPATIBILITY SameMajorVersion
+)
+
+install(
+ FILES ${LIBRARY_HDR_FILES}
+ DESTINATION ${INCLUDE_INSTALL_DIR}
+)
+
+install(
+ FILES ${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}Config.cmake
+ ${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}ConfigVersion.cmake
+ DESTINATION ${CONFIG_INSTALL_DIR}
+)
+
+install(
+ TARGETS ${LIBRARY_NAME}
+ EXPORT "${TARGETS_EXPORT_NAME}"
+ LIBRARY DESTINATION "${LIBRARY_INSTALL_DIR}"
+ ARCHIVE DESTINATION "${LIBRARY_INSTALL_DIR}"
+ INCLUDES DESTINATION "${INCLUDE_FOLDER}"
+)
+
+# Config
+# * /lib/cmake/OpenKF/OpenKFTargets.cmake
+install(
+ EXPORT ${TARGETS_EXPORT_NAME}
+ DESTINATION ${CONFIG_INSTALL_DIR}
+)
+
+add_subdirectory(tests)
\ No newline at end of file
diff --git a/cpp/kalman_filter/conf/Config.cmake.in b/cpp/openkf/conf/Config.cmake.in
similarity index 97%
rename from cpp/kalman_filter/conf/Config.cmake.in
rename to cpp/openkf/conf/Config.cmake.in
index da23610..43aad96 100644
--- a/cpp/kalman_filter/conf/Config.cmake.in
+++ b/cpp/openkf/conf/Config.cmake.in
@@ -1,6 +1,6 @@
-@PACKAGE_INIT@
-
-set_and_check(OPENKF_INCLUDE_DIR "@PACKAGE_INCLUDE_FOLDER@")
-include( "${CMAKE_CURRENT_LIST_DIR}/OpenKFTargets.cmake" )
-
+@PACKAGE_INIT@
+
+set_and_check(OPENKF_INCLUDE_DIR "@PACKAGE_INCLUDE_FOLDER@")
+include( "${CMAKE_CURRENT_LIST_DIR}/OpenKFTargets.cmake" )
+
check_required_components(OpenKF)
\ No newline at end of file
diff --git a/cpp/kalman_filter/dummy.cpp b/cpp/openkf/dummy.cpp
similarity index 94%
rename from cpp/kalman_filter/dummy.cpp
rename to cpp/openkf/dummy.cpp
index e13446f..7827c80 100644
--- a/cpp/kalman_filter/dummy.cpp
+++ b/cpp/openkf/dummy.cpp
@@ -1,2 +1,2 @@
-
-static void dummyFunction() {}
+
+static void dummyFunction() {}
diff --git a/cpp/kalman_filter/kalman_filter.h b/cpp/openkf/kalman_filter/kalman_filter.h
similarity index 96%
rename from cpp/kalman_filter/kalman_filter.h
rename to cpp/openkf/kalman_filter/kalman_filter.h
index 6efc155..38272ce 100644
--- a/cpp/kalman_filter/kalman_filter.h
+++ b/cpp/openkf/kalman_filter/kalman_filter.h
@@ -16,7 +16,7 @@
namespace kf
{
- template
+ template
class KalmanFilter
{
public:
diff --git a/cpp/kalman_filter/square_root_ukf.h b/cpp/openkf/kalman_filter/square_root_ukf.h
similarity index 93%
rename from cpp/kalman_filter/square_root_ukf.h
rename to cpp/openkf/kalman_filter/square_root_ukf.h
index bfa6c4e..e77a595 100644
--- a/cpp/kalman_filter/square_root_ukf.h
+++ b/cpp/openkf/kalman_filter/square_root_ukf.h
@@ -23,7 +23,8 @@ namespace kf
public:
static constexpr int32_t SIGMA_DIM{ 2 * DIM_X + 1 };
- SquareRootUKF() : KalmanFilter()
+ SquareRootUKF()
+ : KalmanFilter()
{
// calculate weights
const float32_t kappa{ static_cast(3 - DIM_X) };
@@ -87,7 +88,7 @@ namespace kf
void setCovarianceR(const Matrix & matR)
{
// cholesky factorization to get matrix R square-root
- Eigen::LLT> lltOfRn(matQ);
+ Eigen::LLT> lltOfRn(matR);
m_matRn = lltOfRn.matrixL(); // sqrt(R)
}
@@ -297,12 +298,17 @@ namespace kf
// _, S_minus = np.linalg.qr(C)
Eigen::HouseholderQR< Matrix > qr(matC);
+ Matrix qrMatrixUpper{ qr.matrixQR() };
+ qrMatrixUpper = util::getUpperTriangulerView(qrMatrixUpper);
+
// get the upper triangular matrix from the factorization
// might need to reimplement QR factorization it as it seems to use dynamic memory allocation
- Matrix matR{
- util::getBlock(
- qr.matrixQR().triangularView(), 0, 0)
- };
+ // Matrix matR{
+ // util::getBlock(
+ // qr.matrixQR().triangularView(), 0, 0)
+ // };
+
+ Matrix matR{ util::getBlock(qrMatrixUpper, 0, 0) };
// Rank - 1 cholesky update
// x_dev = sigmas_X[:, 0] - x_minus
diff --git a/cpp/kalman_filter/unscented_kalman_filter.h b/cpp/openkf/kalman_filter/unscented_kalman_filter.h
similarity index 88%
rename from cpp/kalman_filter/unscented_kalman_filter.h
rename to cpp/openkf/kalman_filter/unscented_kalman_filter.h
index 7cf1be0..d1055fc 100644
--- a/cpp/kalman_filter/unscented_kalman_filter.h
+++ b/cpp/openkf/kalman_filter/unscented_kalman_filter.h
@@ -24,7 +24,8 @@ namespace kf
static constexpr int32_t DIM_A{ DIM_X + DIM_V + DIM_N };
static constexpr int32_t SIGMA_DIM{ 2 * DIM_A + 1 };
- UnscentedKalmanFilter() : KalmanFilter()
+ UnscentedKalmanFilter()
+ : KalmanFilter()
{
// 1. calculate weights
const float32_t kappa{ static_cast(3 - DIM_A) };
@@ -95,10 +96,11 @@ namespace kf
Matrix matSigmaXa{ calculateSigmaPoints(m_vecXa, m_matPa, kappa) };
// xx_sigmas = xa_sigmas[:self.dim_x, :]
- Matrix sigmaXx{ matSigmaXa.block(0, 0) };
+ //Matrix sigmaXx{ matSigmaXa.block(0, 0) };
+ Matrix sigmaXx{ matSigmaXa.block(0, 0, DIM_X, SIGMA_DIM) };
// xv_sigmas = xa_sigmas[self.dim_x : self.dim_x + self.dim_v, :]
- Matrix sigmaXv{ matSigmaXa.block(DIM_X, 0) };
+ Matrix sigmaXv{ matSigmaXa.block(DIM_X, 0, DIM_V, SIGMA_DIM) };
// y_sigmas = np.zeros((self.dim_x, self.n_sigma))
// for i in range(self.n_sigma):
@@ -139,10 +141,10 @@ namespace kf
Matrix matSigmaXa{ calculateSigmaPoints(m_vecXa, m_matPa, kappa) };
// xx_sigmas = xa_sigmas[:self.dim_x, :]
- Matrix sigmaXx{ matSigmaXa.block(0, 0) };
+ Matrix sigmaXx{ matSigmaXa.block(0, 0, DIM_X, SIGMA_DIM) };
// xn_sigmas = xa_sigmas[self.dim_x + self.dim_v :, :]
- Matrix sigmaXn{ matSigmaXa.block(DIM_X + DIM_V, 0) };
+ Matrix sigmaXn{ matSigmaXa.block(DIM_X + DIM_V, 0, DIM_N, SIGMA_DIM) };
// y_sigmas = np.zeros((self.dim_z, self.n_sigma))
// for i in range(self.n_sigma) :
@@ -272,25 +274,25 @@ namespace kf
/// @param vecX output weighted mean
/// @param matP output weighted covariance
///
- template
- void calculateWeightedMeanAndCovariance(const Matrix & sigmaX, Vector & vecX, Matrix & matPxx)
+ template
+ void calculateWeightedMeanAndCovariance(const Matrix & sigmaX, Vector & vecX, Matrix & matPxx)
{
// 1. calculate mean: \bar{y} = \sum_{i_0}^{2n} W[0, i] Y[:, i]
- vecX = m_weight0 * util::getColumnAt(0, sigmaX);
+ vecX = m_weight0 * util::getColumnAt(0, sigmaX);
for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
{
- vecX += m_weighti * util::getColumnAt(i, sigmaX); // y += W[0, i] Y[:, i]
+ vecX += m_weighti * util::getColumnAt(i, sigmaX); // y += W[0, i] Y[:, i]
}
// 2. calculate covariance: P_{yy} = \sum_{i_0}^{2n} W[0, i] (Y[:, i] - \bar{y}) (Y[:, i] - \bar{y})^T
- Vector devXi{ util::getColumnAt(0, sigmaX) - vecX }; // Y[:, 0] - \bar{ y }
+ Vector devXi{ util::getColumnAt(0, sigmaX) - vecX }; // Y[:, 0] - \bar{ y }
matPxx = m_weight0 * devXi * devXi.transpose(); // P_0 = W[0, 0] (Y[:, 0] - \bar{y}) (Y[:, 0] - \bar{y})^T
for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
{
- devXi = util::getColumnAt(i, sigmaX) - vecX; // Y[:, i] - \bar{y}
+ devXi = util::getColumnAt(i, sigmaX) - vecX; // Y[:, i] - \bar{y}
- const Matrix Pi{ m_weighti * devXi * devXi.transpose() }; // P_i = W[0, i] (Y[:, i] - \bar{y}) (Y[:, i] - \bar{y})^T
+ const Matrix Pi{ m_weighti * devXi * devXi.transpose() }; // P_i = W[0, i] (Y[:, i] - \bar{y}) (Y[:, i] - \bar{y})^T
matPxx += Pi; // y += W[0, i] (Y[:, i] - \bar{y}) (Y[:, i] - \bar{y})^T
}
diff --git a/cpp/kalman_filter/unscented_transform.h b/cpp/openkf/kalman_filter/unscented_transform.h
similarity index 80%
rename from cpp/kalman_filter/unscented_transform.h
rename to cpp/openkf/kalman_filter/unscented_transform.h
index 87538c3..220c4bd 100644
--- a/cpp/kalman_filter/unscented_transform.h
+++ b/cpp/openkf/kalman_filter/unscented_transform.h
@@ -27,8 +27,8 @@ namespace kf
UnscentedTransform() {}
~UnscentedTransform() {}
- float32_t weight0() const { return _weight0; }
- float32_t weighti() const { return _weighti; }
+ float32_t weight0() const { return _weights[0]; }
+ float32_t weighti() const { return _weights[1]; }
///
/// @brief algorithm to execute weight and sigma points calculation
@@ -49,21 +49,21 @@ namespace kf
void calculateWeightedMeanAndCovariance(const Matrix & sigmaX, Vector & vecX, Matrix & matPxx)
{
// 1. calculate mean: \bar{y} = \sum_{i_0}^{2n} W[0, i] Y[:, i]
- vecX = _weight0 * util::getColumnAt(0, sigmaX);
+ vecX = _weights[0] * util::getColumnAt(0, sigmaX);
for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
{
- vecX += _weighti * util::getColumnAt(i, sigmaX); // y += W[0, i] Y[:, i]
+ vecX += _weights[1] * util::getColumnAt(i, sigmaX); // y += W[0, i] Y[:, i]
}
// 2. calculate covariance: P_{yy} = \sum_{i_0}^{2n} W[0, i] (Y[:, i] - \bar{y}) (Y[:, i] - \bar{y})^T
Vector devXi{ util::getColumnAt(0, sigmaX) - vecX }; // Y[:, 0] - \bar{ y }
- matPxx = _weight0 * devXi * devXi.transpose(); // P_0 = W[0, 0] (Y[:, 0] - \bar{y}) (Y[:, 0] - \bar{y})^T
+ matPxx = _weights[0] * devXi * devXi.transpose(); // P_0 = W[0, 0] (Y[:, 0] - \bar{y}) (Y[:, 0] - \bar{y})^T
for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
{
devXi = util::getColumnAt(i, sigmaX) - vecX; // Y[:, i] - \bar{y}
- const Matrix Pi{ _weighti * devXi * devXi.transpose() }; // P_i = W[0, i] (Y[:, i] - \bar{y}) (Y[:, i] - \bar{y})^T
+ const Matrix Pi{ _weights[1] * devXi * devXi.transpose() }; // P_i = W[0, i] (Y[:, i] - \bar{y}) (Y[:, i] - \bar{y})^T
matPxx += Pi; // y += W[0, i] (Y[:, i] - \bar{y}) (Y[:, i] - \bar{y})^T
}
@@ -92,22 +92,23 @@ namespace kf
{
std::cout << "DIM_N : " << DIM << "\n";
std::cout << "DIM_M : " << SIGMA_DIM << "\n";
- std::cout << "_weight0 : \n" << _weight0 << "\n";
- std::cout << "_weighti : \n" << _weighti << "\n";
+ std::cout << "_weights[0] : \n" << _weights[0] << "\n";
+ std::cout << "_weights[1] : \n" << _weights[1] << "\n";
std::cout << "_sigmaX : \n" << _sigmaX << "\n";
}
- Matrix<1, SIGMA_DIM> & weights() { return _weights; }
- const Matrix<1, SIGMA_DIM> & weights() const { return _weights; }
+ Matrix<1, 2> & weights() { return _weights; }
+ const Matrix<1, 2> & weights() const { return _weights; }
Matrix & sigmaX() { return _sigmaX; }
const Matrix & sigmaX() const { return _sigmaX; }
private:
- float32_t _weight0; /// @brief unscented transform weight 0 for mean
- float32_t _weighti; /// @brief unscented transform weight 0 for none mean samples
+ /// @brief unscented transform weight 0 for mean
+ Matrix<1, 2> _weights;
- Matrix _sigmaX; /// @brief input sigma points
+ /// @brief input sigma points
+ Matrix _sigmaX;
///
/// @brief algorithm to calculate the weights used to draw the sigma points
@@ -119,8 +120,8 @@ namespace kf
const float32_t denoTerm{ kappa + static_cast(DIM) };
- _weight0 = kappa / denoTerm;
- _weighti = 0.5F / denoTerm;
+ _weights[0] = kappa / denoTerm;
+ _weights[1] = 0.5F / denoTerm;
}
///
@@ -185,21 +186,21 @@ namespace kf
void updateTransformedMeanAndCovariance(const Matrix & sigmaY, Vector & vecY, Matrix & matPyy)
{
// 1. calculate mean: \bar{y} = \sum_{i_0}^{2n} W[0, i] Y[:, i]
- vecY = _weight0 * util::getColumnAt(0, sigmaY);
+ vecY = _weights[0] * util::getColumnAt(0, sigmaY);
for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
{
- vecY += _weighti * util::getColumnAt(i, sigmaY); // y += W[0, i] Y[:, i]
+ vecY += _weights[1] * util::getColumnAt(i, sigmaY); // y += W[0, i] Y[:, i]
}
// 2. calculate covariance: P_{yy} = \sum_{i_0}^{2n} W[0, i] (Y[:, i] - \bar{y}) (Y[:, i] - \bar{y})^T
Vector devYi{ util::getColumnAt(0, sigmaY) - vecY }; // Y[:, 0] - \bar{ y }
- matPyy = _weight0 * devYi * devYi.transpose(); // P_0 = W[0, 0] (Y[:, 0] - \bar{y}) (Y[:, 0] - \bar{y})^T
+ matPyy = _weights[0] * devYi * devYi.transpose(); // P_0 = W[0, 0] (Y[:, 0] - \bar{y}) (Y[:, 0] - \bar{y})^T
for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
{
devYi = util::getColumnAt(i, sigmaY) - vecY; // Y[:, i] - \bar{y}
- const Matrix Pi{ _weighti * devYi * devYi.transpose() }; // P_i = W[0, i] (Y[:, i] - \bar{y}) (Y[:, i] - \bar{y})^T
+ const Matrix Pi{ _weights[1] * devYi * devYi.transpose() }; // P_i = W[0, i] (Y[:, i] - \bar{y}) (Y[:, i] - \bar{y})^T
matPyy += Pi; // y += W[0, i] (Y[:, i] - \bar{y}) (Y[:, i] - \bar{y})^T
}
diff --git a/cpp/openkf/tests/CMakeLists.txt b/cpp/openkf/tests/CMakeLists.txt
new file mode 100644
index 0000000..9607488
--- /dev/null
+++ b/cpp/openkf/tests/CMakeLists.txt
@@ -0,0 +1,32 @@
+if (BUILD_TESTING)
+ cmake_minimum_required(VERSION 3.4)
+ project(unit_tests)
+
+ enable_testing()
+ #find_package(GTest REQUIRED)
+ #include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR})
+
+ add_executable(${PROJECT_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(${PROJECT_NAME} PUBLIC
+ GTest::gtest_main
+ OpenKF
+ )
+
+ if (MSVC)
+ # https://stackoverflow.com/a/18635749
+ add_compile_options(-MTd)
+ set_property(TARGET ${PROJECT_NAME} PROPERTY
+ MSVC_RUNTIME_LIBRARY "MultiThreaded$<$:Debug>")
+ endif(MSVC)
+
+ include(GoogleTest)
+ gtest_discover_tests(${PROJECT_NAME})
+
+endif(BUILD_TESTING)
diff --git a/cpp/openkf/tests/kalman_filter_test.cpp b/cpp/openkf/tests/kalman_filter_test.cpp
new file mode 100644
index 0000000..c8f93cb
--- /dev/null
+++ b/cpp/openkf/tests/kalman_filter_test.cpp
@@ -0,0 +1,115 @@
+#include
+#include "kalman_filter/kalman_filter.h"
+
+class KalmanFilterTest : public testing::Test
+{
+public:
+ virtual void SetUp() override {}
+ virtual void TearDown() override {}
+
+ static constexpr float FLOAT_EPSILON{ 0.00001F };
+
+ static constexpr int32_t DIM_X_LKF{ 2 };
+ static constexpr int32_t DIM_Z_LKF{ 1 };
+ static constexpr int32_t DIM_X_EKF{ 2 };
+ static constexpr int32_t DIM_Z_EKF{ 2 };
+ static constexpr kf::float32_t Q11{ 0.1F }, Q22{ 0.1F };
+
+ kf::KalmanFilter m_lkf;
+ kf::KalmanFilter