diff --git a/localization/doc.txt b/localization/doc.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6c2ec8893f289a025937543ede7f0c6b47e7841b
--- /dev/null
+++ b/localization/doc.txt
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser
+ * General Public License v2.1. See the file LICENSE in the top level
+ * directory for more details.
+ */
+
+/**
+ * @defgroup localization LOCALIZATION
+ * @brief localization and optimization algorithms of distance- and magnetic-based
+ * localization systems.
+ *
+ * @details The localization module contains functions to compute a position of a mobile device
+ * using distance measurements or DC-pulsed, magnetic signals. This module also includes
+ * optimization algorithms such as the Levenberg--Marquardt approach to optimize the
+ * calculated position. The localization module also involves a method to recognize and
+ * mitigate the multipath errors on the mobile station.
+ *
+ * @author Zakaria Kasmi
+ *
+ */
diff --git a/localization/position_algos/distance_based/dist_based_fi.c b/localization/position_algos/distance_based/dist_based_fi.c
new file mode 100644
index 0000000000000000000000000000000000000000..73fb25b7ca7fc025f97967e3bc0a7fc64d154201
--- /dev/null
+++ b/localization/position_algos/distance_based/dist_based_fi.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @{
+ *
+ * @file
+ * @brief Error function of distance-based localization systems.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ *
+ * @}
+ */
+
+#include <math.h>
+
+#include "matrix.h"
+
+matrix_t dist_based_fi(matrix_t point[3], matrix_t ref_point[3], matrix_t ri)
+{
+
+ matrix_t f_i =
+ sqrt(
+ (point[0] - ref_point[0])
+ * (point[0]
+ - ref_point[0])
+ +
+ (point[1] - ref_point[1])
+ * (point[1]
+ - ref_point[1])
+ +
+ (point[2] - ref_point[2])
+ * (point[2]
+ - ref_point[2])
+ ) - ri;
+
+ return f_i;
+}
+
+void dist_based_f_i(uint8_t ref_points_num,
+ matrix_t ref_point_mat[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t d_vec[], matrix_t f_vec[])
+{
+ uint8_t i;
+
+ for (i = 0; i < ref_points_num; i++) {
+ f_vec[i] =
+ sqrt(
+ (point[0] - ref_point_mat[i][0])
+ * (point[0]
+ - ref_point_mat[i][0])
+ +
+ (point[1]
+ - ref_point_mat[i][1])
+ * (point[1]
+ - ref_point_mat[i][1])
+ +
+ (point[2]
+ - ref_point_mat[i][2])
+ * (point[2]
+ - ref_point_mat[i][2])
+ )
+ - d_vec[i];
+ }
+}
diff --git a/localization/position_algos/distance_based/dist_based_jacobian.c b/localization/position_algos/distance_based/dist_based_jacobian.c
new file mode 100644
index 0000000000000000000000000000000000000000..117624ed75317b54d0775526caadf4a07a2bb0d4
--- /dev/null
+++ b/localization/position_algos/distance_based/dist_based_jacobian.c
@@ -0,0 +1,166 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @{
+ *
+ * @file
+ * @brief Jacobian function of distance-based localization systems.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ *
+ * @}
+ */
+
+#include <math.h>
+
+#include "matrix.h"
+#include "vector.h"
+#include "dist_based_fi.h"
+
+void dist_based_jacobian_get_JTf(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t dist_vec[ref_points_num],
+ vector_t JTf[3])
+{
+ uint8_t i;
+ matrix_t f_i = 0.0;
+ matrix_t denom = 0.0;
+
+ vector_clear(3, JTf);
+ for (i = 0; i < ref_points_num; i++) {
+ f_i =
+ dist_based_fi(point,
+ (matrix_t *)&ref_point_matrix[i],
+ dist_vec[i]);
+ denom = f_i + dist_vec[i];
+ if (denom != 0) {
+ JTf[0] += (point[0] - ref_point_matrix[i][0]) * f_i
+ / denom;
+ JTf[1] += (point[1] - ref_point_matrix[i][1]) * f_i
+ / denom;
+ JTf[2] += (point[2] - ref_point_matrix[i][2]) * f_i
+ / denom;
+ }
+ }
+}
+
+void dist_based_jacobian_get_JTJ(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t dist_vec[ref_points_num],
+ matrix_t JTJ[3][3])
+{
+ uint8_t i;
+ matrix_t denom = 0.0;
+ matrix_t f_i = 0.0;
+
+ matrix_clear(3, 3, JTJ);
+ for (i = 0; i < ref_points_num; i++) {
+ f_i =
+ dist_based_fi(point,
+ (matrix_t *)&ref_point_matrix[i],
+ dist_vec[i]) + dist_vec[i];
+ denom = f_i * f_i;
+ if (denom != 0) {
+ JTJ[0][0] += (point[0] - ref_point_matrix[i][0]) *
+ (point[0] - ref_point_matrix[i][0])
+ / denom;
+ JTJ[1][1] += (point[1] - ref_point_matrix[i][1]) *
+ (point[1] - ref_point_matrix[i][1])
+ / denom;
+ JTJ[2][2] += (point[2] - ref_point_matrix[i][2]) *
+ (point[2] - ref_point_matrix[i][2])
+ / denom;
+ JTJ[0][1] += (point[0] - ref_point_matrix[i][0]) *
+ (point[1] - ref_point_matrix[i][1])
+ / denom;
+ JTJ[0][2] += (point[0] - ref_point_matrix[i][0]) *
+ (point[2] - ref_point_matrix[i][2])
+ / denom;
+ JTJ[1][2] += (point[1] - ref_point_matrix[i][1]) *
+ (point[2] - ref_point_matrix[i][2])
+ / denom;
+ }
+ }
+
+ JTJ[1][0] = JTJ[0][1];
+ JTJ[2][0] = JTJ[0][2];
+ JTJ[2][1] = JTJ[1][2];
+}
+
+void dist_based_jacobian_get_J(uint8_t ref_points_num, matrix_t point[3],
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t J[ref_points_num][3])
+{
+ uint8_t i;
+ matrix_t denom;
+
+ for (i = 0; i < ref_points_num; i++) {
+ denom =
+ sqrt(
+ pow(
+ point[0]
+ - ref_point_matrix[i][0],
+ 2)
+ +
+ pow(
+ point[1]
+ - ref_point_matrix[i][1],
+ 2)
+ + pow(
+ point[2]
+ - ref_point_matrix[i][2],
+ 2));
+ if (denom != 0) {
+ J[i][0] = (point[0] - ref_point_matrix[i][0]) / denom;
+ J[i][1] = (point[1] - ref_point_matrix[i][1]) / denom;
+ J[i][2] = (point[2] - ref_point_matrix[i][2]) / denom;
+ }
+ }
+}
+
+//compute: J(x)*s
+void dist_based_jacobian_get_J_mul_s(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3], matrix_t s[3],
+ matrix_t J_s[ref_points_num])
+{
+ uint8_t i;
+ matrix_t denom;
+ matrix_t j_i_0 = 0;
+ matrix_t j_i_1 = 0;
+ matrix_t j_i_2 = 0;
+
+ for (i = 0; i < ref_points_num; i++) {
+ denom =
+ sqrt(
+ pow(
+ point[0]
+ - ref_point_matrix[i][0],
+ 2)
+ +
+ pow(
+ point[1]
+ - ref_point_matrix[i][1],
+ 2)
+ + pow(
+ point[2]
+ - ref_point_matrix[i][2],
+ 2));
+ if (denom != 0) {
+ j_i_0 = (point[0] - ref_point_matrix[i][0]) / denom;
+ j_i_1 = (point[1] - ref_point_matrix[i][1]) / denom;
+ j_i_2 = (point[2] - ref_point_matrix[i][2]) / denom;
+ }
+ J_s[i] = j_i_0 * s[0] + j_i_1 * s[1] + j_i_2 * s[2];
+ }
+}
diff --git a/localization/position_algos/distance_based/dist_based_position.c b/localization/position_algos/distance_based/dist_based_position.c
new file mode 100644
index 0000000000000000000000000000000000000000..b6f55c9a827cc5015be8a7745db434667af54f3d
--- /dev/null
+++ b/localization/position_algos/distance_based/dist_based_position.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @{
+ *
+ * @file
+ * @brief Functions of distance-based localization systems.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ *
+ * @}
+ */
+
+#include "dist_based_position.h"
+
+#include <math.h>
+
+#include "matrix.h"
+#include "vector.h"
+
+void dist_based_get_absolute_error(matrix_t value_arr[],
+ matrix_t approx_value_arr[],
+ matrix_t absolute_error_arr[],
+ uint8_t length)
+{
+ uint8_t i;
+
+ if ((value_arr != NULL) && (approx_value_arr != NULL)
+ && (absolute_error_arr != NULL)) {
+ for (i = 0; i < length; i++) {
+ absolute_error_arr[i] = fabs(
+ value_arr[i] - approx_value_arr[i]);
+ }
+ }
+}
+
+matrix_t dist_based_get_distance_to_anchor(matrix_t ref_point[3],
+ matrix_t point[3])
+{
+ matrix_t dist = 0.0;
+ matrix_t diff_vec[3];
+
+ vector_sub(3, ref_point, point, diff_vec);
+ dist = vector_get_norm2(3, diff_vec);
+
+ return dist;
+}
diff --git a/localization/position_algos/distance_based/doc.txt b/localization/position_algos/distance_based/doc.txt
new file mode 100644
index 0000000000000000000000000000000000000000..30ce964998ba78b38cd94e0ff14d474988e23f74
--- /dev/null
+++ b/localization/position_algos/distance_based/doc.txt
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser
+ * General Public License v2.1. See the file LICENSE in the top level
+ * directory for more details.
+ */
+
+/**
+ * @defgroup distance_based DISTANCE_BASED
+ * @ingroup position_algos
+ *
+ * @brief localization algorithms of distance-based localization systems.
+ *
+ * @details The localization module contains functions to compute a position of a mobile device
+ * using distance measurements signals.
+ *
+ * @author Zakaria Kasmi
+ *
+ */
diff --git a/localization/position_algos/distance_based/include/dist_based_fi.h b/localization/position_algos/distance_based/include/dist_based_fi.h
new file mode 100644
index 0000000000000000000000000000000000000000..7ea19dcf7673927a87066f54dc84cdc34092d3c0
--- /dev/null
+++ b/localization/position_algos/distance_based/include/dist_based_fi.h
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @ingroup distance_based
+ * @{
+ *
+ * @file
+ * @brief Error function of distance-based localization systems.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ *
+ * @}
+ */
+
+#ifndef DIST_BASED_FI_H_
+#define DIST_BASED_FI_H_
+
+#include "matrix.h"
+
+/**
+ * @brief Defines the error function of a distance-based localization system.
+ * @details This error function is related to one reference station.
+ *
+ * @param[in] point[] three-dimensional coordinates of the mobile device.
+ * @param[in] ref_point[] three-dimensional coordinates of a reference station.
+ * @param[in] ri distance to a reference station.
+ *
+ * @return the error related to a reference station and destined position.
+ *
+ */
+matrix_t dist_based_fi(matrix_t point[3], matrix_t ref_point[3], matrix_t ri);
+
+/**
+ * @brief Defines the error function of a distance-based localization system.
+ * @details This error function is related to multiple reference stations.
+ *
+ * @param[in] ref_points_num Number of the reference stations.
+ * @param[in] ref_point_mat[][] three-dimensional coordinates of the reference stations.
+ * @param[in] point[] three-dimensional coordinates of the mobile device.
+ * @param[in] d_vec[] distances to the reference stations.
+ * @param[out] f_vec[] errors related to reference stations and destined position.
+ *
+ */
+void dist_based_f_i(uint8_t ref_points_num,
+ matrix_t ref_point_mat[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t d_vec[], matrix_t f_vec[]);
+
+#endif /* DIST_BASED_FI_H_ */
diff --git a/localization/position_algos/distance_based/include/dist_based_jacobian.h b/localization/position_algos/distance_based/include/dist_based_jacobian.h
new file mode 100644
index 0000000000000000000000000000000000000000..d6db03a81c3e4bf8e2881443fed015b1bb4b1c98
--- /dev/null
+++ b/localization/position_algos/distance_based/include/dist_based_jacobian.h
@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @ingroup distance_based
+ * @{
+ *
+ * @file
+ * @brief Jacobian function of distance-based localization systems.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ *
+ * @}
+ */
+
+#ifndef DIST_BASED_JACOBIAN_H_
+#define DIST_BASED_JACOBIAN_H_
+
+#include "matrix.h"
+#include "vector.h"
+
+/**
+ * @brief Defines \f$ J_f^{T} \vec{f} \f$ of distance-based localization system.
+ * @details Where \f$ J_f \f$ is the Jacobian matrix. This function is a part of
+ * derivatives to minimize the sum of square errors.
+ *
+ * @param[in] ref_points_num number of the reference stations.
+ * @param[in] ref_point_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] point[] three-dimensional coordinates of the mobile device.
+ * @param[in] dist_vec[] distances to the reference stations.
+ * @param[in, out] JTf[] includes the \f$ J_f^{T} \vec{f} \f$ vector.
+ *
+ */
+void dist_based_jacobian_get_JTf(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t dist_vec[ref_points_num],
+ vector_t JTf[3]);
+
+/**
+ * @brief Defines \f$ J_f^{T} J_{f} \f$ of distance-based localization
+ * system.
+ * @details Where \f$ J_f \f$ is the Jacobian matrix. This function is a part of
+ * derivatives to minimize the sum of square errors.
+ *
+ * @param[in] ref_points_num number of the reference stations.
+ * @param[in] ref_point_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] point[] three-dimensional coordinates of the mobile device.
+ * @param[in] dist_vec[][] distances to the reference stations.
+ * @param[out] JTJ[][] includes the \f$ J_f^{T} J_{f} \f$ matrix.
+ *
+ */
+void dist_based_jacobian_get_JTJ(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t dist_vec[ref_points_num],
+ matrix_t JTJ[3][3]);
+
+/**
+ * @brief Computes \f$ J_f^{T} \vec{s} \f$ of distance-based localization system.
+ * @details Where \f$ J_f \f$ is the Jacobian matrix. This function is a part of
+ * derivatives to minimize the sum of square errors.
+ *
+ * @param[in] ref_points_num number of the reference stations.
+ * @param[in] ref_point_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] point[] three-dimensional coordinates of the mobile device.
+ * @param[in] s[] correction vector.
+ * @param[out] J_s[] includes the \f$ J_f^{T} \vec{s} \f$ vector.
+ *
+ */
+void dist_based_jacobian_get_J_mul_s(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3], matrix_t point[3],
+ matrix_t s[3], matrix_t J_s[ref_points_num]);
+
+/**
+ * @brief Computes the Jacobian matrix of distance-based localization system.
+ *
+ *
+ * @param[in] ref_points_num number of the reference stations.
+ * @param[in] point[] three-dimensional coordinates of the mobile device.
+ * @param[in] ref_point_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[out] J[][] includes the Jacobian Matrix.
+ *
+ */
+void dist_based_jacobian_get_J(uint8_t ref_points_num, matrix_t point[3],
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t J[ref_points_num][3]);
+
+#endif /* DIST_BASED_JACOBIAN_H_ */
diff --git a/localization/position_algos/distance_based/include/dist_based_position.h b/localization/position_algos/distance_based/include/dist_based_position.h
new file mode 100644
index 0000000000000000000000000000000000000000..b48dfaec940d27b0c74c8d61c501ab9be959c6b7
--- /dev/null
+++ b/localization/position_algos/distance_based/include/dist_based_position.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @ingroup distance_based
+ * @{
+ *
+ * @file
+ * @brief Functions of distance-based localization systems.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ *
+ * @}
+ */
+
+#ifndef DIST_BASED_POSITION_H_
+#define DIST_BASED_POSITION_H_
+
+#include <math.h>
+#include <inttypes.h>
+
+#include "matrix.h"
+
+/**
+ * @brief Computes the absolute error of a position of a distance-based localization system.
+ *
+ *
+ * @param[in] value_arr[] true position.
+ * @param[in] approx_value_arr[] approximate position of the mobile device.
+ * @param[in, out] absolute_error_arr[] includes the absolute error.
+ * @param[in] length arrays length.
+ *
+ */
+void dist_based_get_absolute_error(matrix_t value_arr[],
+ matrix_t approx_value_arr[],
+ matrix_t absolute_error_arr[],
+ uint8_t length);
+
+/**
+ * @brief Computes the distance between a mobile station and a reference station
+ * of a distance-based localization system.
+ *
+ *
+ * @param[in] ref_point three-dimensional coordinates of the reference stations.
+ * @param[in] point three-dimensional coordinates of the mobile device.
+ *
+ * @return the distance between the mobile station and the reference station.
+ */
+matrix_t dist_based_get_distance_to_anchor(matrix_t ref_point[3],
+ matrix_t point[3]);
+
+#endif /* DIST_BASED_POSITION_H_ */
diff --git a/localization/position_algos/doc.txt b/localization/position_algos/doc.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b1ae616d2ccdce8e1c8217a748cebe800e93784e
--- /dev/null
+++ b/localization/position_algos/doc.txt
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser
+ * General Public License v2.1. See the file LICENSE in the top level
+ * directory for more details.
+ */
+
+/**
+ * @defgroup position_algos POSITION_ALGOS
+ * @ingroup localization
+ *
+ * @brief localization algorithms of distance- and magnetic-based localization systems.
+ *
+ * @details The localization module contains functions to compute a position of a mobile device
+ * using distance measurements or DC-pulsed, magnetic signals.
+ *
+ * @author Zakaria Kasmi
+ *
+ */
diff --git a/localization/position_algos/magnetic_based/doc.txt b/localization/position_algos/magnetic_based/doc.txt
new file mode 100644
index 0000000000000000000000000000000000000000..50574fd2981f58e82ceb73ce0ae62d485d67e0b4
--- /dev/null
+++ b/localization/position_algos/magnetic_based/doc.txt
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser
+ * General Public License v2.1. See the file LICENSE in the top level
+ * directory for more details.
+ */
+
+/**
+ * @defgroup magnetic_based MAGNETIC_BASED
+ * @ingroup position_algos
+ *
+ * @brief localization algorithms of magnetic-based localization systems.
+ *
+ * @details The localization module contains functions to compute a position of a mobile device
+ * using artificially generated DC-pulsed, magnetic signals.
+ *
+ * @author Zakaria Kasmi
+ *
+ */
diff --git a/localization/position_algos/magnetic_based/include/magnetic_based_fi.h b/localization/position_algos/magnetic_based/include/magnetic_based_fi.h
new file mode 100644
index 0000000000000000000000000000000000000000..26a9de0431931a4eb8962c1182901e820d076911
--- /dev/null
+++ b/localization/position_algos/magnetic_based/include/magnetic_based_fi.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @ingroup magnetic_based
+ * @{
+ *
+ * @file
+ * @brief Error function of DC-pulsed, magnetic localization system.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Naouar Guerchali
+ *
+ * @}
+ */
+
+#ifndef MAGNETIC_BASED_F_H_
+#define MAGNETIC_BASED_F_H_
+#include <math.h>
+#include <string.h>
+
+#include "matrix.h"
+
+/**
+ * @brief Defines the error function of a magnetic-based localization system.
+ * @details This error function is related to multiple reference stations.
+ *
+ * @param[in] ref_points_num number of the reference stations.
+ * @param[in] ref_points_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] point[] three-dimensional coordinates of the mobile device.
+ * @param[in] Bi_vec[] the measured magnetic field strength.
+ * @param[in, out] f_vec[] errors related to reference stations and destined position.
+ *
+ */
+void magnetic_based_f_i(uint8_t ref_points_num, matrix_t ref_points_matrix[][3],
+ matrix_t point[], matrix_t Bi_vec[], matrix_t f_vec[]);
+
+#endif /* MAGNETIC_BASED_F_H_ */
diff --git a/localization/position_algos/magnetic_based/include/magnetic_based_jacobian.h b/localization/position_algos/magnetic_based/include/magnetic_based_jacobian.h
new file mode 100644
index 0000000000000000000000000000000000000000..9d7255f73017648021041d26a566b688df9d7652
--- /dev/null
+++ b/localization/position_algos/magnetic_based/include/magnetic_based_jacobian.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @ingroup magnetic_based
+ * @{
+ *
+ * @file
+ * @brief Jacobian function of DC-pulsed, magnetic localization system.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Naouar Guerchali
+ *
+ * @}
+ */
+
+#ifndef MAGNETIC_BASED_JACOBIAN_H_
+#define MAGNETIC_BASED_JACOBIAN_H_
+
+#include "matrix.h"
+
+/**
+ * @brief Computes the Jacobian matrix of magnetic-based localization system.
+ *
+ *
+ * @param[in] ref_point_num number of the reference stations.
+ * @param[in] ref_point_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] point[] three-dimensional coordinates of the mobile device.
+ * @param[in, out] J[][] includes the Jacobian Matrix.
+ *
+ */
+void magnetic_based_jacobian_get_J(uint8_t ref_point_num,
+ matrix_t ref_point_matrix[ref_point_num][3],
+ matrix_t point[],
+ matrix_t J[ref_point_num][3]);
+
+/**
+ * @brief Defines \f$ J_f^{T} J_{f} \f$ of magnetic-based localization
+ * system.
+ * @details Where \f$ J_f \f$ is the Jacobian matrix. This function is a part of
+ * derivatives to minimize the sum of square errors.
+ *
+ * @param[in] ref_points_num number of the reference stations.
+ * @param[in] ref_point_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] point[] three-dimensional coordinates of the mobile device.
+ * @param[in] unused this variable can be set to NULL. It is introduced to guarantee
+ * the compatibility with the @ref dist_based_jacobian_get_JTJ,
+ * @ref modified_gauss_newton, and the @ref opt_levenberg_marquardt
+ * functions.
+ * @param[in, out] JTJ[][] includes the \f$ J_f^{T} J_{f} \f$ matrix.
+ *
+ */
+void magnetic_based_jacobian_get_JTJ(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3], matrix_t *unused, matrix_t JTJ[3][3]);
+/**
+ * @brief Defines \f$ J_f^{T} \vec{f} \f$ of magnetic-based localization system.
+ * @details Where \f$ J_f \f$ is the Jacobian matrix. This function is a part of
+ * derivatives to minimize the sum of square errors.
+ *
+ * @param[in] ref_points_num number of the reference stations.
+ * @param[in] ref_points_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] point[] three-dimensional coordinates of the mobile device.
+ * @param[in] Bi_vec[] magnetic field strengths of the coils (reference stations).
+ * @param[in, out] JTf[] includes the \f$ J_f^{T} \vec{f} \f$ vector.
+ *
+ */
+void magnetic_based_jacobian_get_JTf(uint8_t ref_points_num,
+ matrix_t ref_points_matrix[ref_points_num][3],
+ matrix_t point[3], matrix_t Bi_vec[ref_points_num],
+ matrix_t JTf[3]);
+
+/**
+ * @brief Computes \f$ J_f^{T} \vec{s} \f$ of magnetic-based localization system.
+ * @details Where \f$ J_f \f$ is the Jacobian matrix. This function is a part of
+ * derivatives to minimize the sum of square errors.
+ *
+ * @param[in] ref_points_num number of the reference stations.
+ * @param[in] ref_point_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] point[] three-dimensional coordinates of the mobile device.
+ * @param[in] s[] correction vector.
+ * @param[in, out] J_s[] includes the \f$ J_f^{T} \vec{s} \f$ vector.
+ *
+ */
+void magnetic_based_jacobian_get_J_mul_s(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t s[3], matrix_t J_s[ref_points_num]);
+
+#endif /* MAGNETIC_BASED_JACOBIAN_H_ */
diff --git a/localization/position_algos/magnetic_based/include/magnetic_based_position.h b/localization/position_algos/magnetic_based/include/magnetic_based_position.h
new file mode 100644
index 0000000000000000000000000000000000000000..2304352531d5855d2b6779f0665d33732175d556
--- /dev/null
+++ b/localization/position_algos/magnetic_based/include/magnetic_based_position.h
@@ -0,0 +1,189 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @ingroup magnetic_based
+ * @{
+ *
+ * @file
+ * @brief Functions of of DC-pulsed, magnetic localization system.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Abdelmoumen Norrdine <a.norrdine@googlemail.com>
+ *
+ * @}
+ */
+
+#ifndef MAGNETIC_BASED_POSITION_H_
+#define MAGNETIC_BASED_POSITION_H_
+
+#include <inttypes.h>
+#include <math.h>
+
+#include "matrix.h"
+
+/**
+ * The current running through the coil.
+ */
+#define I0 15
+
+/**
+ * The number of turns of the wire.
+ */
+#define NW 140
+
+/**
+ * The radius of the coil.
+ */
+#define R0 0.25
+
+/**
+ * The pi constant.
+ */
+#define PI 3.14159265358979323846
+
+/**
+ * The base area of the coil.
+ */
+#define AR PI * R0 * R0 //Loop area
+
+/**
+ * The permeability of free space,.
+ */
+#define MU0 4 * PI * 1E-7 //TGm/A
+
+/**
+ * The K constant.
+ */
+#define K MU0 * NW * I0 * AR / (4 * PI)
+
+/**
+ * From Mega Gauss to Tesla.
+ */
+#define MG_TO_T 1E7
+
+/**
+ * The number of turns of the wire.
+ */
+#define K_T K * MG_TO_T
+
+/**
+ * The number of turns of the wire.
+ */
+#define MILPS_MAX_DIST 25
+
+/**
+ * @brief Computes the absolute error of a position of magnet-based localization system.
+ *
+ *
+ * @param[in] value_arr true position.
+ * @param[in] approx_value_arr approximate position of the mobile device.
+ * @param[in, out] absolute_error_arr includes the absolute error.
+ * @param[in] length arrays length.
+ *
+ */
+void magnetic_based_get_absolute_error(matrix_t value_arr[],
+ matrix_t approx_value_arr[],
+ matrix_t absolute_error_arr[],
+ uint8_t length);
+
+/**
+ * @brief Computes the distances between a mobile station and the reference stations
+ * of a magnet-based localization system.
+ *
+ *
+ * @param[in] magnetic_field_strength_arr[] includes the magnetic field strengths.
+ * @param[in] angular_theta_arr[] elevation angles.
+ * @param[in, out] distance_arr[] distance array.
+ * @param[in] length length of the arrays.
+ * @param[in] k magnetic constant.
+ *
+ */
+void magnetic_based_get_distances(matrix_t magnetic_field_strength_arr[],
+ matrix_t angular_theta_arr[],
+ matrix_t distance_arr[], uint8_t length,
+ matrix_t k);
+
+/**
+ * @brief Computes the distance between a mobile station and a reference stations
+ * of a magnet-based localization system.
+ *
+ *
+ * @param[in] B magnetic field strength.
+ * @param[in] theta elevation angle.
+ * @param[in] k magnetic constant.
+ *
+ * @return the distance between the mobile station and the reference station.
+ */
+matrix_t magnetic_based_get_r(matrix_t B, matrix_t theta, matrix_t k);
+
+/**
+ * @brief Computes the distance between a mobile station and a reference station
+ * of magnetic-based localization system.
+ *
+ *
+ * @param[in] ref_point[] three-dimensional coordinates of the reference stations.
+ * @param[in] point[] three-dimensional coordinates of the mobile device.
+ *
+ * @return the distance between the mobile station and the reference station.
+ */
+matrix_t magnetic_based_get_distances_to_anchors(matrix_t ref_point[3],
+ matrix_t point[3]);
+/**
+ * @brief Computes the magnetic field strength from a mobile station to a reference station.
+ *
+ *
+ * @param[in] ref_point[] three-dimensional coordinates of a reference station.
+ * @param[in] target_point[] three-dimensional coordinates of the mobile device.
+ * @param[in] k magnetic constant.
+ *
+ * @return the magnetic field strength.
+ */
+matrix_t magnetic_based_get_magnetic_field(matrix_t ref_point[3],
+ matrix_t target_point[3],
+ matrix_t k);
+/**
+ * @brief Computes the magnetic field strengths from a mobile station to various reference
+ * stations.
+ *
+ * @param[in] m number of the reference stations.
+ * @param[in] ref_point_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] target_point[] three-dimensional coordinates of the mobile device.
+ * @param[in] k magnetic constant.
+ * @param[in, out] magn_field_vec[] includes the magnetic field strengths.
+ *
+ */
+void magnetic_based_get_magnetic_field_vec(uint8_t m,
+ matrix_t ref_point_matrix[m][3],
+ matrix_t target_point[],
+ matrix_t k,
+ matrix_t magn_field_vec[]);
+
+/**
+ * @brief Computes the position of a mobile station by a magnetic-based localization system.
+ * @details The position is computed based on a pre-processed pseudo-inverse matrix and the
+ * homogeneous solution in the case of three reference stations.
+ *
+ * @param[in] anchor_num number of the reference stations.
+ * @param[in] anchor_pos_matrix[][] coordinates of the reference stations.
+ * @param[in] pseudo_inv_matrix[][[] pointer to the pre-processed pseudo-inverse matrix.
+ * @param[in] homog_sol_arr[] pointer to the homogeneous solution.
+ * @param[in, out] solution_x1[] includes the first solution.
+ * @param[in, out] solution_x2[] includes the second solution.
+ *
+ */
+void magnetic_based_preprocessing_get_position(uint8_t anchor_num,
+ matrix_t anchor_pos_matrix[anchor_num][3],
+ matrix_t pseudo_inv_matrix[4][anchor_num],
+ matrix_t homog_sol_arr[],
+ matrix_t solution_x1[],
+ matrix_t solution_x2[]);
+
+#endif /* MAGNETIC_BASED_POSITION_H_ */
diff --git a/localization/position_algos/magnetic_based/magnetic_based_fi.c b/localization/position_algos/magnetic_based/magnetic_based_fi.c
new file mode 100644
index 0000000000000000000000000000000000000000..a8afbc39a8b173ec083ce7535bc42353d34915d2
--- /dev/null
+++ b/localization/position_algos/magnetic_based/magnetic_based_fi.c
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @{
+ *
+ * @file
+ * @brief Error function of DC-pulsed, magnetic localization system.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Naouar Guerchali
+ *
+ * @}
+ */
+
+#include <math.h>
+#include <inttypes.h>
+#include <magnetic_based_position.h>
+
+#include "matrix.h"
+
+void magnetic_based_f_i(uint8_t ref_points_num,
+ matrix_t ref_points_matrix[][3], matrix_t point[],
+ matrix_t Bi_vec[], matrix_t f_vec[])
+{
+
+ for (int i = 0; i < ref_points_num; i++) {
+
+ matrix_t X_Xi_sq = pow(point[0] - ref_points_matrix[i][0], 2);
+ matrix_t Y_Yi_sq = pow(point[1] - ref_points_matrix[i][1], 2);
+ matrix_t Z_Zi_sq = pow(point[2] - ref_points_matrix[i][2], 2);
+
+ f_vec[i] = (K_T * (sqrt(X_Xi_sq + Y_Yi_sq + 4.0 * Z_Zi_sq)) /
+ pow((X_Xi_sq + Y_Yi_sq + Z_Zi_sq), 2))
+ - Bi_vec[i];
+ }
+}
diff --git a/localization/position_algos/magnetic_based/magnetic_based_jacobian.c b/localization/position_algos/magnetic_based/magnetic_based_jacobian.c
new file mode 100644
index 0000000000000000000000000000000000000000..35f19bfe828b4a2ef47a6d17d9f28f5949681089
--- /dev/null
+++ b/localization/position_algos/magnetic_based/magnetic_based_jacobian.c
@@ -0,0 +1,199 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @{
+ *
+ * @file
+ * @brief Jacobian function of DC-pulsed, magnetic localization system.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Naouar Guerchali
+ *
+ * @}
+ */
+
+#include <math.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "magnetic_based_fi.h"
+#include "magnetic_based_position.h"
+#include "matrix.h"
+
+void magnetic_based_jacobian_get_J(uint8_t ref_point_num,
+ matrix_t ref_point_matrix[ref_point_num][3],
+ matrix_t point[],
+ matrix_t J[ref_point_num][3])
+{
+
+ for (int i = 0; i < ref_point_num; i++) {
+
+ matrix_t X_Xi = point[0] - ref_point_matrix[i][0];
+ matrix_t Y_Yi = point[1] - ref_point_matrix[i][1];
+ matrix_t Z_Zi = point[2] - ref_point_matrix[i][2];
+
+ matrix_t X_Xi_sq = pow(X_Xi, 2);
+ matrix_t Y_Yi_sq = pow(Y_Yi, 2);
+ matrix_t Z_Zi_sq = pow(Z_Zi, 2);
+
+ J[i][0] =
+ K_T * (X_Xi)
+ * ((-3.0 * X_Xi_sq
+ - 3.0 * Y_Yi_sq
+ - 15.0 * Z_Zi_sq)
+ /
+ (sqrt(
+ X_Xi_sq
+ + Y_Yi_sq
+ + 4.0
+ * Z_Zi_sq)
+ * pow(
+ (X_Xi_sq
+ + Y_Yi_sq
+ + Z_Zi_sq),
+ 3)));
+
+ J[i][1] =
+ K_T * (Y_Yi)
+ * ((-3.0 * X_Xi_sq
+ - 3.0 * Y_Yi_sq
+ - 15.0 * Z_Zi_sq)
+ /
+ (sqrt(
+ X_Xi_sq
+ + Y_Yi_sq
+ + 4.0
+ * Z_Zi_sq)
+ * pow(
+ (X_Xi_sq
+ + Y_Yi_sq
+ + Z_Zi_sq),
+ 3)));
+
+ J[i][2] =
+ 4.0 * K_T * (Z_Zi)
+ * ((-3.0 * Z_Zi_sq)
+ /
+ (sqrt(
+ X_Xi_sq
+ + Y_Yi_sq
+ + 4.0
+ * Z_Zi_sq)
+ * pow(
+ (X_Xi_sq
+ + Y_Yi_sq
+ + Z_Zi_sq),
+ 3)));
+ }
+}
+
+void magnetic_based_jacobian_get_JTJ(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3], matrix_t *unused, matrix_t JTJ[3][3])
+{
+ matrix_t J[ref_points_num][3];
+
+ magnetic_based_jacobian_get_J(ref_points_num, ref_point_matrix, point,
+ J);
+ matrix_trans_mul_itself(ref_points_num, 3, J, JTJ);
+
+ (void)unused;
+}
+
+void magnetic_based_jacobian_get_JTf(uint8_t ref_points_num,
+ matrix_t ref_points_matrix[ref_points_num][3],
+ matrix_t point[3], matrix_t Bi_vec[ref_points_num],
+ matrix_t JTf[3])
+{
+
+ matrix_t J[ref_points_num][3];
+ matrix_t f_vec[ref_points_num];
+
+ magnetic_based_f_i(ref_points_num, ref_points_matrix, point, Bi_vec,
+ f_vec);
+
+ magnetic_based_jacobian_get_J(ref_points_num, ref_points_matrix, point,
+ J);
+ matrix_trans_mul_vec(ref_points_num, 3, J, ref_points_num, f_vec, JTf);
+}
+
+//compute: J(x)*s
+void magnetic_based_jacobian_get_J_mul_s(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t s[3],
+ matrix_t J_s[ref_points_num])
+{
+ uint8_t i;
+ matrix_t X_Xi, Y_Yi, Z_Zi;
+ matrix_t X_Xi_sq, Y_Yi_sq, Z_Zi_sq;
+ matrix_t j_i_0, j_i_1, j_i_2;
+
+ for (i = 0; i < ref_points_num; i++) {
+ X_Xi = point[0] - ref_point_matrix[i][0];
+ Y_Yi = point[1] - ref_point_matrix[i][1];
+ Z_Zi = point[2] - ref_point_matrix[i][2];
+
+ X_Xi_sq = pow(X_Xi, 2);
+ Y_Yi_sq = pow(Y_Yi, 2);
+ Z_Zi_sq = pow(Z_Zi, 2);
+
+ j_i_0 =
+ K_T * (X_Xi)
+ * ((-3.0 * X_Xi_sq
+ - 3.0 * Y_Yi_sq
+ - 15.0 * Z_Zi_sq)
+ /
+ (sqrt(
+ X_Xi_sq
+ + Y_Yi_sq
+ + 4.0
+ * Z_Zi_sq)
+ * pow(
+ (X_Xi_sq
+ + Y_Yi_sq
+ + Z_Zi_sq),
+ 3)));
+ j_i_1 =
+ K_T * (Y_Yi)
+ * ((-3.0 * X_Xi_sq
+ - 3.0 * Y_Yi_sq
+ - 15.0 * Z_Zi_sq)
+ /
+ (sqrt(
+ X_Xi_sq
+ + Y_Yi_sq
+ + 4.0
+ * Z_Zi_sq)
+ * pow(
+ (X_Xi_sq
+ + Y_Yi_sq
+ + Z_Zi_sq),
+ 3)));
+ j_i_2 =
+ 4.0 * K_T * (Z_Zi)
+ * ((-3.0 * Z_Zi_sq)
+ /
+ (sqrt(
+ X_Xi_sq
+ + Y_Yi_sq
+ + 4.0
+ * Z_Zi_sq)
+ * pow(
+ (X_Xi_sq
+ + Y_Yi_sq
+ + Z_Zi_sq),
+ 3)));
+
+ J_s[i] = j_i_0 * s[0] + j_i_1 * s[1] + j_i_2 * s[2];
+ }
+}
diff --git a/localization/position_algos/magnetic_based/magnetic_based_position.c b/localization/position_algos/magnetic_based/magnetic_based_position.c
new file mode 100644
index 0000000000000000000000000000000000000000..1cca048ece5cdd847418fff40675a935d0ac8ddd
--- /dev/null
+++ b/localization/position_algos/magnetic_based/magnetic_based_position.c
@@ -0,0 +1,201 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @{
+ *
+ * @file
+ * @brief Functions of of DC-pulsed, magnetic localization system.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Abdelmoumen Norrdine <a.norrdine@googlemail.com>
+ *
+ * @}
+ */
+
+#include <math.h>
+#include <complex.h>
+#include <magnetic_based_position.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "matrix.h"
+#include "vector.h"
+#include "trilateration.h"
+
+void magnetic_based_get_absolute_error(matrix_t value_arr[],
+ matrix_t approx_value_arr[],
+ matrix_t absolute_error_arr[],
+ uint8_t length)
+{
+ uint8_t i;
+
+ if ((value_arr != NULL) && (approx_value_arr != NULL)
+ && (absolute_error_arr != NULL)) {
+ for (i = 0; i < length; i++) {
+ absolute_error_arr[i] = fabs(
+ value_arr[i] - approx_value_arr[i]);
+ }
+ }
+}
+
+matrix_t magnetic_based_get_distances_to_anchors(matrix_t ref_point[3],
+ matrix_t point[3])
+{
+ matrix_t dist = 0.0;
+ matrix_t diff_vec[3];
+
+ vector_sub(3, ref_point, point, diff_vec);
+ dist = vector_get_norm2(3, diff_vec);
+
+ return dist;
+}
+
+matrix_t magnetic_based_get_magnetic_field(matrix_t ref_point[3],
+ matrix_t target_point[3], matrix_t k)
+{
+ matrix_t magn_field = 0.0;
+ matrix_t r_square = 0.0;
+ uint8_t i;
+
+ printf("ref_arr = ");
+ vector_flex_print(3, ref_point, 5, 4);
+ puts("");
+ for (i = 0; i < 3; i++) {
+ r_square += pow(target_point[i] - ref_point[i], 2);
+ }
+
+ printf("r_sq = %f\n", r_square);
+ magn_field =
+ k
+ * sqrt(
+ 3
+ * pow(
+ target_point[2]
+ - ref_point[2],
+ 2)
+ + r_square)
+ / pow(r_square, 2);
+
+ return magn_field;
+}
+
+void magnetic_based_get_magnetic_field_vec(uint8_t m,
+ matrix_t ref_point_matrix[m][3],
+ matrix_t target_point[],
+ matrix_t k,
+ matrix_t magn_field_vec[])
+{
+
+ matrix_t r_square = 0.0;
+ uint8_t i, j;
+
+ for (i = 0; i < m; i++) {
+ for (j = 0; i < 3; j++) {
+ r_square +=
+ pow(
+ target_point[i]
+ - ref_point_matrix[i][j],
+ 2);
+ }
+
+ magn_field_vec[i] =
+ k
+ * sqrt(
+ 3
+ * pow(
+ target_point[2]
+ - ref_point_matrix[i][2],
+ 2)
+ + r_square)
+ / pow(r_square, 2);
+ r_square = 0.0;
+ }
+}
+
+
+void magnetic_based_get_distances(matrix_t magnetic_field_strength_arr[],
+ matrix_t angular_theta_arr[],
+ matrix_t distance_arr[], uint8_t length,
+ matrix_t k)
+{
+ int i;
+
+ for (i = 0; i < length; i++) {
+ if (magnetic_field_strength_arr[i] == 0) {
+ distance_arr[i] = MILPS_MAX_DIST;
+ }
+ else {
+ if (angular_theta_arr[i] == 0) {
+ distance_arr[i] =
+ pow(
+ k
+ / magnetic_field_strength_arr[i],
+ (matrix_t)1
+ / 3);
+ }
+ else {
+
+ distance_arr[i] =
+ pow(
+ k
+ * sqrt(
+ 1
+ + 3
+ * pow(
+ sin(
+ angular_theta_arr[i]),
+ 2))
+ / magnetic_field_strength_arr[i],
+ (matrix_t)1
+ / 3);
+ }
+ }
+ }
+}
+
+
+matrix_t magnetic_based_get_r(matrix_t B, matrix_t theta, matrix_t k)
+{
+ matrix_t r = 0;
+
+ if (B == 0) {
+ return MILPS_MAX_DIST;
+ }
+ else {
+ if (theta == 0) {
+ r = pow(k / B, (matrix_t)1 / 3);
+ }
+ else {
+
+ r = pow(k * sqrt(1 + 3 * pow(sin(theta), 2)) / B,
+ (matrix_t)1 / 3);
+ }
+ }
+
+ return r;
+}
+
+// PC proprocessing
+void magnetic_based_preprocessing_get_position(uint8_t anchor_num,
+ matrix_t anchor_pos_matrix[anchor_num][3],
+ matrix_t pseudo_inv_matrix[4][anchor_num],
+ matrix_t homog_sol_arr[],
+ matrix_t solution_x1[],
+ matrix_t solution_x2[])
+{
+
+ matrix_t dist_arr[anchor_num];
+
+ trilateration1(anchor_num, anchor_pos_matrix, pseudo_inv_matrix,
+ homog_sol_arr, dist_arr, solution_x1, solution_x2);
+
+}
diff --git a/localization/position_algos/pos_algos_common/DOP.c b/localization/position_algos/pos_algos_common/DOP.c
new file mode 100644
index 0000000000000000000000000000000000000000..e60588e74281877e0f31401d32c5dbe8a2ac9e5c
--- /dev/null
+++ b/localization/position_algos/pos_algos_common/DOP.c
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @{
+ *
+ * @file
+ * @brief Compute the Position Dilution of Precision (PDOP).
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Naouar Guerchali
+ *
+ * @}
+ */
+
+#include <math.h>
+
+#include "DOP.h"
+
+#include "moore_penrose_pseudo_inverse.h"
+#include "matrix.h"
+
+matrix_t get_PDOP(uint8_t m, matrix_t ref_Matrix[m][3],
+ matrix_t true_pos[m])
+{
+
+ matrix_t PDOP = 0.0;
+ matrix_t H[m][4];
+ matrix_t H_transpose[4][m];
+ matrix_t H_transpose_H[4][4];
+ matrix_t H_transpose_H_pinv[4][4];
+
+ for (int i = 0; i < m; i++) {
+ matrix_t R_i = sqrt(pow(ref_Matrix[i][0] - true_pos[0], 2)
+ + pow(ref_Matrix[i][1] - true_pos[1], 2)
+ + pow(ref_Matrix[i][2] - true_pos[2], 2));
+
+ for (int l = 0; l < 3; l++) {
+ H[i][l] = (ref_Matrix[i][l] - true_pos[l]) / R_i;
+ }
+ H[i][3] = -1;
+ }
+
+ matrix_transpose(m, 4, H, H_transpose);
+ matrix_mul(4, m, H_transpose, m, 4, H, H_transpose_H);
+ moore_penrose_get_pinv(4, 4, H_transpose_H, H_transpose_H_pinv);
+
+ for (int i = 0; i < 3; i++) {
+ PDOP = PDOP + H_transpose_H_pinv[i][i];
+ }
+ PDOP = sqrt(PDOP);
+ return PDOP;
+}
diff --git a/localization/position_algos/pos_algos_common/doc.txt b/localization/position_algos/pos_algos_common/doc.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b8257c30c5def97efbe0085af66763b06c560312
--- /dev/null
+++ b/localization/position_algos/pos_algos_common/doc.txt
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser
+ * General Public License v2.1. See the file LICENSE in the top level
+ * directory for more details.
+ */
+
+/**
+ * @defgroup pos_algos_common POS_ALGOS_COMMON
+ * @ingroup position_algos
+ *
+ * @brief Common algorithms of localization systems.
+ *
+ * @details This module contains common functions such as the trilateration or the Position Dilution
+ * of Precision (PDOP).
+ *
+ * @author Zakaria Kasmi
+ *
+ */
diff --git a/localization/position_algos/pos_algos_common/include/DOP.h b/localization/position_algos/pos_algos_common/include/DOP.h
new file mode 100644
index 0000000000000000000000000000000000000000..6b4f1e90d1846285c0e2b1a2b7aa114824cc0332
--- /dev/null
+++ b/localization/position_algos/pos_algos_common/include/DOP.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @ingroup pos_algos_common
+ * @{
+ *
+ * @file
+ * @brief Compute the Position Dilution of Precision (PDOP).
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Naouar Guerchali
+ *
+ * @}
+ */
+
+#ifndef DOP_H_
+#define DOP_H_
+
+#include <inttypes.h>
+
+#include "matrix.h"
+
+/**
+ * @brief Compute the Position Dilution of Precision (PDOP).
+ *
+ * @param[in] m number of the reference stations.
+ * @param[in] ref_Matrix three-dimensional coordinates of the reference stations.
+ * @param[in] true_pos three-dimensional coordinates of the mobile device.
+ *
+ *
+ * @return the PDOP-value.
+ *
+ */
+matrix_t get_PDOP(uint8_t m, matrix_t ref_Matrix[m][3], matrix_t true_pos[m]);
+
+#endif /* DOP_H_ */
diff --git a/localization/position_algos/pos_algos_common/include/trilateration.h b/localization/position_algos/pos_algos_common/include/trilateration.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb9c787f06670da6ac3957ded8c71ccd4544e509
--- /dev/null
+++ b/localization/position_algos/pos_algos_common/include/trilateration.h
@@ -0,0 +1,190 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @ingroup pos_algos_common
+ * @{
+ *
+ * @file
+ * @brief Implement the trilateration algorithm.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Abdelmoumen Norrdine <a.norrdine@googlemail.com>
+ *
+ * @}
+ */
+
+#ifndef TRILATERATION_H_
+#define TRILATERATION_H_
+
+#include <inttypes.h>
+
+#include "matrix.h"
+
+/**
+ * @brief Implement the trilateration algorithm using the pre-processed pseudo-inverse matrix.
+ * @note This function should be initialized with the pre-processed pseudo-inverse matrix of
+ * the equation system: \f$ A \vec{x} = 0 \f$.
+ * Caution!: The \p solution_x1 and the \p solution_x2 vectors have a length of 3 or 4 in
+ * the case of two-dimensional or three-dimensional space. The first element \f$(x_0)\f$
+ * of the \p solution_x1[] and the \p solution_x2[] vectors is a measure of the solvability
+ * of the multilateration problem. For example, in the three-dimensional space:
+ * \f$ d = x_0 - \left( x_1^{2} + x_2^{2} + x_3^{2} \right)\f$.
+ *
+ *
+ * @param[in] anchor_num number of the reference stations.
+ * @param[in] anchor_pos_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] pseudo_inv_matrix[][] pointer to the pre-processed pseudo-inverse matrix.
+ * @param[in] homog_sol_arr[] the homogeneous solution.
+ * @param[in] dist_arr[] distances to the reference stations.
+ * @param[in, out] solution_x1[] includes the first solution. It has the length of 3 or 4 in
+ * the case of two-dimensional or three-dimensional space.
+ * @param[in, out] solution_x2[] includes the second solution. It has length of 3 or 4 in the
+ * case of two-dimensional or three-dimensional space.
+ *
+ */
+void trilateration1(uint8_t anchor_num,
+ matrix_t anchor_pos_matrix[anchor_num][3],
+ matrix_t pseudo_inv_matrix[4][anchor_num],
+ matrix_t homog_sol_arr[],
+ matrix_t dist_arr[],
+ matrix_t solution_x1[],
+ matrix_t solution_x2[]
+ );
+
+/**
+ * @brief Implement the trilateration algorithm.
+ * @note The pre-processed pseudo-inverse matrix of the equation system:
+ * \f$ A \vec{x} = 0 \f$ is computed on the mobile station.
+ * Caution!: The \p solution_x1 and the \p solution_x2 vectors have a length of 3 or 4 in
+ * the case of two-dimensional or three-dimensional space. The first element \f$(x_0)\f$
+ * of the \p solution_x1[] and the \p solution_x2[] vectors is a measure of the solvability
+ * of the multilateration problem. For example, in the three-dimensional space:
+ * \f$ d = x_0 - \left( x_1^{2} + x_2^{2} + x_3^{2} \right)\f$.
+ *
+ * @param[in] anchor_num number of the reference stations.
+ * @param[in] anchor_pos_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] dist_arr[] distances to the reference stations.
+ * @param[in, out] solution_x1[] includes the first solution. It has the length of 3 or 4 in
+ * the case of two-dimensional or three-dimensional space.
+ * @param[in, out] solution_x2[] includes the second solution. It has the length of 3 or 4 in
+ * the case of two-dimensional or three-dimensional space.
+ *
+ */
+void trilateration2(uint8_t anchor_num,
+ matrix_t anchor_pos_matrix[anchor_num][3],
+ matrix_t dist_arr[],
+ matrix_t solution_x1[],
+ matrix_t solution_x2[]);
+
+/**
+ * @brief Computes the matrix \f$ A \f$ of the equation system:
+ * \f$ A \vec{x} = \vec{b}\f$.
+ *
+ * @param[in] m number of the reference stations.
+ * @param[in] anchor_pos_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in, out] A_matrix[][] pointer to the matrix \f$ A \f$.
+ *
+ */
+void trilateration_get_A_matrix(uint8_t m, matrix_t anchor_pos_matrix[m][3],
+ matrix_t A_matrix[m][4]);
+/**
+ * @brief Computes the vector \f$ \vec{b} \f$ of the equation system:
+ * \f$ A \vec{x} = \vec{b}\f$.
+ *
+ * @param[in] anchor_num number of the reference stations.
+ * @param[in] dist_arr[] distances to the reference stations.
+ * @param[in] anchor_pos_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in, out] b_vec[] pointer to the vector \f$ vec{b} \f$.
+ *
+ */
+void trilateration_get_b_vector(uint8_t anchor_num, matrix_t dist_arr[],
+ matrix_t anchor_pos_matrix[anchor_num][3],
+ matrix_t b_vec[]);
+
+/**
+ * @brief Compute the particular solution, which is the general solution of
+ * \f$ A \vec{x_0} = \vec{b_0}\f$.
+ * @note The particular solution is calculated in the case of three reference stations.
+ * The particular solution is computed using the pre-processed pseudo-inverse matrix.
+ *
+ * @param[in] pseudo_inv_matrix[][] pointer to the pre-processed pseudo-inverse matrix.
+ * @param[in] b_arr[] pointer the vector \f$ vec{b} \f$.
+ * @param[in, out] particular_solu_arr[] includes the particular solution.
+ *
+ */
+void trilateration_preprocessed_get_particular_solution(
+ matrix_t pseudo_inv_matrix[4][3],
+ matrix_t b_arr[],
+ matrix_t particular_solu_arr[]);
+
+/**
+ * @brief Compute the particular solution, which is the general solution of
+ * \f$ A \vec{x_0} = \vec{b_0} \f$.
+ * @note The particular solution is calculated in the case of three reference stations.
+ * The particular solution is computed on the mobile station.
+ *
+ * @param[in] m number of the reference stations.
+ * @param[in] n column number of the reference stations matrix.
+ * @param[in] anchor_pos_mat[][] three-dimensional coordinates of the reference stations.
+ * @param[in] dist_arr[] distances to the reference stations.
+ * @param[in, out] Xp[] includes the particular solution.
+ */
+void trilateration_get_particular_solution(uint8_t m, uint8_t n,
+ matrix_t anchor_pos_mat[m][n],
+ matrix_t dist_arr[], matrix_t Xp[]);
+
+/**
+ * @brief Compute the rank and the solution of the homogeneous system
+ * \f$ A \vec{x_0} = 0 \f$.
+ *
+ * @param[in] anchor_num number of the reference stations.
+ * @param[in] anchor_pos_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in, out] Xh[] includes the homogeneous solution.
+ *
+ * @return the rank of the matrix \f$ A \f$.
+ */
+uint8_t trilateration_get_rank_and_homogeneous_solution(uint8_t anchor_num,
+ matrix_t anchor_pos_matrix[anchor_num][3],
+ matrix_t Xh[]);
+/**
+ * @brief Solve a quadratic equation.
+ * @details The quotients of the quadratic equation are derived from the particular and homogeneous
+ * solution.
+ *
+ * @param[in] particular_solu_arr[] pointer to the particular solution.
+ * @param[in] homogeneous_solution_arr[] pointer to the homogeneous solution.
+ * @param[in, out] solution_x1[] presents the first solution.
+ * @param[in, out] solution_x2[] presents the second solution.
+ *
+ */
+void trilateration_get_quadratic_equation_solution(
+ matrix_t particular_solu_arr[],
+ matrix_t homogeneous_solution_arr[],
+ matrix_t solution_x1[],
+ matrix_t solution_x2[]);
+
+/**
+ * @brief Solve a linear equation.
+ * @details The linear equation is solved by using the pre-processed pseudo-inverse matrix and the
+ * vector \f$ \vec{b} \f$
+ *
+ * @param[in] line_num row number of the pseudo-inverse matrix.
+ * @param[in] col_num column number of the pseudo-inverse matrix.
+ * @param[in] pseudo_inv_matrix[][] pointer to the pre-processed pseudo-inverse matrix.
+ * @param[in] b_vec[] pointer to the vector \f$ \vec{b} \f$.
+ * @param[out] sol_vec[] solution vector.
+ *
+ */
+void trilateration_solve_linear_equation(uint8_t line_num, uint8_t col_num,
+ matrix_t pseudo_inv_matrix[line_num][col_num],
+ matrix_t b_vec[], matrix_t sol_vec[]);
+
+#endif /* TRILATERATION_H_ */
diff --git a/localization/position_algos/pos_algos_common/trilateration.c b/localization/position_algos/pos_algos_common/trilateration.c
new file mode 100644
index 0000000000000000000000000000000000000000..9fe95707c6b295938933af6c3c2a34bcd2278f6a
--- /dev/null
+++ b/localization/position_algos/pos_algos_common/trilateration.c
@@ -0,0 +1,304 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @{
+ *
+ * @file
+ * @brief Implement the trilateration algorithm.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Abdelmoumen Norrdine <a.norrdine@googlemail.com>
+ *
+ * @}
+ */
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "moore_penrose_pseudo_inverse.h"
+#include "matrix.h"
+#include "svd.h"
+#include "trilateration.h"
+
+void trilateration1(uint8_t anchor_num,
+ matrix_t anchor_pos_matrix[anchor_num][3],
+ matrix_t pseudo_inv_matrix[4][anchor_num],
+ matrix_t homog_sol_arr[],
+ matrix_t dist_arr[],
+ matrix_t solution_x1[],
+ matrix_t solution_x2[]
+ )
+{
+
+ matrix_t b_arr[anchor_num];
+ matrix_t particular_solution_arr[4];
+
+ if (anchor_num == 3) {
+ trilateration_get_b_vector(anchor_num, dist_arr,
+ anchor_pos_matrix, b_arr);
+ trilateration_preprocessed_get_particular_solution(
+ pseudo_inv_matrix, b_arr,
+ particular_solution_arr);
+ trilateration_get_quadratic_equation_solution(
+ particular_solution_arr,
+ homog_sol_arr, solution_x1, solution_x2);
+
+ }
+
+ else if (anchor_num > 3) {
+ trilateration_get_b_vector(anchor_num, dist_arr,
+ anchor_pos_matrix, b_arr);
+
+ //compute the solution
+ trilateration_solve_linear_equation(anchor_num, anchor_num,
+ pseudo_inv_matrix, b_arr,
+ solution_x1);
+ }
+}
+
+//On MCU Processing
+void trilateration2(uint8_t anchor_num,
+ matrix_t anchor_pos_matrix[anchor_num][3],
+ matrix_t dist_arr[],
+ matrix_t solution_x1[],
+ matrix_t solution_x2[])
+{
+
+ matrix_t b_arr[anchor_num];
+ matrix_t A_matrix[anchor_num][4];
+ matrix_t pseudo_inv_matrix[4][anchor_num];
+ uint8_t rank = 0;
+
+ if (anchor_num == 3) {
+ //2nd compute the particular solution
+ matrix_t Xp[4];
+ trilateration_get_particular_solution(anchor_num, anchor_num,
+ anchor_pos_matrix, dist_arr, Xp);
+
+ //3th compute the homogeneous solution
+ matrix_t Xh[4];
+ rank = trilateration_get_rank_and_homogeneous_solution(
+ anchor_num, anchor_pos_matrix, Xh);
+
+ //solve the quadratic equation
+ if (rank == 3) {
+ trilateration_get_quadratic_equation_solution(Xp, Xh,
+ solution_x1,
+ solution_x2);
+ }
+ else {
+ puts("rank!=3, the equation system is not solvable.");
+ }
+
+ }
+
+ else if (anchor_num > 3) {
+ //compute A-matrix and b_vector
+ trilateration_get_A_matrix(anchor_num, anchor_pos_matrix,
+ A_matrix);
+ trilateration_get_b_vector(anchor_num, dist_arr,
+ anchor_pos_matrix, b_arr);
+
+ //compute the pseudo inverse
+ moore_penrose_get_pinv(anchor_num, 4, A_matrix,
+ pseudo_inv_matrix);
+ //compute the solution
+ trilateration_solve_linear_equation(4, anchor_num,
+ pseudo_inv_matrix, b_arr,
+ solution_x1);
+ }
+}
+
+void trilateration_preprocessed_get_particular_solution(
+ matrix_t pseudo_inv_matrix[4][3],
+ matrix_t b_arr[],
+ matrix_t particular_solu_arr[])
+{
+ int i = 0;
+ int j = 0;
+
+ memset(particular_solu_arr, 0.0, 4 * sizeof(matrix_t));
+
+ for (; i < 4; i++) {
+ for (; j < 3; j++) {
+ particular_solu_arr[i] += pseudo_inv_matrix[i][j]
+ * b_arr[j];
+ //DEBUGF("pseudo_inv_matrix[%d][%d] = %f\n b_arr[%d] = %f\n", i, j, (double) pseudo_inv_matrix[i][j], j, (double) b_arr[j]);
+ //printf("pseudo_inv_matrix[%d][%d] = %f\n b_arr[%d] = %f\n", i, j, (double) pseudo_inv_matrix[i][j], j, (double) b_arr[j]);
+ }
+ j = 0;
+ }
+#ifdef DEBUG_ENABLED
+ i = 0;
+ for (; i < 4; i++) {
+ DEBUGF("particular_solution_arr[%d] = %f\n", i,
+ (double)particular_solu_arr[i]);
+ }
+#endif
+}
+
+uint8_t trilateration_get_rank_and_homogeneous_solution(uint8_t anchor_num,
+ matrix_t anchor_pos_matrix[anchor_num][3],
+ matrix_t Xh[])
+{
+ uint8_t rank = 0;
+ matrix_t A[anchor_num][4];
+ uint8_t m = anchor_num;
+ uint8_t n = 4;
+ uint8_t s_length = svd_get_single_values_num(m, n);
+ matrix_t s[s_length];
+ matrix_dim_t u_dim;
+
+ svd_get_U_dim(m, n, &u_dim);
+ matrix_t U[u_dim.row_num][u_dim.col_num];
+ matrix_t S[u_dim.col_num][n];
+ matrix_t V[n][n];
+
+ trilateration_get_A_matrix(anchor_num, anchor_pos_matrix, A);
+ svd(m, n, A, u_dim.row_num, u_dim.col_num, U, S, V, s_length, s);
+
+ //the forth column of the V matix
+ for (int i = 0; i < n; i++) {
+ Xh[i] = V[i][3];
+ }
+
+ rank = matrix_get_rank(m, n, s, s_length);
+
+ return rank;
+}
+
+void trilateration_get_particular_solution(uint8_t m, uint8_t n,
+ matrix_t anchor_pos_mat[m][n],
+ matrix_t dist_arr[], matrix_t Xp[])
+{
+ matrix_t A[m][4];
+ matrix_t pinvA[4][m];
+ matrix_t b[m];
+
+ trilateration_get_b_vector(m, dist_arr, anchor_pos_mat, b);
+ trilateration_get_A_matrix(m, anchor_pos_mat, A);
+ moore_penrose_get_pinv(m, 4, A, pinvA);
+ matrix_mul_vec(4, m, pinvA, b, Xp);
+}
+
+void trilateration_get_quadratic_equation_solution(matrix_t Xp[],
+ matrix_t Xh[],
+ matrix_t solution_x1[],
+ matrix_t solution_x2[])
+{
+
+ matrix_t a = 0, b = 0, c = 0, t1 = 0, t2 = 0, delta = 0;
+ int i = 0;
+
+ //compute polynomial coefficients: a*t^2 + b*t^+ c
+ a = pow(Xh[1], 2)
+ + pow(Xh[2], 2)
+ + pow(Xh[3], 2);
+ b = 2
+ * (Xh[1] * Xp[1]
+ + Xh[2] * Xp[2]
+ + Xh[3] * Xp[3])
+ - Xh[0];
+ c = pow(Xp[1], 2) + pow(Xp[2], 2)
+ + pow(Xp[3], 2) - Xp[0];
+
+ //compute the quadratic equation a*t^2 + b*t+ c
+ delta = b * b - 4 * a * c;
+ if ((delta > 0) && (a != 0)) {
+ if (solution_x1 != NULL) {
+ t1 = (-b + sqrt(delta)) / (2 * a);
+ }
+ if (solution_x2 != NULL) {
+ t2 = (-b - sqrt(delta)) / (2 * a);
+ }
+ }
+ else if ((delta == 0) && (a != 0)) {
+ if (solution_x1 != NULL) {
+ t1 = -b / (2 * a);
+ }
+ if (solution_x2 != NULL) {
+ t2 = t1;
+ }
+
+ }
+ else if (delta < 0) { // take only a real part
+ if (solution_x1 != NULL) {
+ t1 = -b / (2 * a);
+ }
+ if (solution_x2 != NULL) {
+ t2 = -b / (2 * a);
+ }
+ }
+
+ for (; i < 4; i++) {
+ if (solution_x1 != NULL) {
+ solution_x1[i] = Xp[i]
+ + t1 * Xh[i];
+ }
+ if (solution_x2 != NULL) {
+ solution_x2[i] = Xp[i]
+ + t2 * Xh[i];
+ }
+ }
+
+ //DEBUGF("a=%f, b=%f, c=%f, t1=%f, t2=%f\n", (double)a, (double)b, (double)c, (double)t1, (double)t2);
+}
+
+void trilateration_get_A_matrix(uint8_t m, matrix_t anchor_pos_matrix[m][3],
+ matrix_t A_matrix[m][4])
+{
+ int i, j;
+
+ //the first column is equal 1
+ for (i = 0; i < m; i++) {
+ A_matrix[i][0] = 1;
+ }
+
+ //the second, third and firth columns are equal: -2x, -2y, -2z respectively
+ for (i = 0; i < m; i++) {
+ for (j = 1; j < 4; j++) {
+ A_matrix[i][j] = -2 * anchor_pos_matrix[i][j - 1];
+
+ }
+ }
+}
+
+void trilateration_get_b_vector(uint8_t anchor_num, matrix_t dist_arr[],
+ matrix_t anchor_pos_matrix[anchor_num][3],
+ matrix_t b_vec[])
+{
+
+ uint8_t i, j;
+
+ for (i = 0; i < anchor_num; i++) {
+ b_vec[i] = dist_arr[i] * dist_arr[i];
+ for (j = 0; j < 3; j++) {
+ b_vec[i] -= anchor_pos_matrix[i][j]
+ * anchor_pos_matrix[i][j];
+ }
+ }
+}
+
+void trilateration_solve_linear_equation(uint8_t line_num, uint8_t col_num,
+ matrix_t pseudo_inv_matrix[line_num][col_num],
+ matrix_t b_vec[], matrix_t sol_vec[])
+{
+ int i, j;
+
+ memset(sol_vec, 0, col_num * sizeof(matrix_t));
+
+ for (i = 0; i < line_num; i++) {
+ for (j = 0; j < col_num; j++) {
+ sol_vec[i] += pseudo_inv_matrix[i][j] * b_vec[j];
+ }
+ }
+}
diff --git a/localization/position_optimization/doc.txt b/localization/position_optimization/doc.txt
new file mode 100644
index 0000000000000000000000000000000000000000..89c9383ed82262745072c409c542f412596ff347
--- /dev/null
+++ b/localization/position_optimization/doc.txt
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser
+ * General Public License v2.1. See the file LICENSE in the top level
+ * directory for more details.
+ */
+
+/**
+ * @defgroup position_optimization POSITION_OPTIMIZATION
+ * @ingroup localization
+ *
+ * @brief position optimization of distance- and magnetic-based localization systems.
+ *
+ * @details The optimization module enables the refinement of a position of a mobile device
+ * using algorithms such as Gauss--Newton, Levenberg, or a Multipath Distance Detection
+ * and Mitigation (MDDM) algorithm.
+ *
+ * @author Zakaria Kasmi
+ */
diff --git a/localization/position_optimization/include/loc_gauss_newton.h b/localization/position_optimization/include/loc_gauss_newton.h
new file mode 100644
index 0000000000000000000000000000000000000000..9a88480fa13e1d20fd8539b74a85dfd97e1031c2
--- /dev/null
+++ b/localization/position_optimization/include/loc_gauss_newton.h
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @ingroup position_optimization
+ * @{
+ *
+ * @file
+ * @brief Implement the Gauss--Newton algorithm for position optimization.
+ * @note This function is adapted for localization algorithms.
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Abdelmoumen Norrdine <a.norrdine@googlemail.com>
+ *
+ * @}
+ */
+
+#ifndef LOC_GAUSS_NEWTON_H_
+#define LOC_GAUSS_NEWTON_H_
+
+#include <inttypes.h>
+
+#include "matrix.h"
+#include "vector.h"
+
+/**
+ * @brief Implements the modified Gauss--Newton algorithm.
+ * @details The user should provide pointers to the error and Jacobian functions.
+ * @note This function is optimized for localization algorithms.
+ *
+ * @param[in] ref_points_num number of the reference stations.
+ * @param[in] ref_points_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] start_pos[] start (approximate) position.
+ * @param[in] measured_data_vec[] pointer to the measured data.
+ * @param[in] eps accuracy bound.
+ * @param[in] fmin termination tolerance on the error function.
+ * @param[in] max_iter_num maximal iteration number of the Gauss-Newton algorithm.
+ * @param[in, out] est_pos[] estimated (optimized) position.
+ * @param[in] (*f_i) pointer to the error function.
+ * @param[in] (*jacobian_get_JTJ) pointer to the function that calculates the
+ * \f$ J_f^{T} J_{f} \f$ matrix.
+ * @param[in] (*jacobian_get_JTf) pointer to the function that calculates the
+ * \f$ J_f^{T} \vec{f} \f$ vector.
+ *
+ * @return required iteration number.
+ */
+uint8_t loc_gauss_newton(uint8_t ref_points_num,
+ matrix_t ref_points_matrix[ref_points_num][3],
+ vector_t start_pos[3],
+ matrix_t measured_data_vec[ref_points_num],
+ matrix_t eps, matrix_t fmin, uint8_t max_iter_num,
+ vector_t est_pos[3],
+ void (*f_i)(uint8_t ref_point_num,
+ matrix_t ref_point_mat[ref_points_num][3],
+ matrix_t point[3], matrix_t d_vec[],
+ matrix_t f_vec[]),
+ void (*jacobian_get_JTJ)(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTJ[3][3]),
+ void (*jacobian_get_JTf)(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTf[3])
+ );
+
+#endif /* LOC_GAUSS_NEWTON_H_ */
diff --git a/localization/position_optimization/include/loc_levenberg_marquardt.h b/localization/position_optimization/include/loc_levenberg_marquardt.h
new file mode 100644
index 0000000000000000000000000000000000000000..108832e57983da987779e4777ba9d51f79f66693
--- /dev/null
+++ b/localization/position_optimization/include/loc_levenberg_marquardt.h
@@ -0,0 +1,176 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @ingroup position_optimization
+ * @{
+ *
+ * @file
+ * @brief Implement the Levenberg--Marquardt (LVM) algorithm for position optimization.
+ * @note This function is adapted for localization algorithms.
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Naouar Guerchali
+ *
+ * @}
+ */
+
+#ifndef LOC_LEVENBERG_MARQUARDT_H_
+#define LOC_LEVENBERG_MARQUARDT_H_
+
+#include <inttypes.h>
+
+#include "matrix.h"
+
+/**
+ * @brief Implements the Levenberg--Marquardt (LVM) algorithm.
+ * @details The user should provide pointers to the error and Jacobian functions.
+ * @note This function is optimized for localization algorithms.
+ *
+ * @param[in] ref_points_num number of the reference stations.
+ * @param[in] ref_points_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] start_pos[] start (approximate) position.
+ * @param[in] measured_data_vec[] pointer to the measured data.
+ * @param[in] eps accuracy bound.
+ * @param[in] tau \f$ \tau \f$ factor.
+ * @param[in] beta0 \f$ \beta_0 \f$ factor.
+ * @param[in] beta1 \f$ \beta_1 \f$ factor.
+ * @param[in] max_iter_num maximal iteration number of the LVM algorithm.
+ * @param[out] est_pos[] estimated (optimized) position.
+ * @param[in] (*f_i) pointer to the error function.
+ * @param[in] (*jacobian_get_JTJ) pointer to the function that calculates the matrix
+ * \f$ J_f^{T} J_{f} \f$.
+ * @param[in] (*jacobian_get_JTf) pointer to the function that calculates the vector
+ * \f$ J_f^{T} \vec{f} \f$.
+ * @param[in] (*jacobian_get_J_mul_s) pointer to the function that calculates the vector
+ * \f$ J_f^{T} \vec{s} \f$.
+ *
+ * @return required iteration number.
+ */
+uint8_t loc_levenberg_marquardt(uint8_t ref_points_num,
+ matrix_t ref_points_matrix[ref_points_num][3],
+ matrix_t start_pos[3],
+ matrix_t measured_data_vec[ref_points_num],
+ matrix_t eps, matrix_t tau, matrix_t beta0,
+ matrix_t beta1, uint8_t max_iter_num,
+ matrix_t est_pos[3],
+ void (*f_i)(uint8_t ref_points_num,
+ matrix_t ref_point_mat[ref_points_num][3],
+ matrix_t point[3], matrix_t d_vec[], matrix_t f_vec[]),
+ void (*jacobian_get_JTJ)(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTJ[3][3]),
+ void (*jacobian_get_JTf)(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTf[3]),
+ void (*jacobian_get_J_mul_s)(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[][3],
+ matrix_t point[3],
+ matrix_t s[3],
+ matrix_t J_s[ref_points_num])
+ );
+
+/**
+ * @brief Implements the correction-function of the Levenberg--Marquardt (LVM) algorithm.
+ * @details The user should provide pointers to the error and Jacobian functions.
+ * @note This function is optimized for localization algorithms.
+ *
+ * @param[in] ref_points_num number of the reference stations.
+ * @param[in] ref_points_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] point[] a position to adjust.
+ * @param[in] measured_data_vec[] pointer to the measured data.
+ * @param[in] mu regularization parameter \f$ \mu \f$.
+ * @param[out] s[] correction vector.
+ * @param[in] (*f_i) pointer to the error function.
+ * @param[in] (*jacobian_get_JTJ) pointer to the function that calculates the matrix
+ * \f$ J_f^{T} J_{f} \f$.
+ * @param[in] (*jacobian_get_JTf) pointer to the function that calculates the vector
+ * \f$ J_f^{T} \vec{f} \f$.
+ * @param[in] (*jacobian_get_J_mul_s) pointer to the function that calculates the vector
+ * \f$ J_f^{T} \vec{s} \f$
+ *
+ *
+ * @return the parameter \f$ \rho_{\mu} \f$
+ */
+matrix_t loc_levenberg_marquardt_correction(uint8_t ref_points_num,
+ matrix_t ref_points_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t measured_data_vec[ref_points_num],
+ matrix_t mu, matrix_t s[3],
+ void (*f_i)(uint8_t ref_point_num,
+ matrix_t ref_point_mat[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t d_vec[], matrix_t f_vec[]),
+ void (*jacobian_get_JTJ)(
+ uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTJ[3][3]),
+ void (*jacobian_get_JTf)(
+ uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTf[3]),
+ void (*jacobian_get_J_mul_s)(
+ uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3], matrix_t s[3],
+ matrix_t J_s[ref_points_num])
+ );
+/**
+ * @brief Compute the initial value \f$ \mu_0 \f$ of the Levenberg--Marquardt (LVM) algorithm.
+ * @details The user should provide a pointer to the matrix
+ * \f$ J_f^{T} J_{f} \f$.
+ *
+ *
+ * @param[in] tau \f$ \tau \f$ factor.
+ * @param[in] JTJ[][] pointer to the matrix \f$ J_f^T J_f \f$.
+ *
+ * @return \f$ \rho_{\mu} \f$-parameter
+ */
+matrix_t loc_levenberg_marquardt_get_mu0(matrix_t tau, matrix_t JTJ[3][3]);
+
+//compute: J'J + mu^2*I
+
+/**
+ * @brief Compute the matrix
+ * \f$ J_f^{T} J_f + \mu^{(i)})^{2} I \f$.
+ * @note This function is optimized for localization algorithms.
+ *
+ * @param[in] ref_points_num number of the reference stations.
+ * @param[in] ref_points_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] point[] a position to adjust.
+ * @param[in] measured_data_vec[] pointer to the measured data.
+ * @param[in] mu \f$ \rho_{\mu} \f$-parameter
+ * @param[out] JTJ_mu2_I[][] includes the matrix
+ * \f$ J_f^{T} J_f + \mu^{(i)})^{2} I \f$.
+ * @param[in] (*jacobian_get_JTJ) pointer to the function that calculates the matrix
+ * \f$ J_f^{T} J_{f} \f$.
+ *
+ */
+void loc_levenberg_marquardt_get_JTJ_mu2_I(uint8_t ref_points_num,
+ matrix_t ref_points_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t measured_data_vec[ref_points_num], matrix_t mu,
+ matrix_t JTJ_mu2_I[3][3],
+ void (*jacobian_get_JTJ)(
+ uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTJ[3][3])
+ );
+
+#endif /* LOC_LEVENBERG_MARQUARDT_H_*/
diff --git a/localization/position_optimization/include/multipath_dist_detection_mitigation.h b/localization/position_optimization/include/multipath_dist_detection_mitigation.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e7089da3a6220bf756ccdc04c9590b2e28932a5
--- /dev/null
+++ b/localization/position_optimization/include/multipath_dist_detection_mitigation.h
@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @ingroup position_optimization
+ * @{
+ *
+ * @file
+ * @brief Implement the Multipath Distance Detection and Mitigation (MDDM) algorithm.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Abdelmoumen Norrdine <a.norrdine@googlemail.com>
+ * @author Naouar Guerchali
+ *
+ * @}
+ */
+
+#ifndef POSITIONING_ROBUST_OTF_H_
+#define POSITIONING_ROBUST_OTF_H_
+
+#include <inttypes.h>
+
+#include "matrix.h"
+
+/**
+ * @brief Computes the exact distance between a mobile station and a reference station.
+ *
+ *
+ * @param[in] ref_point three-dimensional coordinates of a reference station.
+ * @param[in] exact_point three-dimensional coordinates of the mobile device.
+ *
+ * @return the exact distance between the mobile station and the reference station.
+ */
+matrix_t get_exact_distance_to_anchor(matrix_t ref_point[],
+ uint32_t exact_point[]);
+/**
+ * @brief Determine if a candidate is a multipath or not.
+ *
+ * @param[in] vector a candidate.
+ * @param[in] n size of the multipath-vector.
+ * @param[in] multipath[] pointer to the multipath-vector.
+ *
+ * @return true, if a candidate is a multipath.
+ * @return false, if not.
+ *
+ */
+bool is_member(matrix_t vector, uint8_t n, matrix_t multipath[n]);
+
+/**
+ * @brief Determine if a point is an anchor or not.
+ *
+ * @param[in] m number of the reference points.
+ * @param[in] ref_matr[][] three-dimensional coordinates of the reference stations.
+ * @param[in] point[] three-dimensional coordinates of a point.
+ *
+ * @return true, if a point is an anchor.
+ * @return false, if not.
+ */
+bool is_anchor(uint8_t m, matrix_t ref_matr[m][3], uint32_t point[3]);
+
+/**
+ * @brief Simulate an UWB-based localization system.
+ * @details The UWB system is simulated by noising the distances from a mobile to the reference
+ * station.
+ *
+ * @param[in] m number of the reference points.
+ * @param[in] ref_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] exact_point[] three-dimensional coordinates of a true position.
+ * @param[in] sigma \f$ \sigma_{M,W} \f$ parameter.
+ * @param[in] n number of the multipath-affected distances.
+ * @param[in] multipath[] multipath vector.
+ * @param[in] seed seed value.
+ * @param[in] r_noised_vec[] noised distances.
+ *
+ */
+void sim_UWB_dist(uint8_t m, matrix_t ref_matrix[m][3], uint32_t exact_point[],
+ matrix_t sigma, uint8_t n, matrix_t multipath[n], int seed,
+ matrix_t r_noised_vec[]);
+/**
+ * @brief Compute the optimal partial matrix including reference points.
+ *
+ * @param[in] anchors_num number of the reference points.
+ * @param[in] ref_matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] k k anchors (possible sub-experiments).
+ * @param[in] optimal_anchors_comb[] optimal anchor combination.
+ * @param[out] opt_partial_ref_matrix[][] optimal partial matrix of anchors.
+ *
+ */
+void get_optimal_partial_ref_matrix(uint8_t anchors_num,
+ matrix_t ref_matrix[anchors_num][3],
+ uint8_t k, uint8_t optimal_anchors_comb[k],
+ matrix_t opt_partial_ref_matrix[k][3]);
+
+/**
+ * @brief Compute noised distances corresponding to the optimal partial matrix.
+ *
+ * @param[in] k k anchors (possible sub-experiments).
+ * @param[in] r_noised_vec[] include noised distances.
+ * @param[in] optimal_anchors_comb[] optimal k-anchor combination.
+ * @param[in, out] opt_sub_r_noised_vec[][] noised distances corresponding to the optimal
+ * k-anchors.
+ *
+ */
+void get_optimal_partial_r_noised_vec(uint8_t k, matrix_t r_noised_vec[],
+ uint8_t optimal_anchors_comb[k],
+ matrix_t opt_sub_r_noised_vec[k]);
+
+/**
+ * @brief Implement the Multipath Distance Detection and Mitigation (MDDM) algorithm.
+ *
+ * @param[in] k k anchors (possible sub-experiments).
+ * @param[in] m number of anchors.
+ * @param[in] ref_Matrix[][] three-dimensional coordinates of the reference stations.
+ * @param[in] r_noised_vec[] include noised distances.
+ * @param[in] anchors_optimal[] optimal k-anchor combination.
+ * @param[in, out] start_optimal[] optimal position.
+ *
+ */
+void recog_mitigate_multipath(uint8_t k, uint8_t m, matrix_t ref_Matrix[m][3],
+ matrix_t r_noised_vec[m],
+ uint8_t anchors_optimal[k],
+ matrix_t start_optimal[3]);
+
+#endif /* POSITIONING_ROBUST_OTF_H_ */
diff --git a/localization/position_optimization/loc_gauss_newton.c b/localization/position_optimization/loc_gauss_newton.c
new file mode 100644
index 0000000000000000000000000000000000000000..4c352a313b4dd6b9a819a7e1198a2d4782c5e2f8
--- /dev/null
+++ b/localization/position_optimization/loc_gauss_newton.c
@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @{
+ *
+ * @file
+ * @brief Implement the Gauss--Newton algorithm.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Abdelmoumen Norrdine <a.norrdine@googlemail.com>
+ *
+ * @}
+ */
+
+#include <stdio.h>
+
+#include "utils.h"
+#include "matrix.h"
+#include "vector.h"
+#include "moore_penrose_pseudo_inverse.h"
+
+uint8_t loc_gauss_newton(uint8_t ref_points_num,
+ matrix_t ref_points_matrix[ref_points_num][3],
+ vector_t start_pos[3],
+ matrix_t measured_data_vec[ref_points_num],
+ matrix_t eps, matrix_t fmin, uint8_t max_iter_num,
+ vector_t est_pos[3],
+ void (*f_i)(uint8_t ref_point_num,
+ matrix_t ref_point_mat[ref_points_num][3],
+ matrix_t point[3], matrix_t d_vec[],
+ matrix_t f_vec[]),
+ void (*jacobian_get_JTJ)(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTJ[3][3]),
+ void (*jacobian_get_JTf)(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTf[3])
+ )
+{
+
+ matrix_t JT_J[3][3];
+ matrix_t JT_f[3];
+ matrix_t f_vec[ref_points_num];
+ matrix_t f_error;
+ matrix_t pinv_JTJ_mat[3][3];
+ matrix_t correction_vec[3];
+ matrix_t pos[3];
+ matrix_t next_pos[3];
+ matrix_t max_error, min_error;
+ matrix_t step;
+ uint8_t iter_num;
+
+ f_i(ref_points_num, ref_points_matrix, start_pos, measured_data_vec,
+ f_vec);
+ f_error = vector_get_norm2(ref_points_num, f_vec);
+ max_error = f_error;
+ min_error = max_error;
+ step = eps;
+
+ vector_copy(3, start_pos, pos);
+ vector_copy(3, start_pos, est_pos);
+ iter_num = 0;
+ while ((step >= eps) && (iter_num < max_iter_num) && (f_error > fmin)) {
+ /*
+ * Compute then correction terms & next positions
+ */
+
+ //JTJ
+ jacobian_get_JTJ(ref_points_num, ref_points_matrix, pos,
+ measured_data_vec, JT_J);
+
+ //JTf
+ jacobian_get_JTf(ref_points_num, ref_points_matrix, pos,
+ measured_data_vec, JT_f);
+
+ //solve: J'J*s = -J'*f
+ moore_penrose_get_pinv(3, 3, JT_J, pinv_JTJ_mat);
+ //s = (J'J)\J'*f
+ matrix_mul_vec(3, 3, pinv_JTJ_mat, JT_f, correction_vec);
+
+ // x = x - s
+ vector_sub(3, pos, correction_vec, next_pos);
+
+ //next step
+ step = vector_get_euclidean_distance(3, pos, next_pos);
+
+ // pos = next_pos
+ vector_copy(3, next_pos, pos);
+
+ //error vector
+ f_i(ref_points_num, ref_points_matrix, pos, measured_data_vec,
+ f_vec);
+ f_error = vector_get_norm2(ref_points_num, f_vec);
+
+ if (min_error > f_error) { //store the position with the minimum error
+ vector_copy(3, pos, est_pos);
+ min_error = f_error; // update min_error
+ }
+
+ max_error = utils_max(f_error, max_error); // update max_error
+ iter_num++;
+ if ((max_error - min_error) > 10) {
+ break;
+ }
+
+ } //while
+
+ return iter_num;
+}
diff --git a/localization/position_optimization/loc_levenberg_marquardt.c b/localization/position_optimization/loc_levenberg_marquardt.c
new file mode 100644
index 0000000000000000000000000000000000000000..36d5b2b187c728d78c23fd6e0149afcf182ec7f6
--- /dev/null
+++ b/localization/position_optimization/loc_levenberg_marquardt.c
@@ -0,0 +1,258 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @{
+ *
+ * @file
+ * @brief Implement the Levenberg--Marquardt (LVM) algorithm.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Naouar Guerchali
+ *
+ * @}
+ */
+
+#include <stdio.h>
+#include <math.h>
+#include <float.h>
+#include <inttypes.h>
+
+#include "matrix.h"
+#include "vector.h"
+#include "solve.h"
+#include "utils.h"
+#include "loc_levenberg_marquardt.h"
+
+matrix_t loc_levenberg_marquardt_correction(uint8_t ref_points_num,
+ matrix_t ref_points_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t measured_data_vec[ref_points_num], matrix_t mu,
+ matrix_t s[3],
+ void (*f_i)(uint8_t ref_point_num,
+ matrix_t ref_point_mat[ref_points_num][3],
+ matrix_t point[3], matrix_t d_vec[],
+ matrix_t f_vec[]),
+ void (*jacobian_get_JTJ)(
+ uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTJ[3][3]),
+ void (*jacobian_get_JTf)(
+ uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTf[3]),
+ void (*jacobian_get_J_mul_s)(
+ uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3], matrix_t s[3],
+ matrix_t J_s[ref_points_num])
+ )
+{
+
+ matrix_t Fx[ref_points_num];
+ matrix_t JTJ_mu2_I[3][3];
+ matrix_t JTF[3];
+ matrix_t ro_mu = 0;
+ matrix_t Fx_square = 0;
+ matrix_t Fx_plus_s_square = 0;
+ matrix_t Fx_plus_J_mul_s_square = 0;
+ matrix_t Fx_plus_J_mul_s[ref_points_num];
+ matrix_t F_x_plus_s[ref_points_num];
+ matrix_t point_plus_s[3];
+ matrix_t denom;
+
+ //compute: -(J'J + mu^2*I)
+ loc_levenberg_marquardt_get_JTJ_mu2_I(ref_points_num, ref_points_matrix,
+ point,
+ measured_data_vec, mu, JTJ_mu2_I,
+ jacobian_get_JTJ);
+ matrix_mul_scalar(3, 3, JTJ_mu2_I, -1, JTJ_mu2_I); // -(J'*J+mu^2*eye(n)) is an (n,n) matrix
+
+ jacobian_get_JTf(ref_points_num, ref_points_matrix, point,
+ measured_data_vec, JTF);
+
+ //solve the equation: s = -(J'*J+mu^2*I)\(J'*Fx);
+ solve_householder(3, 3, JTJ_mu2_I, JTF, s);
+
+ //f(x)
+ f_i(ref_points_num, ref_points_matrix, point, measured_data_vec, Fx);
+ //f(x)^2
+ Fx_square = vector_get_scalar_product(ref_points_num, Fx, Fx);
+
+ //(x+s)
+ vector_add(3, point, s, point_plus_s);
+
+ //f(x+s)
+ f_i(ref_points_num, ref_points_matrix, point_plus_s, measured_data_vec,
+ F_x_plus_s);
+ //f(x+s)^2
+ Fx_plus_s_square = vector_get_scalar_product(ref_points_num, F_x_plus_s,
+ F_x_plus_s);
+
+ //compute: J(x)*s
+ jacobian_get_J_mul_s(ref_points_num, ref_points_matrix, point, s,
+ Fx_plus_J_mul_s);
+ //compute: f(x) + J(x)*s
+ vector_add(ref_points_num, Fx, Fx_plus_J_mul_s, Fx_plus_J_mul_s);
+
+ //compute: ||f(x) + J(x)*s||^2
+ Fx_plus_J_mul_s_square = vector_get_scalar_product(ref_points_num,
+ Fx_plus_J_mul_s,
+ Fx_plus_J_mul_s);
+
+ denom = Fx_square - Fx_plus_J_mul_s_square;
+ if (denom != 0) {
+ ro_mu = (Fx_square - Fx_plus_s_square) / denom;
+ }
+ else {
+ puts("ro_mu is infinite !!!");
+ ro_mu = FLT_MAX;
+ }
+
+ return ro_mu;
+}
+
+uint8_t loc_levenberg_marquardt(uint8_t ref_points_num,
+ matrix_t ref_points_matrix[ref_points_num][3],
+ matrix_t start_pos[3],
+ matrix_t measured_data_vec[ref_points_num],
+ matrix_t eps, matrix_t tau, matrix_t beta0,
+ matrix_t beta1,
+ uint8_t max_iter_num,
+ matrix_t est_pos[3],
+ void (*f_i)(uint8_t ref_points_num,
+ matrix_t ref_point_mat[ref_points_num][3],
+ matrix_t point[3], matrix_t d_vec[],
+ matrix_t f_vec[]),
+ void (*jacobian_get_JTJ)(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTJ[3][3]),
+ void (*jacobian_get_JTf)(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTf[3]),
+ void (*jacobian_get_J_mul_s)(uint8_t ref_points_num,
+ matrix_t ref_point_matrix[][3],
+ matrix_t point[3], matrix_t s[3],
+ matrix_t J_s[ref_points_num])
+ )
+{
+
+ matrix_t JTF[3];
+ matrix_t JTJ_mu2_I[3][3];
+ matrix_t s[3];
+ matrix_t mu;
+ matrix_t ro_mu;
+ uint8_t it;
+
+ /*
+ * compute mu0 and -(J'J + mu0^2*I)
+ */
+ //store J'J in JTJ_mu2_I
+ jacobian_get_JTJ(ref_points_num, ref_points_matrix, start_pos,
+ measured_data_vec, JTJ_mu2_I);
+ mu = loc_levenberg_marquardt_get_mu0(tau, JTJ_mu2_I);
+
+ //compute: J'J + mu0^2*I
+ loc_levenberg_marquardt_get_JTJ_mu2_I(ref_points_num, ref_points_matrix,
+ start_pos, measured_data_vec, mu,
+ JTJ_mu2_I,
+ jacobian_get_JTJ);
+ //-(J'*J+mu0^2*eye(n)) is an (n,n) matrix
+ matrix_mul_scalar(3, 3, JTJ_mu2_I, -1, JTJ_mu2_I);
+
+ //compute JTF
+ jacobian_get_JTf(ref_points_num, ref_points_matrix, start_pos,
+ measured_data_vec, JTF);
+
+ //solve the equation: s=-(J'*J+mu^2*eye(n))\(J'*F);
+ solve_householder(3, 3, JTJ_mu2_I, JTF, s);
+
+ vector_copy(3, start_pos, est_pos);
+ it = 0;
+ while ((vector_get_norm2(3, s)
+ > eps * (1 + vector_get_norm2(3, est_pos)))
+ && (it < max_iter_num)) { //norm(s,2)>eps*(1+norm(x,2))
+
+ ro_mu = loc_levenberg_marquardt_correction(ref_points_num,
+ ref_points_matrix, est_pos,
+ measured_data_vec,
+ mu, s, f_i, jacobian_get_JTJ,
+ jacobian_get_JTf,
+ jacobian_get_J_mul_s);
+
+ while (1) {
+ if (ro_mu <= beta0) {
+ mu = 2.0 * mu;
+ ro_mu = loc_levenberg_marquardt_correction(
+ ref_points_num,
+ ref_points_matrix, est_pos,
+ measured_data_vec, mu, s, f_i,
+ jacobian_get_JTJ,
+ jacobian_get_JTf,
+ jacobian_get_J_mul_s);
+ }
+ else if (ro_mu >= beta1) {
+ mu = mu / 2.0;
+ break;
+ }
+ else {
+ break;
+ }
+ }
+ vector_add(3, est_pos, s, est_pos);
+ it = it + 1;
+ } //while
+ return it;
+}
+
+matrix_t loc_levenberg_marquardt_get_mu0(matrix_t tau, matrix_t JTJ[3][3])
+{
+ matrix_t max_diag_JTJ = JTJ[0][0];
+
+ for (uint8_t i = 1; i < 3; i++) {
+ if (JTJ[i][i] > max_diag_JTJ) {
+ max_diag_JTJ = JTJ[i][i];
+ }
+ }
+ return tau * max_diag_JTJ;
+}
+
+//compute: J'J + mu^2*I
+void loc_levenberg_marquardt_get_JTJ_mu2_I(uint8_t ref_points_num,
+ matrix_t ref_points_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t measured_data_vec[ref_points_num],
+ matrix_t mu,
+ matrix_t JTJ_mu2_I[3][3],
+ void (*jacobian_get_JTJ)(
+ uint8_t ref_points_num,
+ matrix_t ref_point_matrix[ref_points_num][3],
+ matrix_t point[3],
+ matrix_t data_vec[ref_points_num],
+ matrix_t JTJ[3][3])
+ )
+{
+ uint8_t i;
+
+ jacobian_get_JTJ(ref_points_num, ref_points_matrix, point,
+ measured_data_vec, JTJ_mu2_I); //JTJ_mu2_I = J'J
+
+ for (i = 0; i < 3; i++) {
+ JTJ_mu2_I[i][i] += pow(mu, 2); // JTJ_mu2_I = J'*J+mu^2*I;
+ }
+}
diff --git a/localization/position_optimization/multipath_dist_detection_mitigation.c b/localization/position_optimization/multipath_dist_detection_mitigation.c
new file mode 100644
index 0000000000000000000000000000000000000000..b88c692a43786556419c295fc79bbea104de7739
--- /dev/null
+++ b/localization/position_optimization/multipath_dist_detection_mitigation.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * 2020 Freie Universität Berlin
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @{
+ *
+ * @file
+ * @brief Implement the Multipath Distance Detection and Mitigation (MDDM) algorithm.
+ *
+ *
+ * @author Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
+ * @author Abdelmoumen Norrdine <a.norrdine@googlemail.com>
+ * @author Naouar Guerchali
+ *
+ * @}
+ */
+
+#include <math.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <float.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "matrix.h"
+#include "vector.h"
+#include "combinatorics.h"
+#include "multipath_dist_detection_mitigation.h"
+#include "norm_dist_rnd_generator.h"
+#include "shell_sort.h"
+#include "dist_based_position.h"
+#include "DOP.h"
+#include "trilateration.h"
+
+matrix_t get_exact_distance_to_anchor(matrix_t ref_point[],
+ uint32_t exact_point[])
+{
+ matrix_t d = sqrt(
+ pow((exact_point[0] - ref_point[0]), 2)
+ + pow((exact_point[1] - ref_point[1]),
+ 2)
+ + pow((exact_point[2] - ref_point[2]),
+ 2));
+
+ return d;
+}
+
+bool is_member(matrix_t vector, uint8_t n, matrix_t multipath[n])
+{
+ for (int i = 0; i < n; i++) {
+ if (vector == multipath[i]) {
+ return true;
+ }
+ }
+ return false;
+}
+
+bool is_anchor(uint8_t m, matrix_t ref_matr[m][3], uint32_t point[3])
+{
+
+ for (uint8_t i = 0; i < m; i++) {
+ if (point[0] == ref_matr[i][0] && point[1] == ref_matr[i][1]
+ && point[2] == ref_matr[i][2]) {
+ return true;
+ }
+
+ }
+ return false;
+}
+
+void sim_UWB_dist(uint8_t m, matrix_t ref_matrix[m][3], uint32_t exact_point[],
+ matrix_t sigma, uint8_t n, matrix_t multipath[n], int seed,
+ matrix_t r_noised_vec[])
+{
+ matrix_t M_UW = 0.88;
+ matrix_t sigma_UW = 1.522;
+ matrix_t R = 0.0;
+
+ // set seed
+ get_rand_num(seed);
+
+ for (int i = 0; i < m; i++) {
+ matrix_t d = get_exact_distance_to_anchor(ref_matrix[i],
+ exact_point);
+
+ if (is_member(i + 1, n, multipath) == true) {
+
+ R = log(1 + d)
+ * get_norm_distr_rand_num(M_UW,
+ sigma_UW);
+
+ }
+ else {
+
+ R = sigma * get_norm_distr_rand_num(0, 1);
+ }
+ r_noised_vec[i] = R + d;
+ }
+}
+
+void get_optimal_partial_ref_matrix(uint8_t anchors_num,
+ matrix_t ref_matrix[anchors_num][3],
+ uint8_t k,
+ uint8_t optimal_anchors_comb[k],
+ matrix_t opt_partial_ref_matrix[k][3])
+{
+ for (int i = 0; i < k; i++) {
+ for (int j = 0; j < 3; j++) {
+ opt_partial_ref_matrix[i][j] =
+ ref_matrix[optimal_anchors_comb[i]][j];
+
+ }
+ }
+}
+
+void get_optimal_partial_r_noised_vec(uint8_t k, matrix_t r_noised_vec[],
+ uint8_t optimal_anchors_comb[k],
+ matrix_t opt_sub_r_noised_vec[k])
+{
+ for (int i = 0; i < k; i++) {
+ opt_sub_r_noised_vec[i] = r_noised_vec[optimal_anchors_comb[i]];
+
+ }
+}
+
+void recog_mitigate_multipath(uint8_t k, uint8_t m, matrix_t ref_matrix[m][3],
+ matrix_t noised_r_vec[m],
+ uint8_t anchors_optimal_combi[k],
+ matrix_t start_optimal_pos[3])
+{
+ int8_t status_num;
+ uint8_t combi_arr[k];
+ matrix_t r_vec[3];
+ matrix_t part_opt_ref_matrix[k][3];
+ matrix_t opt_part_r_noised_vec[k];
+ uint8_t index_vector[k];
+ matrix_t qres_optimal = FLT_MAX;
+ matrix_t pos_solution_x1[4];
+
+ // Initialize the combination generator
+ status_num = combinatorics_init(m, k, combi_arr);
+
+ if (status_num == COMBI_ERROR) {
+ fprintf(stderr,
+ "Error: couldn't initialize the combination generator\n");
+
+ return;
+ }
+
+ while (status_num == COMBI_SUCCESS) {
+ get_optimal_partial_ref_matrix(m, ref_matrix, k, combi_arr,
+ part_opt_ref_matrix);
+
+ get_optimal_partial_r_noised_vec(k, noised_r_vec, combi_arr,
+ opt_part_r_noised_vec);
+ // Trilateration
+ trilateration2(k, part_opt_ref_matrix, opt_part_r_noised_vec,
+ pos_solution_x1,
+ NULL);
+
+ matrix_t residual_vec[m];
+ matrix_t residual_vec_copy[m];
+
+ for (int i = 0; i < m; i++) {
+
+ vector_sub(3, pos_solution_x1 + 1, ref_matrix[i],
+ r_vec);
+ residual_vec[i] = vector_get_norm2(3, r_vec)
+ - noised_r_vec[i];
+ }
+
+ vector_square(m, residual_vec, residual_vec);
+
+ vector_copy(m, residual_vec, residual_vec_copy);
+
+ shell_sort(residual_vec_copy, m);
+
+ // find index
+ vector_get_index_vector(k, m, residual_vec, residual_vec_copy,
+ index_vector);
+
+ matrix_t res_vec_copy_sum = 0;
+
+ for (int i = 0; i < k; i++) {
+ res_vec_copy_sum = res_vec_copy_sum
+ + residual_vec_copy[i];
+ }
+
+ if (res_vec_copy_sum <= qres_optimal) {
+ qres_optimal = res_vec_copy_sum;
+ memcpy(anchors_optimal_combi, index_vector,
+ k * sizeof(uint8_t));
+ vector_copy(3, pos_solution_x1 + 1, start_optimal_pos);
+ }
+ status_num = combinatorics_get_next_without_rep(m, k,
+ combi_arr);
+ } //while
+}