diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 5bcd6add6289ee2e128a7615f0b67fa6e1112840..abcbc78d2bfd5d2c47eeed299f9d155d9c497e69 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -15,7 +15,7 @@ struct TdcpBatchParams
   double max_residual; // max allowed residual to be considered good solution. RAIM applied if enabled in this case.
   bool   relinearize_jacobian;
   int    max_iterations;
-  int    residual_opt; // 0: RMS of residual vector. 1: Max residual in Mahalanobis squared distance
+  int    residual_opt; // 0: Normalized RMS of residual vector. 1: Max residual in Mahalanobis squared distance
 };
 
 struct TdcpOutput
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index 23f981a77d2d3d5ed2b711e06dc83f150d35f6d8..e6503da3ce287c552506e01bf320cff0f959d15e 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -145,8 +145,8 @@ TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
     if (n_common_sats < std::max(tdcp_params.min_common_sats, 4))
     {
         #if GNSS_UTILS_TDCP_DEBUG == 1
-        printf("Tdcp: NOT ENOUGH COMMON SATS");
-        printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]", n_common_sats, std::max(tdcp_params.min_common_sats, 4));
+        printf("Tdcp: NOT ENOUGH COMMON SATS\n");
+        printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]\n", n_common_sats, std::max(tdcp_params.min_common_sats, 4));
         #endif
         output.msg = "Not enough common sats";
         output.success = false;
@@ -202,7 +202,7 @@ TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
 
     #if GNSS_UTILS_TDCP_DEBUG == 1
         std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
-        std::cout << "d initial guess: " << d.transpose() << std::endl;
+        std::cout << "d initial guess: " << d_0.transpose() << std::endl;
         std::cout << "common sats: ";
         for (auto row_sat_freq_it : row_2_sat_freq)
             std::cout << row_sat_freq_it.second.first << " ";
@@ -368,63 +368,130 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
                 std::cout << "Tdcp: cov_d         = \n" << cov_d << std::endl;
                 printf("Tdcp: dT            = %8.3f secs\n", d(3));
             #endif
-            output.msg = "NaN values in NLS";
+            output.msg = "Na"
+                    "N values in NLS";
             output.success = false;
             return output;
         }
 
         // residual
-        if (tdcp_params.residual_opt == 0) // RMSE
-            residual = sqrt((r - A * delta_d).squaredNorm() / A.rows()) / sqrt(var_tdcp);
+        if (tdcp_params.residual_opt == 0) // RMSE normalized
+            residual = sqrt((r + A * delta_d).squaredNorm() / A.rows()) / sqrt(var_tdcp);
         else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise)
-            residual = (r - A * delta_d).cwiseAbs2().maxCoeff() / var_tdcp;
+            residual = (r + A * delta_d).cwiseAbs2().maxCoeff() / var_tdcp;
         else
             throw std::runtime_error("unknown value for residual_opt");
         // residual = (r + A * delta_d).norm();
 
         #if GNSS_UTILS_TDCP_DEBUG == 1
