diff --git a/src/constraint_gps_pseudorange_2D.h b/src/constraint_gps_pseudorange_2D.h index 2acc41040f8909e298a359bae47cdfb7fe8ef6f4..498fab25fe4724dc7b592e9700204bcd68f4c8d5 100644 --- a/src/constraint_gps_pseudorange_2D.h +++ b/src/constraint_gps_pseudorange_2D.h @@ -26,8 +26,6 @@ class ConstraintGPSPseudorange2D : public ConstraintSparse<1, 2, 1, 3, 1, 3, 1> { public: - int verbose_level_ = 1; // 0=nothing printed. only for debug purpose - ConstraintGPSPseudorange2D(FeatureBase* _ftr_ptr, ConstraintStatus _status = CTR_ACTIVE) : ConstraintSparse<1, 2, 1, 3, 1, 3, 1>(_ftr_ptr, CTR_GPS_PR_2D, @@ -84,40 +82,45 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c const T* const _map_p, const T* const _map_o, T* _residual) const { + int verbose_level_ = 1; // 0=nothing printed. only for debug purpose + std::stringstream aux; aux << std::setprecision(12); + std::cout << std::setprecision(12); - std::cout << "+OPERATOR()+" << nodeId() << std::endl; if (verbose_level_ >= 2) { - std::cout << std::setprecision(12); - std::cout << "\n++++++OPERATOR()++++++\n"; - std::cout << "_map_p: " << _map_p[0] << ", " << _map_p[1] << ", " << _map_p[2] << std::endl; - std::cout << "_map_o: " << _map_o[0] << std::endl; + std::cout << "+OPERATOR()+" << nodeId() << std::endl; + std::cout << "_sensor_p(_base): " << _sensor_p[0] << ", " << _sensor_p[1] << ", " << _sensor_p[2] << std::endl; std::cout << "_vehicle_p(_map): " << _vehicle_p[0] << ", " << _vehicle_p[1] << std::endl; std::cout << "_vehicle_o(_map): " << _vehicle_o[0] << std::endl; - std::cout << "_sensor_p(_base): " << _sensor_p[0] << ", " << _sensor_p[1] << ", " << _sensor_p[2] << std::endl; + std::cout << "_map_p: " << _map_p[0] << ", " << _map_p[1] << ", " << _map_p[2] << std::endl; + std::cout << "_map_o: " << _map_o[0] << std::endl; } - Eigen::Matrix<T, 3, 1> sensor_p_base(_sensor_p[0], _sensor_p[1], _sensor_p[2]); //sensor position with respect to the base (the vehicle) - Eigen::Matrix<T, 3, 1> vehicle_p_map(_vehicle_p[0], _vehicle_p[1], T(0)); - Eigen::Matrix<T, 3, 1> map_p(_map_p[0], _map_p[1], _map_p[2]); + //Filling Eigen vectors + Eigen::Matrix<T, 4, 1> sensor_p_base(_sensor_p[0], _sensor_p[1], _sensor_p[2], T(1)); //sensor position with respect to the base (the vehicle) + + /* * Base-to-map transform matrix */ - Eigen::Matrix<T, 3, 3> T_base2map = Eigen::Matrix<T, 3, 3>::Identity(); + Eigen::Matrix<T, 4, 4> T_base2map = Eigen::Matrix<T, 4, 4>::Identity(); T_base2map(0, 0) = T(cos(_vehicle_o[0])); - T_base2map(0, 1) = T(sin(_vehicle_o[0])); - T_base2map(1, 0) = T(-sin(_vehicle_o[0])); + T_base2map(0, 1) = T(-sin(_vehicle_o[0])); + T_base2map(1, 0) = T(sin(_vehicle_o[0])); T_base2map(1, 1) = T(cos(_vehicle_o[0])); - Eigen::Matrix<T, 3, 1> sensor_p_map; // sensor position with respect to map frame (initial frame of the experiment) - sensor_p_map = T_base2map * sensor_p_base + vehicle_p_map; - if (verbose_level_ >= 1) + T_base2map(0, 3) = T(_vehicle_p[0]); + T_base2map(1, 3) = T(_vehicle_p[1]); + + // sensor position with respect to map frame + Eigen::Matrix<T, 4, 1> sensor_p_map = T_base2map * sensor_p_base; + + if (verbose_level_ >= 2) { aux.str(std::string()); aux << sensor_p_map(0); if (aux.str().substr(0, 1) != "[") - std::cout << "!!! sensor_p_map: " << sensor_p_map[0] << ", " << sensor_p_map[1] << ", " << - sensor_p_map[2] << std::endl; + std::cout << "!!! sensor_p_map: " << sensor_p_map[0] << ", " << sensor_p_map[1] << ", " << sensor_p_map[2] << std::endl; else { std::cout << "!!! sensor_p_map: " << aux.str().substr(1, aux.str().find(" ") - 1) << ", "; aux.str(std::string()); @@ -129,9 +132,9 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c aux.str(std::string()); } } + /* - * _map_p from ecef to lla - * https://microem.ru/files/2012/08/GPS.G1-X-00006.pdf + * _map_p from ECEF to LLA (math from https://microem.ru/files/2012/08/GPS.G1-X-00006.pdf ) */ // WGS84 ellipsoid constants T a = T(6378137); // earth's radius @@ -145,51 +148,56 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c T th = T(atan2(a * _map_p[2], b * p)); T lon = T(atan2(_map_p[1], _map_p[0])); T lat = T(atan2((_map_p[2] + ep * ep * b * pow(sin(th), 3)), (p - esq * a * pow(cos(th), 3)))); - // T N = T(a/( sqrt(T(1)-esq*pow(sin(lat),2)) )); - // T alt = T(p / cos(lat) - N); - // mod lat to 0-2pi - while (lon <= T(M_PI)) lon += T(2 * M_PI); - while (lon > T(M_PI)) lon -= T(2 * M_PI); - // correction for altitude near poles left out. - if (verbose_level_ >= 2) + + if (verbose_level_ >= 3) { std::cout << "_map_p: " << _map_p[0] << ", " << _map_p[1] << ", " << _map_p[2] << std::endl; std::cout << "_map_p LLA: " << lat << ", " << lon /*<< ", " << alt*/ << std::endl; std::cout << "_map_p LLA degrees: " << lat * T(180 / M_PI) << ", " << lon * T(180 / M_PI) /*<< ", " << alt*/ << std::endl; } + /* * map-to-ECEF transform matrix + * made by the product of the next 4 matrixes */ - Eigen::Matrix<T, 3, 3> R1 = Eigen::Matrix<T, 3, 3>::Identity(); - R1(0, 0) = T(cos(lon)); - R1(0, 1) = T(sin(lon)); - R1(1, 0) = T(-sin(lon)); - R1(1, 1) = T(cos(lon)); - Eigen::Matrix<T, 3, 3> R2 = Eigen::Matrix<T, 3, 3>::Identity(); - R2(0, 0) = T(cos(lat)); - R2(0, 2) = T(sin(lat)); - R2(2, 0) = T(-sin(lat)); - R2(2, 2) = T(cos(lat)); - Eigen::Matrix<T, 3, 3> R3 = Eigen::Matrix<T, 3, 3>::Zero(); - R3(0, 1) = R3(1, 2) = R3(2, 0) = T(1); - Eigen::Matrix<T, 3, 3> R4 = Eigen::Matrix<T, 3, 3>::Identity(); - R4(0, 0) = T(cos(_map_o[0])); - R4(0, 1) = T(sin(_map_o[0])); - R4(1, 0) = T(-sin(_map_o[0])); - R4(1, 1) = T(cos(_map_o[0])); - Eigen::Matrix<T, 3, 3> T_map2ecef = (R4 * R3 * R2 * R1).inverse(); - /* - * result I want to find: sensor position with respect to ecef - */ - Eigen::Matrix<T, 3, 1> sensor_p_ecef; //sensor position with respect to ecef coordinate system - sensor_p_ecef = T_map2ecef * sensor_p_map + map_p; + Eigen::Matrix<T, 4, 4> T_tran = Eigen::Matrix<T, 4, 4>::Identity(); + T_tran(0, 3) = T(_map_p[0]); + T_tran(1, 3) = T(_map_p[1]); + T_tran(2, 3) = T(_map_p[2]); + + Eigen::Matrix<T, 4, 4> T_lon2ecef = Eigen::Matrix<T, 4, 4>::Identity(); + T_lon2ecef(0, 0) = T(cos(lon)); + T_lon2ecef(0, 1) = T(-sin(lon)); + T_lon2ecef(1, 0) = T(sin(lon)); + T_lon2ecef(1, 1) = T(cos(lon)); + + Eigen::Matrix<T, 4, 4> T_lat2lon = Eigen::Matrix<T, 4, 4>::Identity(); + T_lat2lon(0, 0) = T(cos(lat)); + T_lat2lon(0, 2) = T(-sin(lat)); + T_lat2lon(2, 0) = T(sin(lat)); + T_lat2lon(2, 2) = T(cos(lat)); + + + Eigen::Matrix<T, 4, 4> T_enu2aux = Eigen::Matrix<T, 4, 4>::Zero(); + T_enu2aux(0, 2) = T_enu2aux(1, 0) = T_enu2aux(2, 1) = T_enu2aux(3, 3) = T(1); + + Eigen::Matrix<T, 4, 4> T_map2eno = Eigen::Matrix<T, 4, 4>::Identity(); + T_map2eno(0, 0) = T(cos(_map_o[0])); + T_map2eno(0, 1) = T(-sin(_map_o[0])); + T_map2eno(1, 0) = T(sin(_map_o[0])); + T_map2eno(1, 1) = T(cos(_map_o[0])); + + Eigen::Matrix<T, 4, 4> T_map2ecef = T_tran * T_map2eno * T_enu2aux * T_lat2lon * T_lon2ecef ; + + //sensor position with respect to ecef coordinate system + Eigen::Matrix<T, 4, 1> sensor_p_ecef = T_map2ecef * sensor_p_map; + if (verbose_level_ >= 1) { aux.str(std::string()); aux << sensor_p_ecef(0); if (aux.str().substr(0, 1) != "[") - std::cout << "!!! sensor_p_ecef: " << sensor_p_ecef[0] << ", " << sensor_p_ecef[1] << ", " << - sensor_p_ecef[2] << std::endl; + std::cout << "!!! sensor_p_ecef: " << sensor_p_ecef[0] << ", " << sensor_p_ecef[1] << ", " << sensor_p_ecef[2] << std::endl; else { std::cout << "!!! sensor_p_ecef: " << aux.str().substr(1, aux.str().find(" ") - 1) << ", "; aux.str(std::string()); @@ -206,18 +214,15 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c */ T square_sum = T(0); for (int i = 0; i < 3; ++i) - { square_sum += (sensor_p_ecef[i] - T(sat_position_[i]))*(sensor_p_ecef[i] - T(sat_position_[i])); - } + T distance = (square_sum != T(0)) ? sqrt(square_sum) : T(0); // error = (expected measurement) - (actual measurement) _residual[0] = (distance + _bias[0] * T(LIGHT_SPEED)) - (pseudorange_); if (verbose_level_ >= 2) - { std::cout << "!!! Residual: " << _residual[0] << "\n"; - } // normalizing by the covariance _residual[0] = _residual[0] / T(getMeasurementCovariance()(0, 0));//T(sqrt(getMeasurementCovariance()(0, 0))); diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index f51fd2300a53059eaba0824189fd1f17f7d2e6d7..c5745b178b5521d8aec290b46c9ebcf71969cd7d 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -96,10 +96,3 @@ IF(faramotics_FOUND) ENDIF (laser_scan_utils_FOUND) ENDIF(faramotics_FOUND) - -# Testing GPS stuff -IF(raw_gps_utils_FOUND) - ADD_EXECUTABLE(test_gps_wolf test_gps_wolf.cpp) - TARGET_LINK_LIBRARIES(test_gps_wolf ${raw_gps_utils_LIBRARY} ${PROJECT_NAME}) -ENDIF(raw_gps_utils_FOUND) - diff --git a/src/examples/test_gps_wolf.cpp b/src/examples/test_gps_wolf.cpp deleted file mode 100644 index bdec9ca2a196d36fe6e0456af83385c14b569698..0000000000000000000000000000000000000000 --- a/src/examples/test_gps_wolf.cpp +++ /dev/null @@ -1,597 +0,0 @@ -// Testing create and delete full wolf tree with GPS captures - - -//Wolf includes -#include <processor_gps.h> -#include "wolf_manager_gps.h" -#include "ceres_wrapper/ceres_manager.h" - -using namespace std; - - -void addRealGPSMeasurements(rawgpsutils::SatellitesObs &obs, int i); - -int main(int argc, char** argv) -{ - // TEMP! just to try some math using eigen -// testMathQuaternion(); -// return 0; - - bool useCeres = true; - bool ceresVerbose = true; - unsigned int n_captures = 41; - - //Welcome message - cout << endl << " ========= WOLF TREE test ===========" << endl << endl; - - - // Initial x, y, z, bias - // 4789360.65929 177175.418126 4194534.14743 0.000242545313354 - - - /* - * Parameters, to be optimized - */ - // Initial ecef position of the experiment - StateBlock* vehicle_init_p = new StateBlock(Eigen::Vector3s(4789000, 177000, 4194000)); - StateBlock* vehicle_init_o = new StateBlock(Eigen::Vector1s::Zero());// vehicle init orientation - - // Sensor position with respect to vehicle's frame - StateBlock* sensor_p = new StateBlock(Eigen::Vector3s(1, 0, 0), true);//::Zero()); //gps sensor position - StateBlock* sensor_o = new StateBlock(Eigen::Vector4s::Zero(), true); //gps sensor orientation - - //gps sensor bias - StateBlock* sensor_bias = new StateBlock(Eigen::Vector1s::Zero()); - - /* - * GPS Sensor - */ - SensorGPS* gps_sensor_ptr_ = new SensorGPS(sensor_p, sensor_o, sensor_bias, vehicle_init_p, vehicle_init_o); - gps_sensor_ptr_->addProcessor(new ProcessorGPS()); - - Eigen::Vector3s prior = Eigen::Vector3s(10, 10, 90*M_PI/180); -// 4789360.65929, 177175.418126, 4194534.14743 - /* - * GPS WolfManager - */ - WolfManagerGPS* wolf_manager_ = new WolfManagerGPS(PO_2D, //frame structure - nullptr, //_sensor_prior_ptr - prior, //prior - Eigen::Matrix3s::Identity()*0.01, //prior cov - 5, //window size - 1); //time for new keyframe - - - /* - * Ceres wrapper - */ - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH - ceres_options.max_line_search_step_contraction = 1e-3; - ceres_options.max_num_iterations = 1e4; - ceres::Problem::Options problem_options; - problem_options.cost_function_ownership = ceres::TAKE_OWNERSHIP; - problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP; - problem_options.local_parameterization_ownership = ceres::TAKE_OWNERSHIP; - CeresManager* ceres_manager = new CeresManager(wolf_manager_->getProblemPtr(), problem_options); - - - wolf_manager_->addSensor(gps_sensor_ptr_); - - - /* - * Data Captures - */ - for(unsigned int i=0; i < n_captures; ++i) { - cout << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" << endl; - cout << "%%%%%%%%%%%%%%%%%%%%%%%%%%% CAPTURE #" << i << " %%%%%%%%%%%%%%%%%%%%%%%%%%" << endl; - TimeStamp time_stamp(i); - - rawgpsutils::SatellitesObs obs; - -// obs.addPrMeasurement(rawgpsutils::PrMeasurement(1, 666, 5000, 5000, 5000)); -// obs.addPrMeasurement(rawgpsutils::PrMeasurement(2, 777, 4000, 4500, 6000)); -// obs.addPrMeasurement(rawgpsutils::PrMeasurement(3, 888, 1000, 2000, 3000)); - - addRealGPSMeasurements(obs, i); - std::cout << "added " << obs.measurements_.size() << " measurements\n"; - - - // Create synthetic gps capture - CaptureGPS *cpt_ptr_ = new CaptureGPS(time_stamp, gps_sensor_ptr_, obs); - - // Add capture - wolf_manager_->addCapture(cpt_ptr_); - //cout << "capture added" << endl; - - // update wolf tree - wolf_manager_->update(); - //cout << "wolf manager updated" << endl; - - if (useCeres) - { - ceres_manager->update(); - //cout << "ceres manager updated" << endl; - - ceres::Solver::Summary summary; - - summary = ceres_manager->solve(ceres_options); - if (ceresVerbose) - cout << summary.FullReport() << endl; - } - -// - std::cout << setprecision(12); - std::cout << "\n~~~~ RESULTS ~~~~\n"; - std::cout << "|\tinitial P: " << gps_sensor_ptr_->getMapPPtr()->getVector().transpose() << std::endl;// initial vehicle position (ecef) - std::cout << "|\tinitial O: " << gps_sensor_ptr_->getMapOPtr()->getVector().transpose() << std::endl;// initial vehicle orientation (ecef) - std::cout << "|\tVehicle Pose: " << wolf_manager_->getVehiclePose().transpose() << std::endl;// position of the vehicle's frame with respect to the initial pos frame - std::cout << "|\tVehicle P (last frame): " << wolf_manager_->getProblemPtr()->getLastFramePtr()->getPPtr()->getVector().transpose() << std::endl;// position of the vehicle's frame with respect to the initial pos frame - std::cout << "|\tVehicle O (last frame): " << wolf_manager_->getProblemPtr()->getLastFramePtr()->getOPtr()->getVector().transpose() << std::endl;// position of the vehicle's frame with respect to the initial pos frame - -// //To print all the previous frames -// for (auto it : *(wolf_manager_->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr())) -// { -// std::cout << "|\tVehicle P: " << it->getPPtr()->getVector().transpose() << std::endl; -// std::cout << "|\tVehicle O: " << it->getOPtr()->getVector().transpose() << std::endl; -// } - std::cout << "|\tsensor P: " << gps_sensor_ptr_->getPPtr()->getVector().transpose() << std::endl;// position of the sensor with respect to the vehicle's frame -// std::cout << "|\tsensor O (not needed):" << gps_sensor_ptr_->getOPtr()->getVector().transpose() << std::endl;// orientation of antenna is not needed, because omnidirectional - std::cout << "|\tbias: " << gps_sensor_ptr_->getIntrinsicPtr()->getVector().transpose() << std::endl;//intrinsic parameter = receiver time bias - std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n\n"; - - - std::cin.ignore(); - } - - - cout << std::endl << " ========= calling delete wolf_manager_ (should not crash) =============" << std::endl; - delete wolf_manager_; //not necessary to delete anything more, wolf will do it! - - cout << " ========= calling delete ceres_manager " - << ((useCeres) ? "(and now a seg fault)" : "(should not crash) ") - << " ==========" << std::endl; - delete ceres_manager; //not necessary to delete anything more, wolf will do it! - - //End message - cout << std::endl << " ================================= END =================================" << std::endl; - - //exit - return 0; -} - -void addRealGPSMeasurements(rawgpsutils::SatellitesObs &obs, int i) -{ - switch (i) - { - case 0: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21721495.6298, 26409387.8247, 573003.543324, 3153763.63525)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23190316.5079, 4860606.44665, -16885408.0132, 19792037.557)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21732058.6383, 25421007.324, -6415001.98836, 4230739.78945)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20846969.1402, 22341766.4761, 6695143.58254, 13194248.4467)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21767220.8807, 15879718.1232, -14384827.6997, 15839259.0549)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21222035.1976, 12072285.4853, 10288910.8024, 21282580.8394)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619862.6735, 4525327.84989, 20248977.9349, 16503748.902)); - break; - - case 1: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21721345.135, 26409349.7003, 573035.482988, 3154075.72471)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23190161.1089, 4860774.43561, -16885217.1117, 19792160.6616)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21732008.1874, 25421064.0542, -6414982.67639, 4230427.52703)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20846855.9292, 22341605.5691, 6695178.92475, 13194501.6944)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21767113.8518, 15879915.2988, -14384836.5217, 15839043.5821)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221971.0998, 12072178.5912, 10289159.7953, 21282521.0628)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619815.8035, 4525226.88799, 20249161.8406, 16503544.535)); - break; - - case 2: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21721194.6161, 26409311.5721, 573067.423116, 3154387.81351)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23190005.738, 4860942.42657, -16885026.21, 19792283.7618)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731957.7946, 25421120.7807, -6414963.36422, 4230115.2637)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20846742.7733, 22341444.6593, 6695214.26827, 13194754.9393)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21767006.9898, 15880112.4717, -14384845.3444, 15838828.1061)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221907.004, 12072071.6987, 10289408.7882, 21282461.2816)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619768.9004, 4525125.92805, 20249345.7445, 16503340.1645)); - break; - - case 3: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21721044.0433, 26409273.4403, 573099.363711, 3154699.90164)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23189850.3561, 4861110.41953, -16884835.3079, 19792406.8579)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731907.3208, 25421177.5033, -6414944.05185, 4229802.99947)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20846629.5333, 22341283.7465, 6695249.61311, 13195008.1815)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21766899.8628, 15880309.6421, -14384854.1677, 15838612.6266)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221842.8843, 12071964.808, 10289657.7809, 21282401.4959)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619722.0693, 4525024.97006, 20249529.6465, 16503135.7905)); - break; - - case 4: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21720893.5354, 26409235.3047, 573131.30477, 3155011.9891)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23189694.9961, 4861278.41449, -16884644.4056, 19792529.9496)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731856.798, 25421234.2222, -6414924.73929, 4229490.73434)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20846516.2603, 22341122.8308, 6695284.95926, 13195261.421)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21766792.6979, 15880506.8098, -14384862.9915, 15838397.1439)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221778.8615, 12071857.919, 10289906.7736, 21282341.7056)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619675.1572, 4524924.01402, 20249713.5469, 16502931.413)); - break; - - case 5: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21720742.9175, 26409197.1654, 573163.246295, 3155324.0759)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23189539.5562, 4861446.41144, -16884453.503, 19792653.0371)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731806.4642, 25421290.9373, -6414905.42653, 4229178.46831)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20846403.0594, 22340961.9122, 6695320.30673, 13195514.6577)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21766685.7149, 15880703.9749, -14384871.816, 15838181.6578)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221714.7837, 12071751.0316, 10290155.7662, 21282281.9108)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619628.2672, 4524823.05994, 20249897.4454, 16502727.0319)); - break; - - case 6: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21720592.4027, 26409159.0223, 573195.188285, 3155636.16202)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23189384.0943, 4861614.41039, -16884262.6001, 19792776.1204)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731756.1334, 25421347.6486, -6414886.11357, 4228866.20138)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20846289.9764, 22340800.9906, 6695355.65551, 13195767.8917)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21766578.7469, 15880901.1373, -14384880.6411, 15837966.1684)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221650.7059, 12071644.146, 10290404.7587, 21282222.1115)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619581.3061, 4524722.10782, 20250081.3421, 16502522.6473)); - break; - - case 7: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21720441.8258, 26409120.8756, 573227.13074, 3155948.24748)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23189228.5953, 4861782.41134, -16884071.697, 19792899.1994)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731705.7605, 25421404.3561, -6414866.80042, 4228553.93356)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20846176.7885, 22340640.0662, 6695391.00561, 13196021.123)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21766471.604, 15881098.2971, -14384889.4669, 15837750.6756)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221586.7222, 12071537.2621, 10290653.7511, 21282162.3076)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619534.412, 4524621.15765, 20250265.2371, 16502318.2591)); - break; - - case 8: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21720291.255, 26409082.7251, 573259.073661, 3156260.33227)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23189073.0504, 4861950.41429, -16883880.7935, 19793022.2742)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731655.4107, 25421461.0599, -6414847.48707, 4228241.66483)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20846063.6915, 22340479.1388, 6695426.35703, 13196274.3515)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21766364.585, 15881295.4543, -14384898.2932, 15837535.1795)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221522.6794, 12071430.3798, 10290902.7434, 21282102.4992)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619487.5019, 4524520.20943, 20250449.1303, 16502113.8674)); - break; - - case 9: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21720140.7401, 26409044.5709, 573291.017048, 3156572.41639)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23188917.6395, 4862118.41923, -16883689.8898, 19793145.3447)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731605.0759, 25421517.7599, -6414828.17352, 4227929.3952)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20845950.5735, 22340318.2085, 6695461.70976, 13196527.5773)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21766257.563, 15881492.6089, -14384907.1201, 15837319.6801)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221458.6056, 12071323.4993, 10291151.7357, 21282042.6863)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619440.5589, 4524419.26317, 20250633.0217, 16501909.4722)); - break; - - case 10: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21719990.0603, 26409006.413, 573322.9609, 3156884.49984)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23188762.3356, 4862286.42617, -16883498.9858, 19793268.4109)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731554.7611, 25421574.4561, -6414808.85977, 4227617.12467)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20845837.3536, 22340157.2752, 6695497.06381, 13196780.8004)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21766150.5711, 15881689.7608, -14384915.9477, 15837104.1773)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221394.4918, 12071216.6205, 10291400.7278, 21281982.8688)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619393.6468, 4524318.31887, 20250816.9113, 16501705.0735)); - break; - - case 11: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21719839.4764, 26408968.2514, 573354.905218, 3157196.58262)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23188606.7836, 4862454.43511, -16883308.0815, 19793391.4729)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731504.4493, 25421631.1485, -6414789.54583, 4227304.85324)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20845724.0736, 22339996.339, 6695532.41918, 13197034.0207)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21766043.5921, 15881886.9101, -14384924.7758, 15836888.6712)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221330.3761, 12071109.7433, 10291649.7199, 21281923.0468)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619346.7187, 4524217.37651, 20251000.7991, 16501500.6712)); - break; - - case 12: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21719688.8426, 26408930.086, 573386.850001, 3157508.66474)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23188451.3797, 4862622.44605, -16883117.1769, 19793514.5306)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731454.1215, 25421687.8372, -6414770.23169, 4226992.58091)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20845610.8597, 22339835.4, 6695567.77586, 13197287.2383)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21765936.4381, 15882084.0568, -14384933.6046, 15836673.1618)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221266.2263, 12071002.8679, 10291898.7119, 21281863.2203)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619299.7796, 4524116.43612, 20251184.6852, 16501296.2654)); - break; - - case 13: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21719538.3057, 26408891.9169, 573418.79525, 3157820.74618)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23188295.9548, 4862790.45898, -16882926.2721, 19793637.5841)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731403.7596, 25421744.522, -6414750.91735, 4226680.30768)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20845497.7067, 22339674.4579, 6695603.13386, 13197540.4532)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21765829.6092, 15882281.2008, -14384942.4339, 15836457.649)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221202.1775, 12070895.9942, 10292147.7038, 21281803.3892)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619252.8386, 4524015.49768, 20251368.5694, 16501091.8561)); - break; - - case 14: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21719387.7459, 26408853.7441, 573450.740965, 3158132.82696)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23188140.7138, 4862958.47391, -16882735.3669, 19793760.6333)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731353.4158, 25421801.2031, -6414731.60281, 4226368.03355)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20845384.5058, 22339513.513, 6695638.49317, 13197793.6653)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21765722.6682, 15882478.3422, -14384951.2639, 15836242.1328)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221138.1827, 12070789.1222, 10292396.6956, 21281743.5536)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619205.8485, 4523914.56119, 20251552.4519, 16500887.4432)); - break; - - case 15: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21719237.102, 26408815.5676, 573482.687145, 3158444.90707)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23187985.4169, 4863126.49084, -16882544.4615, 19793883.6782)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731303.061, 25421857.8805, -6414712.28808, 4226055.75852)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20845271.3578, 22339352.5651, 6695673.8538, 13198046.8747)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21765615.8922, 15882675.481, -14384960.0945, 15836026.6134)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221074.194, 12070682.2518, 10292645.6873, 21281683.7135)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619158.8964, 4523813.62666, 20251736.3326, 16500683.0268)); - break; - - case 16: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21719086.5982, 26408777.3874, 573514.633792, 3158756.98651)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23187830.073, 4863294.50977, -16882353.5558, 19794006.7189)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731252.7022, 25421914.554, -6414692.97315, 4225743.4826)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20845158.2168, 22339191.6144, 6695709.21575, 13198300.0814)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21765509.0033, 15882872.6171, -14384968.9257, 15835811.0906)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21221010.0962, 12070575.3832, 10292894.6789, 21281623.8688)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619111.9974, 4523712.69408, 20251920.2115, 16500478.6069)); - break; - - case 17: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21718936.1013, 26408739.2034, 573546.580904, 3159069.06528)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23187674.664, 4863462.53069, -16882162.6498, 19794129.7554)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731202.3364, 25421971.2238, -6414673.65802, 4225431.20577)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20845045.0739, 22339030.6606, 6695744.57901, 13198553.2853)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21765402.0793, 15883069.7506, -14384977.7575, 15835595.5645)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220946.0934, 12070468.5163, 10293143.6705, 21281564.0196)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619065.0853, 4523611.76345, 20252104.0887, 16500274.1834)); - break; - - case 18: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21718785.5145, 26408701.0157, 573578.528482, 3159381.14338)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23187519.3581, 4863630.55361, -16881971.7436, 19794252.7876)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731152.0176, 25422027.8898, -6414654.34269, 4225118.92804)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20844931.9329, 22338869.704, 6695779.94359, 13198806.4865)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21765295.1164, 15883266.8815, -14384986.5899, 15835380.035)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220881.9756, 12070361.6511, 10293392.6619, 21281504.1658)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23619018.1642, 4523510.83479, 20252287.964, 16500069.7565)); - break; - - case 19: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21718634.9906, 26408662.8243, 573610.476526, 3159693.22081)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23187364.0632, 4863798.57853, -16881780.837, 19794375.8155)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731101.7227, 25422084.552, -6414635.02717, 4224806.64942)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20844818.778, 22338708.7444, 6695815.30948, 13199059.6849)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21765188.2354, 15883464.0097, -14384995.423, 15835164.5022)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220817.8509, 12070254.7876, 10293641.6533, 21281444.3075)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618971.2551, 4523409.90807, 20252471.8376, 16499865.3259)); - break; - - case 20: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21718484.5158, 26408624.6292, 573642.425037, 3160005.29757)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23187208.7663, 4863966.60544, -16881589.9302, 19794498.8392)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731051.3889, 25422141.2104, -6414615.71145, 4224494.36989)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20844705.637, 22338547.782, 6695850.6767, 13199312.8806)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21765081.2044, 15883661.1354, -14385004.2566, 15834948.966)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220753.7411, 12070147.9257, 10293890.6446, 21281384.4447)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618924.2651, 4523308.98331, 20252655.7094, 16499660.8919)); - break; - - case 21: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21718333.9769, 26408586.4303, 573674.374013, 3160317.37367)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23187053.4743, 4864134.63435, -16881399.0231, 19794621.8586)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21731001.0851, 25422197.8651, -6414596.39553, 4224182.08947)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20844592.503, 22338386.8166, 6695886.04523, 13199566.0736)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21764974.1835, 15883858.2584, -14385013.0908, 15834733.4266)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220689.6413, 12070041.0656, 10294139.6358, 21281324.5773)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618877.321, 4523208.06051, 20252839.5794, 16499456.4543)); - break; - - case 22: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21718183.4061, 26408548.2277, 573706.323456, 3160629.44909)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23186898.1574, 4864302.66526, -16881208.1157, 19794744.8738)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730950.7413, 25422254.5159, -6414577.07941, 4223869.80814)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20844479.3661, 22338225.8482, 6695921.41507, 13199819.2639)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21764867.3025, 15884055.3787, -14385021.9257, 15834517.8837)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220625.5165, 12069934.2072, 10294388.6269, 21281264.7054)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618830.3669, 4523107.13966, 20253023.4476, 16499252.0132)); - break; - - case 23: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21718032.7842, 26408510.0215, 573738.273364, 3160941.52385)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23186742.8555, 4864470.69817, -16881017.208, 19794867.8847)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730900.3665, 25422311.1631, -6414557.7631, 4223557.52592)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20844366.2371, 22338064.877, 6695956.78623, 13200072.4514)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21764760.5915, 15884252.4964, -14385030.7611, 15834302.3376)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220561.4338, 12069827.3505, 10294637.6179, 21281204.829)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618783.4058, 4523006.22076, 20253207.314, 16499047.5686)); - break; - - case 24: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21717882.2554, 26408471.8114, 573770.223739, 3161253.59793)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23186587.5615, 4864638.73307, -16880826.3001, 19794990.8913)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730849.9206, 25422367.8064, -6414538.44658, 4223245.24279)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20844253.0852, 22337903.9028, 6695992.15871, 13200325.6362)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21764653.7376, 15884449.6115, -14385039.5972, 15834086.7881)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220497.318, 12069720.4955, 10294886.6088, 21281144.948)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618736.5298, 4522905.30382, 20253391.1787, 16498843.1204)); - break; - - case 25: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21717731.7605, 26408433.5977, 573802.17458, 3161565.67135)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23186432.2816, 4864806.76997, -16880635.3919, 19795113.8937)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730799.4828, 25422424.4459, -6414519.12987, 4222932.95877)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20844139.9152, 22337742.9257, 6696027.53251, 13200578.8182)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21764546.6936, 15884646.724, -14385048.4339, 15833871.2353)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220433.2132, 12069613.6421, 10295135.5996, 21281085.0625)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618689.6717, 4522804.38883, 20253575.0416, 16498638.6688)); - break; - - case 26: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21717581.2727, 26408395.3802, 573834.125888, 3161877.74409)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23186276.9877, 4864974.80887, -16880444.4834, 19795236.8919)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730749.139, 25422481.0817, -6414499.81296, 4222620.67385)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20844026.7942, 22337581.9457, 6696062.90762, 13200831.9975)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21764439.7286, 15884843.8338, -14385057.2712, 15833655.6791)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220369.0994, 12069506.7905, 10295384.5904, 21281025.1725)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618642.8916, 4522703.4758, 20253758.9027, 16498434.2135)); - break; - - case 27: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21717430.6668, 26408357.1591, 573866.077662, 3162189.81617)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23186121.6887, 4865142.84976, -16880253.5746, 19795359.8857)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730698.6902, 25422537.7137, -6414480.49585, 4222308.38803)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20843913.8163, 22337420.9627, 6696098.28405, 13201085.1741)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21764332.8037, 15885040.941, -14385066.1091, 15833440.1196)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220305.0797, 12069399.9406, 10295633.5811, 21280965.2779)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618596.0136, 4522602.56473, 20253942.762, 16498229.7548)); - break; - - case 28: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21717280.158, 26408318.9342, 573898.029903, 3162501.88758)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23185966.4248, 4865310.89266, -16880062.6655, 19795482.8754)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730648.3604, 25422594.3419, -6414461.17855, 4221996.10131)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20843800.7473, 22337259.9768, 6696133.66179, 13201338.3479)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21764225.9207, 15885238.0456, -14385074.9476, 15833224.5567)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220241.0979, 12069293.0924, 10295882.5716, 21280905.3788)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618549.1305, 4522501.6556, 20254126.6195, 16498025.2925)); - break; - - case 29: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21717129.6501, 26408280.7056, 573929.982609, 3162813.95832)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23185811.1329, 4865478.93755, -16879871.7562, 19795605.8607)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730598.0196, 25422650.9664, -6414441.86104, 4221683.81369)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20843687.6174, 22337098.9881, 6696169.04086, 13201591.519)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21764118.9797, 15885435.1475, -14385083.7867, 15833008.9906)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220176.9881, 12069186.2459, 10296131.5621, 21280845.4751)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618502.2054, 4522400.74844, 20254310.4752, 16497820.8267)); - break; - - case 30: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21716979.1333, 26408242.4732, 573961.935783, 3163126.02838)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23185655.843, 4865646.98443, -16879680.8465, 19795728.8419)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730547.6777, 25422707.587, -6414422.54334, 4221371.52518)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20843574.4514, 22336937.9963, 6696204.42123, 13201844.6874)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21764012.1368, 15885632.2469, -14385092.6264, 15832793.4211)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220112.8993, 12069079.4011, 10296380.5525, 21280785.5669)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618455.2013, 4522299.84322, 20254494.3292, 16497616.3574)); - break; - - case 31: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21716828.6474, 26408204.2372, 573993.889423, 3163438.09778)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23185500.535, 4865815.03332, -16879489.9366, 19795851.8187)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730497.1959, 25422764.2039, -6414403.22544, 4221059.23576)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20843461.3354, 22336777.0017, 6696239.80293, 13202097.853)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21763905.1288, 15885829.3435, -14385101.4668, 15832577.8482)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21220048.8796, 12068972.558, 10296629.5428, 21280725.6542)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618408.2493, 4522198.93996, 20254678.1814, 16497411.8845)); - break; - - case 32: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21716678.1286, 26408165.9974, 574025.84353, 3163750.16651)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23185345.2601, 4865983.0842, -16879299.0264, 19795974.7913)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730446.8521, 25422820.817, -6414383.90734, 4220746.94545)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20843348.1815, 22336616.0041, 6696275.18594, 13202351.0159)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21763798.2578, 15886026.4376, -14385110.3077, 15832362.272)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21219984.9068, 12068865.7166, 10296878.5331, 21280665.7369)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618361.3642, 4522098.03866, 20254862.0317, 16497207.4081)); - break; - - case 33: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21716527.6107, 26408127.7539, 574057.798103, 3164062.23457)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23185189.9252, 4866151.13707, -16879108.116, 19796097.7597)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730396.5173, 25422877.4264, -6414364.58905, 4220434.65424)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20843235.0275, 22336455.0036, 6696310.57027, 13202604.1761)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21763691.4219, 15886223.529, -14385119.1492, 15832146.6925)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21219920.844, 12068758.8769, 10297127.5232, 21280605.8151)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618314.4841, 4521997.13931, 20255045.8803, 16497002.9282)); - break; - - case 34: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21716377.1319, 26408089.5066, 574089.753143, 3164374.30195)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23185034.5322, 4866319.19195, -16878917.2052, 19796220.7237)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730346.1495, 25422934.0319, -6414345.27055, 4220122.36213)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20843121.9086, 22336294.0002, 6696345.95592, 13202857.3335)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21763584.5699, 15886420.6178, -14385127.9914, 15831931.1096)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21219856.7422, 12068652.0389, 10297376.5132, 21280545.8888)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618267.661, 4521896.24191, 20255229.7272, 16496798.4447)); - break; - - case 35: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21716226.548, 26408051.2557, 574121.70865, 3164686.36867)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23184879.2203, 4866487.24882, -16878726.2942, 19796343.6836)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730295.6947, 25422990.6337, -6414325.95186, 4219810.06912)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20843008.8036, 22336132.9939, 6696381.34288, 13203110.4882)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21763477.7609, 15886617.7039, -14385136.8342, 15831715.5234)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21219792.7425, 12068545.2026, 10297625.5032, 21280485.9579)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618220.873, 4521795.34647, 20255413.5722, 16496593.9577)); - break; - - case 36: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21716075.9232, 26408013.001, 574153.664624, 3164998.43472)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23184723.9524, 4866655.30769, -16878535.3829, 19796466.6391)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730245.3578, 25423047.2317, -6414306.63296, 4219497.77521)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20842895.6946, 22335971.9846, 6696416.73117, 13203363.6402)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21763370.783, 15886814.7875, -14385145.6775, 15831499.9339)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21219728.6437, 12068438.368, 10297874.4931, 21280426.0225)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618173.8599, 4521694.45299, 20255597.4155, 16496389.4672)); - break; - - case 37: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21715925.4063, 26407974.7426, 574185.621065, 3165310.5001)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23184568.6954, 4866823.36855, -16878344.4713, 19796589.5905)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730195.014, 25423103.8259, -6414287.31387, 4219185.48041)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20842782.5337, 22335810.9725, 6696452.12076, 13203616.7894)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21763263.748, 15887011.8683, -14385154.5215, 15831284.341)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21219664.4749, 12068331.5351, 10298123.4828, 21280366.0826)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618126.8498, 4521593.56145, 20255781.2569, 16496184.9732)); - break; - - case 38: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21715774.9055, 26407936.4805, 574217.577972, 3165622.5648)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23184413.4055, 4866991.43141, -16878153.5594, 19796712.5375)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730144.5362, 25423160.4164, -6414267.99458, 4218873.1847)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20842669.4307, 22335649.9574, 6696487.51168, 13203869.9359)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21763156.8501, 15887208.9466, -14385163.3661, 15831068.7448)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21219600.3631, 12068224.7039, 10298372.4725, 21280306.1381)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618079.9587, 4521492.67188, 20255965.0966, 16495980.4756)); - break; - - case 39: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21715624.4106, 26407898.2147, 574249.535347, 3165934.62884)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23184258.0036, 4867159.49627, -16877962.6473, 19796835.4803)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730094.1974, 25423217.0031, -6414248.67509, 4218560.8881)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20842556.3418, 22335488.9393, 6696522.90391, 13204123.0796)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21763050.0031, 15887406.0222, -14385172.2113, 15830853.1453)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21219536.3724, 12068117.8744, 10298621.4621, 21280246.1891)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23618033.0577, 4521391.78425, 20256148.9345, 16495775.9745)); - break; - - case 40: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21715473.8818, 26407859.9452, 574281.493188, 3166246.6922)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23184102.7677, 4867327.56313, -16877771.7349, 19796958.4189)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21730043.8296, 25423273.586, -6414229.3554, 4218248.5906)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20842443.1088, 22335327.9184, 6696558.29746, 13204376.2206)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21762943.0571, 15887603.0952, -14385181.0571, 15830637.5424)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21219472.3056, 12068011.0466, 10298870.4516, 21280186.2356)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23617986.1776, 4521290.89858, 20256332.7706, 16495571.4699)); - break; - - case 41: - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21715323.3469, 26407821.6719, 574313.451497, 3166558.7549)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23183947.4437, 4867495.63198, -16877580.8221, 19797081.3532)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21729993.4838, 25423330.1651, -6414210.03552, 4217936.2922)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20842329.9678, 22335166.8945, 6696593.69233, 13204629.3589)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21762836.2442, 15887800.1656, -14385189.9035, 15830421.9362)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21219408.1958, 12067904.2205, 10299119.4411, 21280126.2775)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23617939.2745, 4521190.01487, 20256516.605, 16495366.9617)); - break; - - default : - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 30, 21715172.8171, 26407783.3949, 574345.410273, 3166870.81692)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 5, 23183792.0468, 4867663.70283, -16877389.9092, 19797204.2832)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 6, 21729943.1179, 25423386.7404, -6414190.71543, 4217623.99291)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 7, 20842216.7579, 22335005.8677, 6696629.08851, 13204882.4945)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 2, 21762729.2452, 15887997.2333, -14385198.7505, 15830206.3267)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 9, 21219344.085, 12067797.3961, 10299368.4304, 21280066.3149)); - obs.addPrMeasurement(rawgpsutils::PrMeasurement( 23, 23617892.3135, 4521089.13311, 20256700.4375, 16495162.45)); - break; - } -}