diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h index cf3d2d0860b08c8eb7ca386b073002ebb1a8d58f..85ceff77b556554577244d292e41da5db2468e83 100644 --- a/include/gnss_utils/gnss_utils.h +++ b/include/gnss_utils/gnss_utils.h @@ -110,7 +110,7 @@ struct TdcpOptions int min_common_sats; int raim_n; bool validate_residual; - double max_residual; // max allowed residual to be considered good solution. RAIM applied if enabled in this case. + double max_residual_ci; // Confidence interval that defines the max residual to be considered good solution. RAIM applied if enabled in this case. bool relinearize_jacobian; int max_iterations; int residual_opt; // 0: Normalized RMS of residual vector. 1: Max residual in Mahalanobis squared distance diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h index 818fe3fb735047f0c2a7d373fb59a121ef7371ed..28cef3d6bac068e8638ab2278a10a842ce165f6d 100644 --- a/include/gnss_utils/tdcp.h +++ b/include/gnss_utils/tdcp.h @@ -4,6 +4,7 @@ #define GNSS_UTILS_TDCP_DEBUG 0 #include "gnss_utils/gnss_utils.h" +#include "gnss_utils/utils/chisquare_ci.h" namespace GnssUtils { @@ -17,7 +18,7 @@ struct TdcpOutput Eigen::Vector4d d = Eigen::Vector4d::Zero(); Eigen::Matrix4d cov_d = Eigen::Matrix4d::Zero(); double dt = 0; - double residual = 0; + double residual_ci = 0; }; TdcpOutput Tdcp(SnapshotPtr snapshot_r, diff --git a/include/gnss_utils/utils/chisquare_ci.h b/include/gnss_utils/utils/chisquare_ci.h new file mode 100644 index 0000000000000000000000000000000000000000..83274b542cb81868f651f3ccf41bca2ad445caa4 --- /dev/null +++ b/include/gnss_utils/utils/chisquare_ci.h @@ -0,0 +1,58 @@ +/* + * chisquare_ci.h + * + * Created on: Jul 28, 2021 + * Author: joanvallve + */ + +#ifndef INCLUDE_GNSS_UTILS_UTILS_CHISQUARE_CI_H_ +#define INCLUDE_GNSS_UTILS_UTILS_CHISQUARE_CI_H_ + +#include "chisquare_ci_maps.h" + +namespace GnssUtils +{ + +double chisq2ci(double chisq, int dof) +{ + assert(dof > 0); + + if (dof > 30) + dof = 30; + + if (chisq_2_CI.at(dof).count(chisq) == 1) + return chisq_2_CI.at(dof).at(chisq); + + auto upper = chisq_2_CI.at(dof).upper_bound(chisq); + + if (upper == chisq_2_CI.at(dof).begin()) + return upper->second; + + auto lower = std::prev(upper); + + return lower->second + (chisq - lower->first) / (upper->first - lower->first) * (upper->second - lower->second); +}; + +double ci2chisq(double ci, int dof) +{ + assert(dof > 0); + + if (dof > 30) + dof = 30; + + if (CI_2_chisq.at(dof).count(ci) == 1) + return CI_2_chisq.at(dof).at(ci); + + auto upper = CI_2_chisq.at(dof).upper_bound(ci); + + if (upper == CI_2_chisq.at(dof).begin()) + return upper->second; + + auto lower = std::prev(upper); + + return lower->second + (ci - lower->first) / (upper->first - lower->first) * (upper->second - lower->second); +}; + +} // namespace GnssUtils + +#endif /* INCLUDE_GNSS_UTILS_UTILS_CHISQUARE_CI_H_ */ diff --git a/include/gnss_utils/utils/chisquare_ci_maps.h b/include/gnss_utils/utils/chisquare_ci_maps.h new file mode 100644 index 0000000000000000000000000000000000000000..193d03d01be7df8bc238e419eb9f6d674b8c8d65 --- /dev/null +++ b/include/gnss_utils/utils/chisquare_ci_maps.h @@ -0,0 +1,1397 @@ +#ifndef INCLUDE_GNSS_UTILS_UTILS_CHISQUARE_CI_MAPS_H_ +#define INCLUDE_GNSS_UTILS_UTILS_CHISQUARE_CI_MAPS_H_ + +#include <map> + +namespace GnssUtils +{ + +static std::map<int, std::map<double, double>> CI_2_chisq = { + // 1 dof + {1, { + {0.10, 0.0157908}, + {0.15, 0.0357658}, + {0.20, 0.0641848}, + {0.25, 0.101531}, + {0.30, 0.148472}, + {0.35, 0.2059}, + {0.40, 0.274996}, + {0.45, 0.357317}, + {0.50, 0.454936}, + {0.55, 0.570652}, + {0.60, 0.708326}, + {0.65, 0.873457}, + {0.70, 1.07419}, + {0.75, 1.3233}, + {0.80, 1.64237}, + {0.85, 2.07225}, + {0.90, 2.70554}, + {0.95, 3.84146}, + {0.99, 6.6349}, + {1.00, 1e+12}, + }}, + // 2 dof + {2, { + {0.10, 0.210721}, + {0.15, 0.325038}, + {0.20, 0.446287}, + {0.25, 0.575364}, + {0.30, 0.71335}, + {0.35, 0.861566}, + {0.40, 1.02165}, + {0.45, 1.19567}, + {0.50, 1.38629}, + {0.55, 1.59702}, + {0.60, 1.83258}, + {0.65, 2.09964}, + {0.70, 2.40795}, + {0.75, 2.77259}, + {0.80, 3.21888}, + {0.85, 3.79424}, + {0.90, 4.60517}, + {0.95, 5.99146}, + {0.99, 9.21034}, + {1.00, 1e+12}, + }}, + // 3 dof + {3, { + {0.10, 0.584374}, + {0.15, 0.797771}, + {0.20, 1.00517}, + {0.25, 1.21253}, + {0.30, 1.42365}, + {0.35, 1.64158}, + {0.40, 1.86917}, + {0.45, 2.10947}, + {0.50, 2.36597}, + {0.55, 2.64301}, + {0.60, 2.94617}, + {0.65, 3.28311}, + {0.70, 3.66487}, + {0.75, 4.10834}, + {0.80, 4.64163}, + {0.85, 5.31705}, + {0.90, 6.25139}, + {0.95, 7.81473}, + {0.99, 11.3449}, + {1.00, 1e+12}, + }}, + // 4 dof + {4, { + {0.10, 1.06362}, + {0.15, 1.36648}, + {0.20, 1.64878}, + {0.25, 1.92256}, + {0.30, 2.1947}, + {0.35, 2.47009}, + {0.40, 2.75284}, + {0.45, 3.04695}, + {0.50, 3.35669}, + {0.55, 3.68713}, + {0.60, 4.04463}, + {0.65, 4.43769}, + {0.70, 4.87843}, + {0.75, 5.38527}, + {0.80, 5.98862}, + {0.85, 6.74488}, + {0.90, 7.77944}, + {0.95, 9.48773}, + {0.99, 13.2767}, + {1.00, 1e+12}, + }}, + // 5 dof + {5, { + {0.10, 1.61031}, + {0.15, 1.99382}, + {0.20, 2.34253}, + {0.25, 2.6746}, + {0.30, 2.99991}, + {0.35, 3.32511}, + {0.40, 3.6555}, + {0.45, 3.99594}, + {0.50, 4.35146}, + {0.55, 4.72776}, + {0.60, 5.13187}, + {0.65, 5.57307}, + {0.70, 6.06443}, + {0.75, 6.62568}, + {0.80, 7.28928}, + {0.85, 8.1152}, + {0.90, 9.23636}, + {0.95, 11.0705}, + {0.99, 15.0863}, + {1.00, 1e+12}, + }}, + // 6 dof + {6, { + {0.10, 2.20413}, + {0.15, 2.66127}, + {0.20, 3.07009}, + {0.25, 3.4546}, + {0.30, 3.82755}, + {0.35, 4.19727}, + {0.40, 4.57015}, + {0.45, 4.95188}, + {0.50, 5.34812}, + {0.55, 5.7652}, + {0.60, 6.21076}, + {0.65, 6.69476}, + {0.70, 7.23114}, + {0.75, 7.8408}, + {0.80, 8.55806}, + {0.85, 9.4461}, + {0.90, 10.6446}, + {0.95, 12.5916}, + {0.99, 16.8119}, + {1.00, 1e+12}, + }}, + // 7 dof + {7, { + {0.10, 2.83311}, + {0.15, 3.35828}, + {0.20, 3.82232}, + {0.25, 4.25485}, + {0.30, 4.67133}, + {0.35, 5.08165}, + {0.40, 5.49323}, + {0.45, 5.91252}, + {0.50, 6.34581}, + {0.55, 6.79997}, + {0.60, 7.28321}, + {0.65, 7.80612}, + {0.70, 8.38343}, + {0.75, 9.03715}, + {0.80, 9.80325}, + {0.85, 10.7479}, + {0.90, 12.017}, + {0.95, 14.0671}, + {0.99, 18.4753}, + {1.00, 1e+12}, + }}, + // 8 dof + {8, { + {0.10, 3.48954}, + {0.15, 4.0782}, + {0.20, 4.59357}, + {0.25, 5.07064}, + {0.30, 5.52742}, + {0.35, 5.97529}, + {0.40, 6.42265}, + {0.45, 6.87663}, + {0.50, 7.34412}, + {0.55, 7.83251}, + {0.60, 8.35053}, + {0.65, 8.90936}, + {0.70, 9.52446}, + {0.75, 10.2189}, + {0.80, 11.0301}, + {0.85, 12.0271}, + {0.90, 13.3616}, + {0.95, 15.5073}, + {0.99, 20.0902}, + {1.00, 1e+12}, + }}, + // 9 dof + {9, { + {0.10, 4.16816}, + {0.15, 4.81652}, + {0.20, 5.38005}, + {0.25, 5.89883}, + {0.30, 6.39331}, + {0.35, 6.87627}, + {0.40, 7.35703}, + {0.45, 7.84342}, + {0.50, 8.34283}, + {0.55, 8.86317}, + {0.60, 9.41364}, + {0.65, 10.006}, + {0.70, 10.6564}, + {0.75, 11.3888}, + {0.80, 12.2421}, + {0.85, 13.288}, + {0.90, 14.6837}, + {0.95, 16.919}, + {0.99, 21.666}, + {1.00, 1e+12}, + }}, + // 10 dof + {10, { + {0.10, 4.86518}, + {0.15, 5.57006}, + {0.20, 6.17908}, + {0.25, 6.7372}, + {0.30, 7.26722}, + {0.35, 7.78324}, + {0.40, 8.29547}, + {0.45, 8.81235}, + {0.50, 9.34182}, + {0.55, 9.89222}, + {0.60, 10.4732}, + {0.65, 11.0971}, + {0.70, 11.7807}, + {0.75, 12.5489}, + {0.80, 13.442}, + {0.85, 14.5339}, + {0.90, 15.9872}, + {0.95, 18.307}, + {0.99, 23.2093}, + {1.00, 1e+12}, + }}, + // 11 dof + {11, { + {0.10, 5.57778}, + {0.15, 6.33643}, + {0.20, 6.98867}, + {0.25, 7.58414}, + {0.30, 8.14787}, + {0.35, 8.69524}, + {0.40, 9.23729}, + {0.45, 9.78306}, + {0.50, 10.341}, + {0.55, 10.9199}, + {0.60, 11.5298}, + {0.65, 12.1836}, + {0.70, 12.8987}, + {0.75, 13.7007}, + {0.80, 14.6314}, + {0.85, 15.7671}, + {0.90, 17.275}, + {0.95, 19.6751}, + {0.99, 24.725}, + {1.00, 1e+12}, + }}, + // 12 dof + {12, { + {0.10, 6.3038}, + {0.15, 7.11384}, + {0.20, 7.80733}, + {0.25, 8.43842}, + {0.30, 9.03428}, + {0.35, 9.61152}, + {0.40, 10.182}, + {0.45, 10.7553}, + {0.50, 11.3403}, + {0.55, 11.9463}, + {0.60, 12.5838}, + {0.65, 13.2661}, + {0.70, 14.0111}, + {0.75, 14.8454}, + {0.80, 15.812}, + {0.85, 16.9893}, + {0.90, 18.5493}, + {0.95, 21.0261}, + {0.99, 26.217}, + {1.00, 1e+12}, + }}, + // 13 dof + {13, { + {0.10, 7.0415}, + {0.15, 7.90084}, + {0.20, 8.63386}, + {0.25, 9.29907}, + {0.30, 9.92568}, + {0.35, 10.5315}, + {0.40, 11.1291}, + {0.45, 11.7288}, + {0.50, 12.3398}, + {0.55, 12.9717}, + {0.60, 13.6356}, + {0.65, 14.3451}, + {0.70, 15.1187}, + {0.75, 15.9839}, + {0.80, 16.9848}, + {0.85, 18.202}, + {0.90, 19.8119}, + {0.95, 22.362}, + {0.99, 27.6882}, + {1.00, 1e+12}, + }}, + // 14 dof + {14, { + {0.10, 7.78953}, + {0.15, 8.6963}, + {0.20, 9.46733}, + {0.25, 10.1653}, + {0.30, 10.8215}, + {0.35, 11.4548}, + {0.40, 12.0785}, + {0.45, 12.7034}, + {0.50, 13.3393}, + {0.55, 13.9961}, + {0.60, 14.6853}, + {0.65, 15.4209}, + {0.70, 16.2221}, + {0.75, 17.1169}, + {0.80, 18.1508}, + {0.85, 19.4062}, + {0.90, 21.0641}, + {0.95, 23.6848}, + {0.99, 29.1412}, + {1.00, 1e+12}, + }}, + // 15 dof + {15, { + {0.10, 8.54676}, + {0.15, 9.49928}, + {0.20, 10.307}, + {0.25, 11.0365}, + {0.30, 11.7212}, + {0.35, 12.3809}, + {0.40, 13.0297}, + {0.45, 13.679}, + {0.50, 14.3389}, + {0.55, 15.0197}, + {0.60, 15.7332}, + {0.65, 16.494}, + {0.70, 17.3217}, + {0.75, 18.2451}, + {0.80, 19.3107}, + {0.85, 20.603}, + {0.90, 22.3071}, + {0.95, 24.9958}, + {0.99, 30.5779}, + {1.00, 1e+12}, + }}, + // 16 dof + {16, { + {0.10, 9.31224}, + {0.15, 10.309}, + {0.20, 11.1521}, + {0.25, 11.9122}, + {0.30, 12.6243}, + {0.35, 13.3096}, + {0.40, 13.9827}, + {0.45, 14.6555}, + {0.50, 15.3385}, + {0.55, 16.0425}, + {0.60, 16.7795}, + {0.65, 17.5646}, + {0.70, 18.4179}, + {0.75, 19.3689}, + {0.80, 20.4651}, + {0.85, 21.7931}, + {0.90, 23.5418}, + {0.95, 26.2962}, + {0.99, 31.9999}, + {1.00, 1e+12}, + }}, + // 17 dof + {17, { + {0.10, 10.0852}, + {0.15, 11.1249}, + {0.20, 12.0023}, + {0.25, 12.7919}, + {0.30, 13.5307}, + {0.35, 14.2407}, + {0.40, 14.9373}, + {0.45, 15.6328}, + {0.50, 16.3382}, + {0.55, 17.0646}, + {0.60, 17.8244}, + {0.65, 18.633}, + {0.70, 19.511}, + {0.75, 20.4887}, + {0.80, 21.6146}, + {0.85, 22.977}, + {0.90, 24.769}, + {0.95, 27.5871}, + {0.99, 33.4087}, + {1.00, 1e+12}, + }}, + // 18 dof + {18, { + {0.10, 10.8649}, + {0.15, 11.9463}, + {0.20, 12.857}, + {0.25, 13.6753}, + {0.30, 14.4399}, + {0.35, 15.1738}, + {0.40, 15.8932}, + {0.45, 16.6108}, + {0.50, 17.3379}, + {0.55, 18.086}, + {0.60, 18.8679}, + {0.65, 19.6993}, + {0.70, 20.6014}, + {0.75, 21.6049}, + {0.80, 22.7595}, + {0.85, 24.1555}, + {0.90, 25.9894}, + {0.95, 28.8693}, + {0.99, 34.8053}, + {1.00, 1e+12}, + }}, + // 19 dof + {19, { + {0.10, 11.6509}, + {0.15, 12.7727}, + {0.20, 13.7158}, + {0.25, 14.562}, + {0.30, 15.3517}, + {0.35, 16.1089}, + {0.40, 16.8504}, + {0.45, 17.5894}, + {0.50, 18.3377}, + {0.55, 19.1069}, + {0.60, 19.9102}, + {0.65, 20.7638}, + {0.70, 21.6891}, + {0.75, 22.7178}, + {0.80, 23.9004}, + {0.85, 25.3289}, + {0.90, 27.2036}, + {0.95, 30.1435}, + {0.99, 36.1909}, + {1.00, 1e+12}, + }}, + // 20 dof + {20, { + {0.10, 12.4426}, + {0.15, 13.6039}, + {0.20, 14.5784}, + {0.25, 15.4518}, + {0.30, 16.2659}, + {0.35, 17.0458}, + {0.40, 17.8088}, + {0.45, 18.5687}, + {0.50, 19.3374}, + {0.55, 20.1272}, + {0.60, 20.9514}, + {0.65, 21.8265}, + {0.70, 22.7745}, + {0.75, 23.8277}, + {0.80, 25.0375}, + {0.85, 26.4976}, + {0.90, 28.412}, + {0.95, 31.4104}, + {0.99, 37.5662}, + {1.00, 1e+12}, + }}, + // 21 dof + {21, { + {0.10, 13.2396}, + {0.15, 14.4393}, + {0.20, 15.4446}, + {0.25, 16.3444}, + {0.30, 17.1823}, + {0.35, 17.9843}, + {0.40, 18.7683}, + {0.45, 19.5485}, + {0.50, 20.3372}, + {0.55, 21.147}, + {0.60, 21.9915}, + {0.65, 22.8876}, + {0.70, 23.8578}, + {0.75, 24.9348}, + {0.80, 26.1711}, + {0.85, 27.662}, + {0.90, 29.6151}, + {0.95, 32.6706}, + {0.99, 38.9322}, + {1.00, 1e+12}, + }}, + // 22 dof + {22, { + {0.10, 14.0415}, + {0.15, 15.2788}, + {0.20, 16.314}, + {0.25, 17.2396}, + {0.30, 18.1007}, + {0.35, 18.9243}, + {0.40, 19.7288}, + {0.45, 20.5288}, + {0.50, 21.337}, + {0.55, 22.1663}, + {0.60, 23.0307}, + {0.65, 23.9473}, + {0.70, 24.939}, + {0.75, 26.0393}, + {0.80, 27.3015}, + {0.85, 28.8225}, + {0.90, 30.8133}, + {0.95, 33.9244}, + {0.99, 40.2894}, + {1.00, 1e+12}, + }}, + // 23 dof + {23, { + {0.10, 14.848}, + {0.15, 16.1219}, + {0.20, 17.1865}, + {0.25, 18.1373}, + {0.30, 19.0211}, + {0.35, 19.8657}, + {0.40, 20.6902}, + {0.45, 21.5096}, + {0.50, 22.3369}, + {0.55, 23.1852}, + {0.60, 24.0689}, + {0.65, 25.0055}, + {0.70, 26.0184}, + {0.75, 27.1413}, + {0.80, 28.4288}, + {0.85, 29.9792}, + {0.90, 32.0069}, + {0.95, 35.1725}, + {0.99, 41.6384}, + {1.00, 1e+12}, + }}, + // 24 dof + {24, { + {0.10, 15.6587}, + {0.15, 16.9686}, + {0.20, 18.0618}, + {0.25, 19.0373}, + {0.30, 19.9432}, + {0.35, 20.8084}, + {0.40, 21.6525}, + {0.45, 22.4908}, + {0.50, 23.3367}, + {0.55, 24.2037}, + {0.60, 25.1063}, + {0.65, 26.0625}, + {0.70, 27.096}, + {0.75, 28.2412}, + {0.80, 29.5533}, + {0.85, 31.1325}, + {0.90, 33.1962}, + {0.95, 36.415}, + {0.99, 42.9798}, + {1.00, 1e+12}, + }}, + // 25 dof + {25, { + {0.10, 16.4734}, + {0.15, 17.8184}, + {0.20, 18.9398}, + {0.25, 19.9393}, + {0.30, 20.867}, + {0.35, 21.7524}, + {0.40, 22.6156}, + {0.45, 23.4724}, + {0.50, 24.3366}, + {0.55, 25.2218}, + {0.60, 26.143}, + {0.65, 27.1183}, + {0.70, 28.1719}, + {0.75, 29.3389}, + {0.80, 30.6752}, + {0.85, 32.2825}, + {0.90, 34.3816}, + {0.95, 37.6525}, + {0.99, 44.3141}, + {1.00, 1e+12}, + }}, + // 26 dof + {26, { + {0.10, 17.2919}, + {0.15, 18.6714}, + {0.20, 19.8202}, + {0.25, 20.8434}, + {0.30, 21.7924}, + {0.35, 22.6975}, + {0.40, 23.5794}, + {0.45, 24.4544}, + {0.50, 25.3365}, + {0.55, 26.2395}, + {0.60, 27.1789}, + {0.65, 28.173}, + {0.70, 29.2463}, + {0.75, 30.4346}, + {0.80, 31.7946}, + {0.85, 33.4295}, + {0.90, 35.5632}, + {0.95, 38.8851}, + {0.99, 45.6417}, + {1.00, 1e+12}, + }}, + // 27 dof + {27, { + {0.10, 18.1139}, + {0.15, 19.5272}, + {0.20, 20.703}, + {0.25, 21.7494}, + {0.30, 22.7192}, + {0.35, 23.6437}, + {0.40, 24.544}, + {0.45, 25.4368}, + {0.50, 26.3363}, + {0.55, 27.2569}, + {0.60, 28.2141}, + {0.65, 29.2266}, + {0.70, 30.3193}, + {0.75, 31.5284}, + {0.80, 32.9117}, + {0.85, 34.5736}, + {0.90, 36.7412}, + {0.95, 40.1133}, + {0.99, 46.9629}, + {1.00, 1e+12}, + }}, + // 28 dof + {28, { + {0.10, 18.9392}, + {0.15, 20.3857}, + {0.20, 21.588}, + {0.25, 22.6572}, + {0.30, 23.6475}, + {0.35, 24.5909}, + {0.40, 25.5093}, + {0.45, 26.4195}, + {0.50, 27.3362}, + {0.55, 28.274}, + {0.60, 29.2486}, + {0.65, 30.2791}, + {0.70, 31.3909}, + {0.75, 32.6205}, + {0.80, 34.0266}, + {0.85, 35.715}, + {0.90, 37.9159}, + {0.95, 41.3371}, + {0.99, 48.2782}, + {1.00, 1e+12}, + }}, + // 29 dof + {29, { + {0.10, 19.7677}, + {0.15, 21.2468}, + {0.20, 22.4751}, + {0.25, 23.5666}, + {0.30, 24.577}, + {0.35, 25.5391}, + {0.40, 26.4751}, + {0.45, 27.4025}, + {0.50, 28.3361}, + {0.55, 29.2908}, + {0.60, 30.2825}, + {0.65, 31.3308}, + {0.70, 32.4612}, + {0.75, 33.7109}, + {0.80, 35.1394}, + {0.85, 36.8538}, + {0.90, 39.0875}, + {0.95, 42.557}, + {0.99, 49.5879}, + {1.00, 1e+12}, + }}, + // 30 dof + {30, { + {0.10, 20.5992}, + {0.15, 22.1103}, + {0.20, 23.3641}, + {0.25, 24.4776}, + {0.30, 25.5078}, + {0.35, 26.4881}, + {0.40, 27.4416}, + {0.45, 28.3858}, + {0.50, 29.336}, + {0.55, 30.3073}, + {0.60, 31.3159}, + {0.65, 32.3815}, + {0.70, 33.5302}, + {0.75, 34.7997}, + {0.80, 36.2502}, + {0.85, 37.9903}, + {0.90, 40.256}, + {0.95, 43.773}, + {0.99, 50.8922}, + {1.00, 1e+12}, + }}, +}; + +static std::map<int, std::map<double, double>> chisq_2_CI = { + // 1 dof + {1, { + {0.0157908, 0.10}, + {0.0357658, 0.15}, + {0.0641848, 0.20}, + {0.101531, 0.25}, + {0.148472, 0.30}, + {0.2059, 0.35}, + {0.274996, 0.40}, + {0.357317, 0.45}, + {0.454936, 0.50}, + {0.570652, 0.55}, + {0.708326, 0.60}, + {0.873457, 0.65}, + {1.07419, 0.70}, + {1.3233, 0.75}, + {1.64237, 0.80}, + {2.07225, 0.85}, + {2.70554, 0.90}, + {3.84146, 0.95}, + {6.6349, 0.99}, + { 1e+12, 1.00}, + }}, + // 2 dof + {2, { + {0.210721, 0.10}, + {0.325038, 0.15}, + {0.446287, 0.20}, + {0.575364, 0.25}, + {0.71335, 0.30}, + {0.861566, 0.35}, + {1.02165, 0.40}, + {1.19567, 0.45}, + {1.38629, 0.50}, + {1.59702, 0.55}, + {1.83258, 0.60}, + {2.09964, 0.65}, + {2.40795, 0.70}, + {2.77259, 0.75}, + {3.21888, 0.80}, + {3.79424, 0.85}, + {4.60517, 0.90}, + {5.99146, 0.95}, + {9.21034, 0.99}, + { 1e+12, 1.00}, + }}, + // 3 dof + {3, { + {0.584374, 0.10}, + {0.797771, 0.15}, + {1.00517, 0.20}, + {1.21253, 0.25}, + {1.42365, 0.30}, + {1.64158, 0.35}, + {1.86917, 0.40}, + {2.10947, 0.45}, + {2.36597, 0.50}, + {2.64301, 0.55}, + {2.94617, 0.60}, + {3.28311, 0.65}, + {3.66487, 0.70}, + {4.10834, 0.75}, + {4.64163, 0.80}, + {5.31705, 0.85}, + {6.25139, 0.90}, + {7.81473, 0.95}, + {11.3449, 0.99}, + { 1e+12, 1.00}, + }}, + // 4 dof + {4, { + {1.06362, 0.10}, + {1.36648, 0.15}, + {1.64878, 0.20}, + {1.92256, 0.25}, + {2.1947, 0.30}, + {2.47009, 0.35}, + {2.75284, 0.40}, + {3.04695, 0.45}, + {3.35669, 0.50}, + {3.68713, 0.55}, + {4.04463, 0.60}, + {4.43769, 0.65}, + {4.87843, 0.70}, + {5.38527, 0.75}, + {5.98862, 0.80}, + {6.74488, 0.85}, + {7.77944, 0.90}, + {9.48773, 0.95}, + {13.2767, 0.99}, + { 1e+12, 1.00}, + }}, + // 5 dof + {5, { + {1.61031, 0.10}, + {1.99382, 0.15}, + {2.34253, 0.20}, + {2.6746, 0.25}, + {2.99991, 0.30}, + {3.32511, 0.35}, + {3.6555, 0.40}, + {3.99594, 0.45}, + {4.35146, 0.50}, + {4.72776, 0.55}, + {5.13187, 0.60}, + {5.57307, 0.65}, + {6.06443, 0.70}, + {6.62568, 0.75}, + {7.28928, 0.80}, + {8.1152, 0.85}, + {9.23636, 0.90}, + {11.0705, 0.95}, + {15.0863, 0.99}, + { 1e+12, 1.00}, + }}, + // 6 dof + {6, { + {2.20413, 0.10}, + {2.66127, 0.15}, + {3.07009, 0.20}, + {3.4546, 0.25}, + {3.82755, 0.30}, + {4.19727, 0.35}, + {4.57015, 0.40}, + {4.95188, 0.45}, + {5.34812, 0.50}, + {5.7652, 0.55}, + {6.21076, 0.60}, + {6.69476, 0.65}, + {7.23114, 0.70}, + {7.8408, 0.75}, + {8.55806, 0.80}, + {9.4461, 0.85}, + {10.6446, 0.90}, + {12.5916, 0.95}, + {16.8119, 0.99}, + { 1e+12, 1.00}, + }}, + // 7 dof + {7, { + {2.83311, 0.10}, + {3.35828, 0.15}, + {3.82232, 0.20}, + {4.25485, 0.25}, + {4.67133, 0.30}, + {5.08165, 0.35}, + {5.49323, 0.40}, + {5.91252, 0.45}, + {6.34581, 0.50}, + {6.79997, 0.55}, + {7.28321, 0.60}, + {7.80612, 0.65}, + {8.38343, 0.70}, + {9.03715, 0.75}, + {9.80325, 0.80}, + {10.7479, 0.85}, + {12.017, 0.90}, + {14.0671, 0.95}, + {18.4753, 0.99}, + { 1e+12, 1.00}, + }}, + // 8 dof + {8, { + {3.48954, 0.10}, + {4.0782, 0.15}, + {4.59357, 0.20}, + {5.07064, 0.25}, + {5.52742, 0.30}, + {5.97529, 0.35}, + {6.42265, 0.40}, + {6.87663, 0.45}, + {7.34412, 0.50}, + {7.83251, 0.55}, + {8.35053, 0.60}, + {8.90936, 0.65}, + {9.52446, 0.70}, + {10.2189, 0.75}, + {11.0301, 0.80}, + {12.0271, 0.85}, + {13.3616, 0.90}, + {15.5073, 0.95}, + {20.0902, 0.99}, + { 1e+12, 1.00}, + }}, + // 9 dof + {9, { + {4.16816, 0.10}, + {4.81652, 0.15}, + {5.38005, 0.20}, + {5.89883, 0.25}, + {6.39331, 0.30}, + {6.87627, 0.35}, + {7.35703, 0.40}, + {7.84342, 0.45}, + {8.34283, 0.50}, + {8.86317, 0.55}, + {9.41364, 0.60}, + {10.006, 0.65}, + {10.6564, 0.70}, + {11.3888, 0.75}, + {12.2421, 0.80}, + {13.288, 0.85}, + {14.6837, 0.90}, + {16.919, 0.95}, + {21.666, 0.99}, + { 1e+12, 1.00}, + }}, + // 10 dof + {10, { + {4.86518, 0.10}, + {5.57006, 0.15}, + {6.17908, 0.20}, + {6.7372, 0.25}, + {7.26722, 0.30}, + {7.78324, 0.35}, + {8.29547, 0.40}, + {8.81235, 0.45}, + {9.34182, 0.50}, + {9.89222, 0.55}, + {10.4732, 0.60}, + {11.0971, 0.65}, + {11.7807, 0.70}, + {12.5489, 0.75}, + {13.442, 0.80}, + {14.5339, 0.85}, + {15.9872, 0.90}, + {18.307, 0.95}, + {23.2093, 0.99}, + { 1e+12, 1.00}, + }}, + // 11 dof + {11, { + {5.57778, 0.10}, + {6.33643, 0.15}, + {6.98867, 0.20}, + {7.58414, 0.25}, + {8.14787, 0.30}, + {8.69524, 0.35}, + {9.23729, 0.40}, + {9.78306, 0.45}, + {10.341, 0.50}, + {10.9199, 0.55}, + {11.5298, 0.60}, + {12.1836, 0.65}, + {12.8987, 0.70}, + {13.7007, 0.75}, + {14.6314, 0.80}, + {15.7671, 0.85}, + {17.275, 0.90}, + {19.6751, 0.95}, + {24.725, 0.99}, + { 1e+12, 1.00}, + }}, + // 12 dof + {12, { + {6.3038, 0.10}, + {7.11384, 0.15}, + {7.80733, 0.20}, + {8.43842, 0.25}, + {9.03428, 0.30}, + {9.61152, 0.35}, + {10.182, 0.40}, + {10.7553, 0.45}, + {11.3403, 0.50}, + {11.9463, 0.55}, + {12.5838, 0.60}, + {13.2661, 0.65}, + {14.0111, 0.70}, + {14.8454, 0.75}, + {15.812, 0.80}, + {16.9893, 0.85}, + {18.5493, 0.90}, + {21.0261, 0.95}, + {26.217, 0.99}, + { 1e+12, 1.00}, + }}, + // 13 dof + {13, { + {7.0415, 0.10}, + {7.90084, 0.15}, + {8.63386, 0.20}, + {9.29907, 0.25}, + {9.92568, 0.30}, + {10.5315, 0.35}, + {11.1291, 0.40}, + {11.7288, 0.45}, + {12.3398, 0.50}, + {12.9717, 0.55}, + {13.6356, 0.60}, + {14.3451, 0.65}, + {15.1187, 0.70}, + {15.9839, 0.75}, + {16.9848, 0.80}, + {18.202, 0.85}, + {19.8119, 0.90}, + {22.362, 0.95}, + {27.6882, 0.99}, + { 1e+12, 1.00}, + }}, + // 14 dof + {14, { + {7.78953, 0.10}, + {8.6963, 0.15}, + {9.46733, 0.20}, + {10.1653, 0.25}, + {10.8215, 0.30}, + {11.4548, 0.35}, + {12.0785, 0.40}, + {12.7034, 0.45}, + {13.3393, 0.50}, + {13.9961, 0.55}, + {14.6853, 0.60}, + {15.4209, 0.65}, + {16.2221, 0.70}, + {17.1169, 0.75}, + {18.1508, 0.80}, + {19.4062, 0.85}, + {21.0641, 0.90}, + {23.6848, 0.95}, + {29.1412, 0.99}, + { 1e+12, 1.00}, + }}, + // 15 dof + {15, { + {8.54676, 0.10}, + {9.49928, 0.15}, + {10.307, 0.20}, + {11.0365, 0.25}, + {11.7212, 0.30}, + {12.3809, 0.35}, + {13.0297, 0.40}, + {13.679, 0.45}, + {14.3389, 0.50}, + {15.0197, 0.55}, + {15.7332, 0.60}, + {16.494, 0.65}, + {17.3217, 0.70}, + {18.2451, 0.75}, + {19.3107, 0.80}, + {20.603, 0.85}, + {22.3071, 0.90}, + {24.9958, 0.95}, + {30.5779, 0.99}, + { 1e+12, 1.00}, + }}, + // 16 dof + {16, { + {9.31224, 0.10}, + {10.309, 0.15}, + {11.1521, 0.20}, + {11.9122, 0.25}, + {12.6243, 0.30}, + {13.3096, 0.35}, + {13.9827, 0.40}, + {14.6555, 0.45}, + {15.3385, 0.50}, + {16.0425, 0.55}, + {16.7795, 0.60}, + {17.5646, 0.65}, + {18.4179, 0.70}, + {19.3689, 0.75}, + {20.4651, 0.80}, + {21.7931, 0.85}, + {23.5418, 0.90}, + {26.2962, 0.95}, + {31.9999, 0.99}, + { 1e+12, 1.00}, + }}, + // 17 dof + {17, { + {10.0852, 0.10}, + {11.1249, 0.15}, + {12.0023, 0.20}, + {12.7919, 0.25}, + {13.5307, 0.30}, + {14.2407, 0.35}, + {14.9373, 0.40}, + {15.6328, 0.45}, + {16.3382, 0.50}, + {17.0646, 0.55}, + {17.8244, 0.60}, + {18.633, 0.65}, + {19.511, 0.70}, + {20.4887, 0.75}, + {21.6146, 0.80}, + {22.977, 0.85}, + {24.769, 0.90}, + {27.5871, 0.95}, + {33.4087, 0.99}, + { 1e+12, 1.00}, + }}, + // 18 dof + {18, { + {10.8649, 0.10}, + {11.9463, 0.15}, + {12.857, 0.20}, + {13.6753, 0.25}, + {14.4399, 0.30}, + {15.1738, 0.35}, + {15.8932, 0.40}, + {16.6108, 0.45}, + {17.3379, 0.50}, + {18.086, 0.55}, + {18.8679, 0.60}, + {19.6993, 0.65}, + {20.6014, 0.70}, + {21.6049, 0.75}, + {22.7595, 0.80}, + {24.1555, 0.85}, + {25.9894, 0.90}, + {28.8693, 0.95}, + {34.8053, 0.99}, + { 1e+12, 1.00}, + }}, + // 19 dof + {19, { + {11.6509, 0.10}, + {12.7727, 0.15}, + {13.7158, 0.20}, + {14.562, 0.25}, + {15.3517, 0.30}, + {16.1089, 0.35}, + {16.8504, 0.40}, + {17.5894, 0.45}, + {18.3377, 0.50}, + {19.1069, 0.55}, + {19.9102, 0.60}, + {20.7638, 0.65}, + {21.6891, 0.70}, + {22.7178, 0.75}, + {23.9004, 0.80}, + {25.3289, 0.85}, + {27.2036, 0.90}, + {30.1435, 0.95}, + {36.1909, 0.99}, + { 1e+12, 1.00}, + }}, + // 20 dof + {20, { + {12.4426, 0.10}, + {13.6039, 0.15}, + {14.5784, 0.20}, + {15.4518, 0.25}, + {16.2659, 0.30}, + {17.0458, 0.35}, + {17.8088, 0.40}, + {18.5687, 0.45}, + {19.3374, 0.50}, + {20.1272, 0.55}, + {20.9514, 0.60}, + {21.8265, 0.65}, + {22.7745, 0.70}, + {23.8277, 0.75}, + {25.0375, 0.80}, + {26.4976, 0.85}, + {28.412, 0.90}, + {31.4104, 0.95}, + {37.5662, 0.99}, + { 1e+12, 1.00}, + }}, + // 21 dof + {21, { + {13.2396, 0.10}, + {14.4393, 0.15}, + {15.4446, 0.20}, + {16.3444, 0.25}, + {17.1823, 0.30}, + {17.9843, 0.35}, + {18.7683, 0.40}, + {19.5485, 0.45}, + {20.3372, 0.50}, + {21.147, 0.55}, + {21.9915, 0.60}, + {22.8876, 0.65}, + {23.8578, 0.70}, + {24.9348, 0.75}, + {26.1711, 0.80}, + {27.662, 0.85}, + {29.6151, 0.90}, + {32.6706, 0.95}, + {38.9322, 0.99}, + { 1e+12, 1.00}, + }}, + // 22 dof + {22, { + {14.0415, 0.10}, + {15.2788, 0.15}, + {16.314, 0.20}, + {17.2396, 0.25}, + {18.1007, 0.30}, + {18.9243, 0.35}, + {19.7288, 0.40}, + {20.5288, 0.45}, + {21.337, 0.50}, + {22.1663, 0.55}, + {23.0307, 0.60}, + {23.9473, 0.65}, + {24.939, 0.70}, + {26.0393, 0.75}, + {27.3015, 0.80}, + {28.8225, 0.85}, + {30.8133, 0.90}, + {33.9244, 0.95}, + {40.2894, 0.99}, + { 1e+12, 1.00}, + }}, + // 23 dof + {23, { + {14.848, 0.10}, + {16.1219, 0.15}, + {17.1865, 0.20}, + {18.1373, 0.25}, + {19.0211, 0.30}, + {19.8657, 0.35}, + {20.6902, 0.40}, + {21.5096, 0.45}, + {22.3369, 0.50}, + {23.1852, 0.55}, + {24.0689, 0.60}, + {25.0055, 0.65}, + {26.0184, 0.70}, + {27.1413, 0.75}, + {28.4288, 0.80}, + {29.9792, 0.85}, + {32.0069, 0.90}, + {35.1725, 0.95}, + {41.6384, 0.99}, + { 1e+12, 1.00}, + }}, + // 24 dof + {24, { + {15.6587, 0.10}, + {16.9686, 0.15}, + {18.0618, 0.20}, + {19.0373, 0.25}, + {19.9432, 0.30}, + {20.8084, 0.35}, + {21.6525, 0.40}, + {22.4908, 0.45}, + {23.3367, 0.50}, + {24.2037, 0.55}, + {25.1063, 0.60}, + {26.0625, 0.65}, + {27.096, 0.70}, + {28.2412, 0.75}, + {29.5533, 0.80}, + {31.1325, 0.85}, + {33.1962, 0.90}, + {36.415, 0.95}, + {42.9798, 0.99}, + { 1e+12, 1.00}, + }}, + // 25 dof + {25, { + {16.4734, 0.10}, + {17.8184, 0.15}, + {18.9398, 0.20}, + {19.9393, 0.25}, + {20.867, 0.30}, + {21.7524, 0.35}, + {22.6156, 0.40}, + {23.4724, 0.45}, + {24.3366, 0.50}, + {25.2218, 0.55}, + {26.143, 0.60}, + {27.1183, 0.65}, + {28.1719, 0.70}, + {29.3389, 0.75}, + {30.6752, 0.80}, + {32.2825, 0.85}, + {34.3816, 0.90}, + {37.6525, 0.95}, + {44.3141, 0.99}, + { 1e+12, 1.00}, + }}, + // 26 dof + {26, { + {17.2919, 0.10}, + {18.6714, 0.15}, + {19.8202, 0.20}, + {20.8434, 0.25}, + {21.7924, 0.30}, + {22.6975, 0.35}, + {23.5794, 0.40}, + {24.4544, 0.45}, + {25.3365, 0.50}, + {26.2395, 0.55}, + {27.1789, 0.60}, + {28.173, 0.65}, + {29.2463, 0.70}, + {30.4346, 0.75}, + {31.7946, 0.80}, + {33.4295, 0.85}, + {35.5632, 0.90}, + {38.8851, 0.95}, + {45.6417, 0.99}, + { 1e+12, 1.00}, + }}, + // 27 dof + {27, { + {18.1139, 0.10}, + {19.5272, 0.15}, + {20.703, 0.20}, + {21.7494, 0.25}, + {22.7192, 0.30}, + {23.6437, 0.35}, + {24.544, 0.40}, + {25.4368, 0.45}, + {26.3363, 0.50}, + {27.2569, 0.55}, + {28.2141, 0.60}, + {29.2266, 0.65}, + {30.3193, 0.70}, + {31.5284, 0.75}, + {32.9117, 0.80}, + {34.5736, 0.85}, + {36.7412, 0.90}, + {40.1133, 0.95}, + {46.9629, 0.99}, + { 1e+12, 1.00}, + }}, + // 28 dof + {28, { + {18.9392, 0.10}, + {20.3857, 0.15}, + {21.588, 0.20}, + {22.6572, 0.25}, + {23.6475, 0.30}, + {24.5909, 0.35}, + {25.5093, 0.40}, + {26.4195, 0.45}, + {27.3362, 0.50}, + {28.274, 0.55}, + {29.2486, 0.60}, + {30.2791, 0.65}, + {31.3909, 0.70}, + {32.6205, 0.75}, + {34.0266, 0.80}, + {35.715, 0.85}, + {37.9159, 0.90}, + {41.3371, 0.95}, + {48.2782, 0.99}, + { 1e+12, 1.00}, + }}, + // 29 dof + {29, { + {19.7677, 0.10}, + {21.2468, 0.15}, + {22.4751, 0.20}, + {23.5666, 0.25}, + {24.577, 0.30}, + {25.5391, 0.35}, + {26.4751, 0.40}, + {27.4025, 0.45}, + {28.3361, 0.50}, + {29.2908, 0.55}, + {30.2825, 0.60}, + {31.3308, 0.65}, + {32.4612, 0.70}, + {33.7109, 0.75}, + {35.1394, 0.80}, + {36.8538, 0.85}, + {39.0875, 0.90}, + {42.557, 0.95}, + {49.5879, 0.99}, + { 1e+12, 1.00}, + }}, + // 30 dof + {30, { + {20.5992, 0.10}, + {22.1103, 0.15}, + {23.3641, 0.20}, + {24.4776, 0.25}, + {25.5078, 0.30}, + {26.4881, 0.35}, + {27.4416, 0.40}, + {28.3858, 0.45}, + {29.336, 0.50}, + {30.3073, 0.55}, + {31.3159, 0.60}, + {32.3815, 0.65}, + {33.5302, 0.70}, + {34.7997, 0.75}, + {36.2502, 0.80}, + {37.9903, 0.85}, + {40.256, 0.90}, + {43.773, 0.95}, + {50.8922, 0.99}, + { 1e+12, 1.00}, + }}, +}; + +}// namespace GnssUtils + +#endif /* INCLUDE_GNSS_UTILS_UTILS_CHISQUARE_CI_MAPS_H_ */ diff --git a/src/tdcp.cpp b/src/tdcp.cpp index 44b0f247fffc0cf5136050d46417da190b525c84..6093631bb23b501b4bfd9c091d475be8ba20f16b 100644 --- a/src/tdcp.cpp +++ b/src/tdcp.cpp @@ -3,6 +3,7 @@ #include "gnss_utils/utils/rcv_position.h" #include "gnss_utils/utils/satellite.h" #include "gnss_utils/snapshot.h" +#include "gnss_utils/utils/chisquare_ci.h" namespace GnssUtils { @@ -287,7 +288,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, TdcpOutput output; Eigen::Vector4d& d(output.d); Eigen::Matrix4d& cov_d(output.cov_d); - double& residual(output.residual); + double& residual_ci(output.residual_ci); // Initial guess Eigen::Vector4d delta_d(Eigen::Vector4d::Zero()); @@ -373,11 +374,11 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, return output; } - // residual - 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_ci + if (tdcp_params.residual_opt == 0) // all problem + residual_ci = chisq2ci((r + A * delta_d).squaredNorm() / var_tdcp, A.rows()); + else if (tdcp_params.residual_opt == 1) // worst sat + residual_ci = chisq2ci((r + A * delta_d).cwiseAbs2().maxCoeff() / var_tdcp, 1); else throw std::runtime_error("unknown value for residual_opt"); // residual = (r + A * delta_d).norm(); @@ -390,7 +391,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, 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: residual_ci = %13.10f\n", residual_ci); 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; @@ -403,7 +404,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, 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"); + std::cout << "\tresidual_ci > tdcp_params.max_residual_ci: " << (residual_ci > tdcp_params.max_residual_ci ? "OK\n":"FAILED\n"); } #endif @@ -411,25 +412,25 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, if (j == 0 and A.rows() > tdcp_params.min_common_sats and tdcp_params.raim_n > 0 and - residual > tdcp_params.max_residual) + residual_ci > tdcp_params.max_residual_ci) { #if GNSS_UTILS_TDCP_DEBUG == 1 std::cout << "//////////////// Performing RAIM ////////////////\n"; #endif int worst_sat_row = -1; - double best_residual = 1e12; + double best_residual_ci = 1e12; Eigen::Vector4d best_d; - // remove up to 'raim_n' observations (if enough satellites and residual condition holds) + // remove up to 'raim_n' observations (if enough satellites and residual_ci condition holds) while (raim_discarded_rows.size() < tdcp_params.raim_n and A.rows() - raim_discarded_rows.size() > tdcp_params.min_common_sats and - residual > tdcp_params.max_residual) + residual_ci > tdcp_params.max_residual_ci) { auto A_raim = A; auto r_raim = r; - // METHOD A: using normalized RMSE residual + // METHOD A: using normalized RMSE residual_ci if (tdcp_params.residual_opt == 0) { // solve removing each row @@ -450,11 +451,11 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, // 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; + // residual_ci + if (tdcp_params.residual_opt == 0) // all problem + residual_ci = chisq2ci((r_raim + A_raim * delta_d_raim).squaredNorm() / var_tdcp, A_raim.rows() - raim_discarded_rows.size()); + else if (tdcp_params.residual_opt == 1) // worst sat + residual_ci = chisq2ci((r_raim + A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp, 1); else throw std::runtime_error("unknown value for residual_opt"); // residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1); @@ -462,18 +463,18 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, #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); + printf("\tresidual_ci = %13.10f\n", residual_ci); //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) + // store if best residual_ci + if (residual_ci < best_residual_ci) { worst_sat_row = row_removed; - best_residual = residual; + best_residual_ci = residual_ci; best_d = d_raim; } } @@ -482,10 +483,10 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, r_raim(row_removed) = r(row_removed); } } - // METHOD B: using max residual in Mahalanobis distance + // METHOD B: using max residual_ci in Mahalanobis distance else if (tdcp_params.residual_opt == 1) { - // find index of max residual + // find index of max residual_ci (r + A * delta_d).cwiseAbs2().maxCoeff(&worst_sat_row); // remove row @@ -499,11 +500,11 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, // 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; + // residual_ci + if (tdcp_params.residual_opt == 0) // all problem + residual_ci = chisq2ci((r_raim + A_raim * delta_d_raim).squaredNorm() / var_tdcp, A_raim.rows() - raim_discarded_rows.size()); + else if (tdcp_params.residual_opt == 1) // worst sat + residual_ci = chisq2ci((r_raim + A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp, 1); else throw std::runtime_error("unknown value for residual_opt"); // residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1); @@ -511,15 +512,15 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, #if GNSS_UTILS_TDCP_DEBUG == 1 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); + printf("\tresidual_ci = %13.10f\n", residual_ci); //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 as best residual - best_residual = residual; + // store as best residual_ci + best_residual_ci = residual_ci; best_d = d_raim; } } @@ -545,7 +546,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, // Choose the best RAIM solution d = best_d; cov_d = (A.transpose() * A).inverse(); - residual = best_residual; + residual_ci = best_residual_ci; } #if GNSS_UTILS_TDCP_DEBUG == 1 @@ -557,7 +558,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, 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 = " << best_residual << std::endl; + std::cout << "\tResidual = " << best_residual_ci << 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; @@ -573,7 +574,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, 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 << "\tResidual = " << residual_ci << 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; @@ -585,21 +586,20 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, 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); - - // residual - if (tdcp_params.residual_opt == 0) // RMSE - residual = sqrt(r.squaredNorm() / (r.rows() - raim_discarded_rows.size())) / sqrt(var_tdcp); - else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise) - residual = r.cwiseAbs2().maxCoeff() / var_tdcp; + // residual_ci + if (tdcp_params.residual_opt == 0) // all problem + residual_ci = chisq2ci(r.squaredNorm() / var_tdcp, r.rows() - raim_discarded_rows.size()); + else if (tdcp_params.residual_opt == 1) // worst sat + residual_ci = chisq2ci(r.cwiseAbs2().maxCoeff() / var_tdcp, 1); else throw std::runtime_error("unknown value for residual_opt"); //residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size())); - // check residual condition - if (tdcp_params.validate_residual and residual > tdcp_params.max_residual) + // check residual_ci condition + if (tdcp_params.validate_residual and residual_ci > tdcp_params.max_residual_ci) { - 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"; + printf("Tdcp: Didn't success. Final residual_ci=%f bigger than max_residual_ci=%f.\n", residual_ci, tdcp_params.max_residual_ci); + output.msg = "Residual bigger than max_residual_ci"; output.success = false; } else diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 4b38d2e10f58811e98ba3d282072327530cc0892..c180b0041c8d6001ce578b5c25b9813061259dec 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -26,4 +26,8 @@ target_link_libraries(gtest_tdcp ${PROJECT_NAME}) # Transformations test gnss_utils_add_gtest(gtest_transformations gtest_transformations.cpp) -target_link_libraries(gtest_transformations ${PROJECT_NAME}) \ No newline at end of file +target_link_libraries(gtest_transformations ${PROJECT_NAME}) + +# ChiSquare test +gnss_utils_add_gtest(gtest_chisquare gtest_chisquare.cpp) +target_link_libraries(gtest_chisquare ${PROJECT_NAME}) \ No newline at end of file diff --git a/test/gtest_chisquare.cpp b/test/gtest_chisquare.cpp new file mode 100644 index 0000000000000000000000000000000000000000..63c81c57d10ecc784f1510c393272bc6478b2bee --- /dev/null +++ b/test/gtest_chisquare.cpp @@ -0,0 +1,64 @@ +#include "gtest/utils_gtest.h" +#include "gnss_utils/utils/chisquare_ci.h" + +using namespace GnssUtils; + +/* chisq_2_CI: + * 4 DOF + * {1.06362, 0.10} + * {1.36648, 0.15}, + * {1.64878, 0.20}, + * {1.92256, 0.25}, + */ +TEST(ChiSquareTest, chisq2ci) +{ + // (almost) exact + ASSERT_NEAR(chisq2ci(1.36648, 4), 0.15, 1e-12); + + // Before first value + ASSERT_NEAR(chisq2ci(0.8, 4), 0.10, 1e-12); + + // Interpolation + for (double r = 0; r <= 1; r+=0.1) + ASSERT_NEAR(chisq2ci(r*1.36648+(1-r)*1.64878, 4), r*0.15+(1-r)*0.2, 1e-12); + + // negative or zero dof + ASSERT_DEATH(chisq2ci(0.4, -2), ""); + ASSERT_DEATH(chisq2ci(0.4, 0), ""); + + // more than 30 dof + ASSERT_NEAR(chisq2ci(0.8, 34), chisq2ci(0.8, 30), 1e-12); +} + +/* CI_2_chisq + * 4 DOF + * {0.10, 1.06362}, + * {0.15, 1.36648}, + * {0.20, 1.64878}, + * {0.25, 1.92256}, + */ +TEST(ChiSquareTest, ci2chisq) +{ + // (almost) exact + ASSERT_NEAR(ci2chisq(0.15, 4), 1.36648, 1e-12); + + // Before first value + ASSERT_NEAR(ci2chisq(0.05, 4), 1.06362, 1e-12); + + // Interpolation + for (double r = 0; r <= 1; r+=0.1) + ASSERT_NEAR(ci2chisq(r*0.15+(1-r)*0.2, 4), r*1.36648+(1-r)*1.64878, 1e-12); + + // negative or zero dof + ASSERT_DEATH(ci2chisq(0.4, -2), ""); + ASSERT_DEATH(ci2chisq(0.4, 0), ""); + + // more than 30 dof + ASSERT_NEAR(ci2chisq(0.8, 34), ci2chisq(0.8, 30), 1e-12); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp index 78811d820f500ac9da1267d7d333b0d84912d964..cd9487900a915e04c2e9142177ecf7334cdf8374 100644 --- a/test/gtest_tdcp.cpp +++ b/test/gtest_tdcp.cpp @@ -73,7 +73,7 @@ TEST(Tdcp, Tdcp) tdcp_params.min_common_sats = 6; tdcp_params.raim_n = 0; tdcp_params.residual_opt = 0; - tdcp_params.max_residual = 1e20; + tdcp_params.max_residual_ci = 1e20; tdcp_params.validate_residual = false; tdcp_params.sagnac_correction = false; tdcp_params.relinearize_jacobian = true; @@ -87,7 +87,7 @@ TEST(Tdcp, Tdcp) Vector2d azel, azel2; Vector4d d_gt; Matrix4d cov_d; - double range, residual; + double range, residual_ci; // Snapshots auto snapshot_r = std::make_shared<Snapshot>(); @@ -200,7 +200,7 @@ TEST(Tdcp, Tdcp) // CHECKS std::cout << "CHECKS Test " << i << std::endl; ASSERT_TRUE(tdcp_out.success); - ASSERT_LE(tdcp_out.residual, 1e-3); + ASSERT_LE(tdcp_out.residual_ci, 0.8); ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3); } } @@ -214,7 +214,7 @@ TEST(Tdcp, Tdcp_raim_residual_rmse) tdcp_params.min_common_sats = 6; tdcp_params.raim_n = 2; tdcp_params.residual_opt = 0; // Normalized RMSE - tdcp_params.max_residual = 0.1; // low threshold to detect outliers... + tdcp_params.max_residual_ci = 0.1; // low threshold to detect outliers... tdcp_params.validate_residual = false; tdcp_params.sagnac_correction = false; tdcp_params.relinearize_jacobian = true; @@ -229,7 +229,7 @@ TEST(Tdcp, Tdcp_raim_residual_rmse) Vector2d azel, azel2; Vector4d d, d_gt; Matrix4d cov_d; - double range, residual; + double range, residual_ci; // Snapshots auto snapshot_r = std::make_shared<Snapshot>(); @@ -357,7 +357,7 @@ TEST(Tdcp, Tdcp_raim_residual_rmse) 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_LE(tdcp_out.residual_ci, 0.8); ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3); } } @@ -370,8 +370,8 @@ TEST(Tdcp, Tdcp_raim_residual_max_mah) tdcp_params.max_iterations = 5; tdcp_params.min_common_sats = 6; tdcp_params.raim_n = 2; - tdcp_params.residual_opt = 1; // Max residual in Mahalanobis distance - tdcp_params.max_residual = 3.84; // 95% of confidence + tdcp_params.residual_opt = 1; // Max residual_ci in Mahalanobis distance + tdcp_params.max_residual_ci = 0.95; // 95% of confidence tdcp_params.validate_residual = false; tdcp_params.sagnac_correction = false; tdcp_params.relinearize_jacobian = true; @@ -386,7 +386,7 @@ TEST(Tdcp, Tdcp_raim_residual_max_mah) Vector2d azel, azel2; Vector4d d, d_gt; Matrix4d cov_d; - double range, residual; + double range, residual_ci; // Snapshots auto snapshot_r = std::make_shared<Snapshot>(); @@ -513,7 +513,7 @@ TEST(Tdcp, Tdcp_raim_residual_max_mah) 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_LE(tdcp_out.residual_ci, 0.8); ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3); } }