-            std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl;
-            std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
-            printf("Ref distance      = %7.3f m\n", d_0.norm());
-            printf("Computed distance = %7.3f m\n", d.head<3>().norm());
-            printf("Tdcp: residual      = %13.10f\n", residual);
-            printf("Tdcp: rows          = %lu\n", r.rows());
-            std::cout << "Tdcp: r             = " << r.transpose() << std::endl;
-            std::cout << "Tdcp: drho_m        = " << drho_m.transpose() << std::endl;
-            std::cout << "Tdcp: A             = \n" << A << std::endl;
-            std::cout << "Tdcp: H             = \n" << A.transpose() * A << std::endl;
-            std::cout << "Tdcp: cov_d         = \n" << cov_d << std::endl;
-            printf("Tdcp: dT            = %8.3f secs\n", d(3));
+            if (j == 0)
+            {
+                std::cout << "\n//////////////// Initial solution at first iteration ////////////////\n";
+                std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl;
+                std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
+                printf("Ref distance      = %7.3f m\n", d_0.norm());
+                printf("Computed distance = %7.3f m\n", d.head<3>().norm());
+                printf("Tdcp: residual      = %13.10f\n", residual);
+                printf("Tdcp: rows          = %lu\n", r.rows());
+                std::cout << "Tdcp: r             = " << r.transpose() << std::endl;
+                std::cout << "Tdcp: r+A*delta_d   = " << (r + A * delta_d).transpose() << std::endl;
+                std::cout << "Tdcp: drho_m        = " << drho_m.transpose() << std::endl;
+                std::cout << "Tdcp: A             = \n" << A << std::endl;
+                std::cout << "Tdcp: H             = \n" << A.transpose() * A << std::endl;
+                std::cout << "Tdcp: cov_d         = \n" << cov_d << std::endl;
+                printf("Tdcp: dT            = %8.3f secs\n", d(3));
+                std::cout << "Check RAIM conditions...\n";
+                std::cout << "\titeration == 0:                         " << (j==0 ? "OK\n":"FAILED\n");
+                std::cout << "\tA.rows() > tdcp_params.min_common_sats: " << (A.rows() > tdcp_params.min_common_sats ? "OK\n":"FAILED\n");
+                std::cout << "\ttdcp_params.raim_n > 0:                 " << (tdcp_params.raim_n > 0 ? "OK\n":"FAILED\n");
+                std::cout << "\tresidual > tdcp_params.max_residual:    " << (residual > tdcp_params.max_residual ? "OK\n":"FAILED\n");
+            }
         #endif
 
