Update tdcp.cpp

src/tdcp.cpp

@@ -453,20 +453,19 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
                         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;
+                        Eigen::Vector4d d_raim  = d_0 - (A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_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);
+                                residual = sqrt((r_raim + A_raim * 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 * 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);
+                            // residual = (r_raim + A_raim * d_raim).norm() / (A_raim.rows() - 1);
 
                             #if GNSS_UTILS_TDCP_DEBUG == 1
                                 std::cout << "RAIM excluding row " << row_removed;// << std::endl;
@@ -502,20 +501,19 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
                     r_raim(worst_sat_row)     = 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;
+                    Eigen::Vector4d d_raim    = d_0 - (A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_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);
+                            residual = sqrt((r_raim + A_raim * 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 * 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);
+                        // residual = (r_raim + A_raim * d_raim).norm() / (A_raim.rows() - 1);
 
                         #if GNSS_UTILS_TDCP_DEBUG == 1
                             std::cout << "RAIM excluding row " << worst_sat_row;// << std::endl;