-        // RAIM ====================================== (at first iteration)
+        // RAIM ====================================== (after first iteration)
         if (j == 0 and
-            A.cols() > tdcp_params.min_common_sats and
+            A.rows() > tdcp_params.min_common_sats and
             tdcp_params.raim_n > 0 and
             residual > tdcp_params.max_residual)
         {
+            #if GNSS_UTILS_TDCP_DEBUG == 1
+                std::cout << "//////////////// Performing RAIM ////////////////\n";
+            #endif
+
             int             worst_sat_row = -1;
             double          best_residual = 1e12;
             Eigen::Vector4d best_d;
 
             // remove up to 'raim_n' observations (if enough satellites and residual condition holds)
             while (raim_discarded_rows.size() < tdcp_params.raim_n and
-                   A.cols() - raim_discarded_rows.size() > tdcp_params.min_common_sats and
+                   A.rows() - raim_discarded_rows.size() > tdcp_params.min_common_sats and
                    residual > tdcp_params.max_residual)
             {
                 auto A_raim = A;
                 auto r_raim = r;
 
-                // solve removing each row
-                for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
+                // METHOD A: using normalized RMSE residual
+                if (tdcp_params.residual_opt == 0)
+                {
+                    // solve removing each row
+                    for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
+                    {
+                        // skip already discarded rows
+                        if (raim_discarded_rows.count(row_removed) != 0)
+                            continue;
+
+                        // remove row
+                        A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose();
+                        r_raim(row_removed)     = 0;
+
+                        // solve
+                        Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
+                        Eigen::Vector4d d_raim       = d_0 + delta_d_raim;
+
+                        // no NaN
+                        if (!d_raim.array().isNaN().any())
+                        {
+                            // residual
+                            if (tdcp_params.residual_opt == 0) // RMSE
+                                residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / (A_raim.rows() - raim_discarded_rows.size() - 1)) / sqrt(var_tdcp);
+                            else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise)
+                                residual = (r_raim + A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp;
+                            else
+                                throw std::runtime_error("unknown value for residual_opt");
+                            // residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
+
+                            #if GNSS_UTILS_TDCP_DEBUG == 1
+                                std::cout << "RAIM excluding row " << row_removed;// << std::endl;
+                                //std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
+                                printf("\tresidual          = %13.10f\n", residual);
+                                //std::cout << "Tdcp: drho          = " << r_raim.transpose() << std::endl;
+                                //std::cout << "Tdcp: A             = \n" << A_raim << std::endl;
+                                //std::cout << "Tdcp: H             = \n" << A_raim.transpose() * A_raim << std::endl;
+                                //printf("Tdcp: dT            = %8.3f secs\n", d_raim(3));
+                            #endif
+
+                            // store if best residual
+                            if (residual < best_residual)
+                            {
+                                worst_sat_row = row_removed;
+                                best_residual = residual;
+                                best_d        = d_raim;
+                            }
+                        }
+                        // restore initial A and r
+                        A_raim.row(row_removed) = A.row(row_removed);
+                        r_raim(row_removed)     = r(row_removed);
+                    }
+                }
+                // METHOD B: using max residual in Mahalanobis distance
+                else
                 {
-                    // skip already discarded rows
-                    if (raim_discarded_rows.count(row_removed) != 0)
-                        continue;
+                    // find index of max residual
+                    (r + A * delta_d).cwiseAbs2().maxCoeff(&worst_sat_row);
 
                     // remove row
-                    A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose();
-                    r_raim(row_removed)     = 0;  // not necessary
+                    A_raim.row(worst_sat_row) = Eigen::Vector4d::Zero().transpose();
+                    r_raim(worst_sat_row)     = 0;
 
                     // solve
                     Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
@@ -435,43 +502,33 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
                     {
                         // residual
                         if (tdcp_params.residual_opt == 0) // RMSE
-                            residual = sqrt((r_raim - A_raim * delta_d_raim).squaredNorm() / (A_raim.rows() - raim_discarded_rows.size() - 1)) / sqrt(var_tdcp);
+                            residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / (A_raim.rows() - raim_discarded_rows.size() - 1)) / sqrt(var_tdcp);
                         else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise)
-                            residual = (r_raim - A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp;
+                            residual = (r_raim + A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp;
                         else
                             throw std::runtime_error("unknown value for residual_opt");
                         // residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
 
                         #if GNSS_UTILS_TDCP_DEBUG == 1
-                            std::cout << "RAIM excluding row " << row_removed << std::endl;
-                            std::cout << "Displacement vector =" << d_raim.head<3>().transpose() << std::endl;
-                            std::cout << "Displacement update =" << delta_d_raim.head<3>().transpose() << std::endl;
-                            printf("Ref distance      = %7.3f m\n", d_0.norm());
-                            printf("Computed distance = %7.3f m\n", d_raim.head<3>().norm());
-                            printf("Tdcp: residual      = %13.10f\n", residual);
-                            std::cout << "Tdcp: drho          = " << r_raim.transpose() << std::endl;
-                            std::cout << "Tdcp: A             = \n" << A_raim << std::endl;
-                            std::cout << "Tdcp: H             = \n" << A_raim.transpose() * A_raim << std::endl;
-                            printf("Tdcp: dT            = %8.3f secs\n", d_raim(3));
+                            std::cout << "RAIM excluding row " << worst_sat_row;// << std::endl;
+                            //std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
+                            printf("\tresidual          = %13.10f\n", residual);
+                            //std::cout << "Tdcp: drho          = " << r_raim.transpose() << std::endl;
+                            //std::cout << "Tdcp: A             = \n" << A_raim << std::endl;
+                            //std::cout << "Tdcp: H             = \n" << A_raim.transpose() * A_raim << std::endl;
+                            //printf("Tdcp: dT            = %8.3f secs\n", d_raim(3));
                         #endif
 
-                        // store if best residual
-                        if (residual < best_residual)
-                        {
-                            worst_sat_row = row_removed;
-                            best_residual = residual;
-                            best_d        = d_raim;
-                        }
+                        // store as best residual
+                        best_residual = residual;
+                        best_d        = d_raim;
                     }
-                    // restore initial A and r
-                    A_raim.row(row_removed) = A.row(row_removed);
-                    r_raim(row_removed)     = r(row_removed);
                 }
 
                 // No successful RAIM solution
                 if (worst_sat_row == -1)
                 {
-                    printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
+                    printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!\n");
                     output.msg = "NaN values in NLS after RAIM";
                     output.success = false;
                     return output;
@@ -482,15 +539,16 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
 
                 // remove row (set zero)
                 A.row(worst_sat_row) = Eigen::Vector4d::Zero().transpose();
-                r(worst_sat_row)     = 0;  // not necessary
+                r(worst_sat_row)     = 0;
 
                 // Choose de best RAIM solution
                 d     = best_d;
                 cov_d = (A.transpose() * A).inverse();
+                residual = best_residual;
             }
 
             #if GNSS_UTILS_TDCP_DEBUG == 1
-                std::cout << "Tdcp After RAIM " << std::endl;
+                std::cout << "//////////////// Best solution after RAIM ////////////////" << std::endl;
                 std::cout << "\tExcluded rows : ";
                 for (auto excl_row : raim_discarded_rows)
                     std::cout << excl_row << " ";
@@ -506,30 +564,27 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
         }
 
         #if GNSS_UTILS_TDCP_DEBUG == 1
-        std::cout << "Tdcp iteration " << j << std::endl;
-        std::cout << "\tExcluded rows : ";
-        for (auto excl_row : raim_discarded_rows)
-            std::cout << excl_row << " ";
-        std::cout << std::endl;
-        std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
-        std::cout << "\tSolution      = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
-        std::cout << "\tClock offset  = " << d(3) << std::endl;
-        std::cout << "\tResidual      = " << residual << std::endl;
-        std::cout << "\tA             = \n" << A << std::endl;
-        std::cout << "\tH             = \n" << A.transpose() * A << std::endl;
-        std::cout << "\tcov_d         = \n" << cov_d << std::endl;
+            std::cout << "\n//////////////// Final solution at iteration " << j << "////////////////\n";
+            std::cout << "\tExcluded rows : ";
+            for (auto excl_row : raim_discarded_rows)
+                std::cout << excl_row << " ";
+            std::cout << std::endl;
+            std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
+            std::cout << "\tSolution      = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
+            std::cout << "\tClock offset  = " << d(3) << std::endl;
+            std::cout << "\tResidual      = " << residual << std::endl;
+            std::cout << "\tA             = \n" << A << std::endl;
+            std::cout << "\tH             = \n" << A.transpose() * A << std::endl;
+            std::cout << "\tcov_d         = \n" << cov_d << std::endl;
         #endif
     }
 
     // Computing error on measurement space
-    std::cout << "r (r-A*delta_d) = " << (r - A * (d-d_0)).transpose() << std::endl;
     r.setZero();
     for (int row = 0; row < r.size(); row++)
         if (raim_discarded_rows.count(row) == 0)
             r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
 
-    std::cout << "r (reevaluated) = " << r.transpose() << std::endl;
-
     // residual
     if (tdcp_params.residual_opt == 0) // RMSE
         residual = sqrt(r.squaredNorm() / (r.rows() - raim_discarded_rows.size())) / sqrt(var_tdcp);
@@ -542,7 +597,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
     // check residual condition
     if (residual > tdcp_params.max_residual)
     {
-        printf("Tdcp: Didn't success. Final residual bigger than max_residual.");
+        printf("Tdcp: Didn't success. Final residual=%f bigger than max_residual=%f.\n", residual, tdcp_params.max_residual);
         output.msg = "Residual bigger than max_residual";
         output.success = false;
     }
diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp
index f022cd9340b27d731a1858b122464cd3ab977388..d8158e0108411bace868743dd7e3d98978c9d881 100644
--- a/test/gtest_tdcp.cpp
+++ b/test/gtest_tdcp.cpp
@@ -7,6 +7,11 @@
 using namespace GnssUtils;
 using namespace Eigen;
 
+int N_sats = 20;
+int N_tests = 100;
+double distortion1 = -30;
+double distortion2 = -20;
+
 Vector3d computeRandomReceiverLatLonAlt()
 {
     Vector3d receiver_LLA = Vector3d::Random(); // in [-1, 1]
@@ -18,11 +23,12 @@ Vector3d computeRandomReceiverLatLonAlt()
     return receiver_LLA;
 }
 
-void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
-                                   Vector3d& sat_ENU,
-                                   Vector3d& sat_ECEF,
-                                   Vector2d& sat_azel,
-                                   double& range)
+void computeVisibleSatellite(const Vector3d& receiver_latlonalt,
+                             const int& n_sat,
+                             Vector3d& sat_ENU,
+                             Vector3d& sat_ECEF,
+                             Vector2d& sat_azel,
+                             double& range)
 {
     Vector3d t_ECEF_ENU, t_ENU_ECEF;
     Matrix3d R_ENU_ECEF;
@@ -31,10 +37,21 @@ void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
 
     computeEnuEcefFromLatLonAlt(receiver_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
 
+    // equidistributed azimuth at elevation 45º
+    if (n_sat < 6)
+    {
+        sat_azel(0) = -M_PI + n_sat * 2*M_PI/6 + 0.1;   // in [-pi, pi]
+        sat_azel(1) = M_PI / 4;                 // in [0, pi/2]
+    }
     // random elevation and azimuth
-    sat_azel = Vector2d::Random();               // in [-1, 1]
-    sat_azel(0) *= M_PI;                                // in [-pi, pi]
-    sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2;     // in [0, pi/2]
+    else
+    {
+        sat_azel = Vector2d::Random();               // in [-1, 1]
+        sat_azel(0) *= M_PI;                                // in [-pi, pi]
+        sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2;     // in [0, pi/2]
+    }
+
+    // random range
     range = VectorXd::Random(1)(0) * 5e2 + 1e3;  // in [500, 1500]
 
     // ENU
@@ -51,16 +68,16 @@ void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
 
 TEST(Tdcp, Tdcp)
 {
-    int N_sats = 20;
-
     TdcpBatchParams tdcp_params;
-    tdcp_params.max_iterations = 10;
+    tdcp_params.max_iterations = 5;
     tdcp_params.min_common_sats = 6;
     tdcp_params.raim_n = 0;
-    tdcp_params.max_residual = 0;
+    tdcp_params.residual_opt = 0;
+    tdcp_params.max_residual = 1e20;
     tdcp_params.relinearize_jacobian = true;
     tdcp_params.tdcp.multi_freq = false;
-    tdcp_params.residual_opt = 0;
+    tdcp_params.tdcp.sigma_atm = 1;
+    tdcp_params.tdcp.sigma_carrier = 1;
 
     Vector3d sat_ENU, sat_ECEF;
     Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
@@ -87,7 +104,7 @@ TEST(Tdcp, Tdcp)
     snapshot_k->getObservations()->addObservation(obs_k);
 
     // Random receiver position
-    for (auto i = 0; i<100; i++)
+    for (auto i = 0; i<N_tests; i++)
     {
         // clear satellites and ranges
         snapshot_r->getSatellites().clear();
@@ -109,7 +126,7 @@ TEST(Tdcp, Tdcp)
         d_gt.head<3>() = d_ECEF;
         d_gt(3) = (clock_k - clock_r) * CLIGHT;
 
-        std::cout << "Iteration " << i << ":\n";
+        std::cout << "Test " << i << ":\n";
         std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
         std::cout << "\tclock_r " << clock_r << ":\n";
         std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
@@ -124,8 +141,8 @@ TEST(Tdcp, Tdcp)
         {
             common_sats.insert(j);
 
-            // Satellite r (random)
-            computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range);
+            // Satellite r
+            computeVisibleSatellite(x_r_LLA, j, sat_ENU, sat_ECEF, azel, range);
             ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
 
             Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
@@ -143,12 +160,12 @@ TEST(Tdcp, Tdcp)
 
             snapshot_r->getRanges().emplace(j, range_r);
 
-            std::cout << "\tsat: " << j << "\n";
-            std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
-            std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
+            //std::cout << "\tsat: " << j << "\n";
+            //std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
+            //std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
 
             // Satellite k (random)
-            computeRandomVisibleSatellite(x_k_LLA, sat_ENU, sat_ECEF, azel, range);
+            computeVisibleSatellite(x_k_LLA, j, sat_ENU, sat_ECEF, azel, range);
             ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
 
             Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
@@ -166,8 +183,8 @@ TEST(Tdcp, Tdcp)
 
             snapshot_k->getRanges().emplace(j, range_k);
 
-            std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
-            std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
+            //std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
+            //std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
         }
 
         // TDCP
@@ -179,25 +196,28 @@ TEST(Tdcp, Tdcp)
                              tdcp_params);
 
         // CHECKS
-        std::cout << "CHECKS iteration " << i << std::endl;
+        std::cout << "CHECKS Test " << i << std::endl;
         ASSERT_TRUE(tdcp_out.success);
-        ASSERT_LE(tdcp_out.residual, 1e-9);
-        ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6);
+        ASSERT_LE(tdcp_out.residual, 1e-3);
+        ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
     }
 }
 
-TEST(Tdcp, Tdcp_raim)
+TEST(Tdcp, Tdcp_raim_residual_rmse)
 {
     int N_sats = 20;
 
     TdcpBatchParams tdcp_params;
-    tdcp_params.max_iterations = 10;
+    tdcp_params.max_iterations = 5;
     tdcp_params.min_common_sats = 6;
     tdcp_params.raim_n = 2;
-    tdcp_params.max_residual = 0;
     tdcp_params.relinearize_jacobian = true;
+    tdcp_params.residual_opt = 0; // Normalized RMSE
+    tdcp_params.max_residual = 0.1; // low threshold to detect outliers...
     tdcp_params.tdcp.multi_freq = false;
-    tdcp_params.residual_opt = 0;
+    tdcp_params.tdcp.sigma_atm = 1;
+    tdcp_params.tdcp.sigma_carrier = 1;
+
 
     Vector3d sat_ENU, sat_ECEF;
     Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
@@ -224,7 +244,7 @@ TEST(Tdcp, Tdcp_raim)
     snapshot_k->getObservations()->addObservation(obs_k);
 
     // Random receiver position
-    for (auto i = 0; i<100; i++)
+    for (auto i = 0; i<N_tests; i++)
     {
         // clear satellites and ranges
         snapshot_r->getSatellites().clear();
@@ -246,12 +266,17 @@ TEST(Tdcp, Tdcp_raim)
         d_gt.head<3>() = d_ECEF;
         d_gt(3) = (clock_k - clock_r) * CLIGHT;
 
-        std::cout << "Iteration " << i << ":\n";
+        // distorted sats
+        int wrong_sat1 = i % N_sats;
+        int wrong_sat2 = (i + N_sats / 2) % N_sats;
+
+        std::cout << "Test " << i << ":\n";
         std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
         std::cout << "\tclock_r " << clock_r << ":\n";
         std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
         std::cout << "\tclock_k " << clock_k << ":\n";
         std::cout << "\td_gt " << d_gt.transpose() << ":\n";
+        std::cout << "\tdistorted sats: " << wrong_sat1 << ", " << wrong_sat2 << std::endl;
 
         std::set<int> common_sats;
         std::set<int> raim_discarded_sats;
@@ -262,7 +287,7 @@ TEST(Tdcp, Tdcp_raim)
             common_sats.insert(j);
 
             // Satellite r (random)
-            computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range);
+            computeVisibleSatellite(x_r_LLA, j, sat_ENU, sat_ECEF, azel, range);
             ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
 
             Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
@@ -280,12 +305,12 @@ TEST(Tdcp, Tdcp_raim)
 
             snapshot_r->getRanges().emplace(j, range_r);
 
-            std::cout << "\tsat: " << j << "\n";
-            std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
-            std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
+            //std::cout << "\tsat: " << j << "\n";
+            //std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
+            //std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
 
             // Satellite k (random)
-            computeRandomVisibleSatellite(x_k_LLA, sat_ENU, sat_ECEF, azel, range);
+            computeVisibleSatellite(x_k_LLA, j, sat_ENU, sat_ECEF, azel, range);
             ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
 
             Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
@@ -303,18 +328,170 @@ TEST(Tdcp, Tdcp_raim)
 
             snapshot_k->getRanges().emplace(j, range_k);
 
-            std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
-            std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
+            //std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
+            //std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
         }
 
         // Distort 2 ranges -> to be detected by RAIM
+        snapshot_k->getRanges().at(wrong_sat1).L_corrected += distortion1;
+        snapshot_k->getRanges().at(wrong_sat1).L += distortion1;
+        snapshot_r->getRanges().at(wrong_sat2).L_corrected += distortion2;
+        snapshot_r->getRanges().at(wrong_sat2).L += distortion2;
+
+
+        // TDCP
+        auto tdcp_out = Tdcp(snapshot_r,
+                             snapshot_k,
+                             x_r_ECEF,
+                             common_sats,
+                             Vector4d::Zero(),
+                             tdcp_params);
+
+        // CHECKS
+        std::cout << "CHECKS iteration " << i << std::endl;
+        ASSERT_EQ(tdcp_out.raim_discarded_sats.size(), 2);
+        ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat1));
+        ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat2));
+        ASSERT_TRUE(tdcp_out.success);
+        ASSERT_LE(tdcp_out.residual, 1e-3);
+        ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
+    }
+}
+
+TEST(Tdcp, Tdcp_raim_residual_max_mah)
+{
+    int N_sats = 20;
+
+    TdcpBatchParams tdcp_params;
+    tdcp_params.max_iterations = 5;
+    tdcp_params.min_common_sats = 6;
+    tdcp_params.raim_n = 2;
+    tdcp_params.relinearize_jacobian = true;
+    tdcp_params.residual_opt = 1; // Max residual in Mahalanobis distance
+    tdcp_params.max_residual = 3.84; // 95% of confidence
+    tdcp_params.tdcp.multi_freq = false;
+    tdcp_params.tdcp.sigma_atm = 1;
+    tdcp_params.tdcp.sigma_carrier = 1;
+
+
+    Vector3d sat_ENU, sat_ECEF;
+    Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
+    Matrix3d R_ENU_ECEF;
+    Vector2d azel, azel2;
+    Vector4d d, d_gt;
+    Matrix4d cov_d;
+    double range, residual;
+
+    // Snapshots
+    auto snapshot_r = std::make_shared<Snapshot>();
+    auto snapshot_k = std::make_shared<Snapshot>();
+
+    // Observations (just needed for the GPST)
+    snapshot_r->setObservations(std::make_shared<Observations>());
+    snapshot_k->setObservations(std::make_shared<Observations>());
+    obsd_t obs_r;
+    obs_r.time.sec = 0;
+    obs_r.time.time = 0;
+    snapshot_r->getObservations()->addObservation(obs_r);
+    obsd_t obs_k;
+    obs_k.time.sec = 0;
+    obs_k.time.time = 1;
+    snapshot_k->getObservations()->addObservation(obs_k);
+
+    // Random receiver position
+    for (auto i = 0; i<N_tests; i++)
+    {
+        // clear satellites and ranges
+        snapshot_r->getSatellites().clear();
+        snapshot_k->getSatellites().clear();
+        snapshot_r->getRanges().clear();
+        snapshot_k->getRanges().clear();
+
+        // random setup
+        x_r_LLA = computeRandomReceiverLatLonAlt();
+        x_r_ECEF = latLonAltToEcef(x_r_LLA);
+        d_ECEF = Vector3d::Random() * 10;
+        x_k_ECEF = x_r_ECEF + d_ECEF;
+        x_k_LLA = ecefToLatLonAlt(x_k_ECEF);
+
+        double clock_r = Vector2d::Random()(0) * 1e-6;
+        double clock_k = Vector2d::Random()(0) * 1e-6;
+
+        // groundtruth
+        d_gt.head<3>() = d_ECEF;
+        d_gt(3) = (clock_k - clock_r) * CLIGHT;
+
+        // distorted sats
         int wrong_sat1 = i % N_sats;
-        snapshot_k->getRanges().at(wrong_sat1).L_corrected += 20;
-        snapshot_k->getRanges().at(wrong_sat1).L += 20;
         int wrong_sat2 = (i + N_sats / 2) % N_sats;
-        snapshot_r->getRanges().at(wrong_sat2).L_corrected -= 10;
-        snapshot_r->getRanges().at(wrong_sat2).L -= 10;
 
+        std::cout << "\nTest " << i << ":\n";
+        std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
+        std::cout << "\tclock_r " << clock_r << ":\n";
+        std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
+        std::cout << "\tclock_k " << clock_k << ":\n";
+        std::cout << "\td_gt " << d_gt.transpose() << ":\n";
+        std::cout << "\tdistorted sats: " << wrong_sat1 << ", " << wrong_sat2 << std::endl;
+
+        std::set<int> common_sats;
+        std::set<int> raim_discarded_sats;
+
+        // random visible satellites
+        for (auto j = 0; j<N_sats; j++)
+        {
+            common_sats.insert(j);
+
+            // Satellite r (random)
+            computeVisibleSatellite(x_r_LLA, j, sat_ENU, sat_ECEF, azel, range);
+            ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
+
+            Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+            snapshot_r->getSatellites().emplace(j, sat_r);
+
+            // Range r
+            Range range_r;
+            range_r.sat = j;
+            range_r.L = range + CLIGHT * clock_r;
+            range_r.L_corrected = range_r.L;
+            range_r.L_var = 1;
+            range_r.L2 = range_r.L;
+            range_r.L2_corrected = range_r.L;
+            range_r.L2_var = range_r.L_var;
+
+            snapshot_r->getRanges().emplace(j, range_r);
+
+            //std::cout << "\tsat: " << j << "\n";
+            //std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
+            //std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
+
+            // Satellite k (random)
+            computeVisibleSatellite(x_k_LLA, j, sat_ENU, sat_ECEF, azel, range);
+            ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
+
+            Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+            snapshot_k->getSatellites().emplace(j, sat_k);
+
+            // Range k
+            Range range_k;
+            range_k.sat = j;
+            range_k.L = range + CLIGHT * clock_k;
+            range_k.L_corrected = range_k.L;
+            range_k.L_var = 1;
+            range_k.L2 = range_k.L;
+            range_k.L2_corrected = range_k.L;
+            range_k.L2_var = range_k.L_var;
+
+            snapshot_k->getRanges().emplace(j, range_k);
+
+            //std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
+            //std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
+        }
+
+        // Distort 2 ranges -> to be detected by RAIM
+        snapshot_k->getRanges().at(wrong_sat1).L_corrected += distortion1;
+        snapshot_k->getRanges().at(wrong_sat1).L += distortion1;
+        snapshot_r->getRanges().at(wrong_sat2).L_corrected += distortion2;
+        snapshot_r->getRanges().at(wrong_sat2).L += distortion2;
 
         // TDCP
         auto tdcp_out = Tdcp(snapshot_r,
@@ -325,13 +502,13 @@ TEST(Tdcp, Tdcp_raim)
                              tdcp_params);
 
         // CHECKS
-        std::cout << "CHECKS iteration " << i << std::endl;
+        std::cout << "Checks iteration " << i << std::endl;
         ASSERT_EQ(tdcp_out.raim_discarded_sats.size(), 2);
         ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat1));
         ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat2));
         ASSERT_TRUE(tdcp_out.success);
-        ASSERT_LE(tdcp_out.residual, 1e-9);
-        ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6);
+        ASSERT_LE(tdcp_out.residual, 1e-3);
+        ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
     }
 }