├── CMakeLists.txt ├── data ├── errore_eskf.txt ├── errore_in.txt ├── errore_shaf.txt ├── errore_shfaf.txt ├── pianta_eskf.txt ├── pianta_in.txt ├── pianta_shaf.txt └── pianta_shfaf.txt ├── dependencies.sh ├── launch ├── control.launch ├── description.launch ├── spawn_husky_uwb.launch ├── uwb.launch ├── uwb_empty.launch └── world_uwb.launch ├── models └── husky_uwb.urdf.xacro ├── package.xml ├── readme.md ├── rqt_multiplot.xml ├── src ├── fuzzy_demo.py ├── node_sage-husa.py ├── shfaf.py └── shfaf.pyc ├── start_all.sh └── worlds └── uwb_empty.world /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ins_uwb_positioning) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES ins_uwb_positioning 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/ins_uwb_positioning.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/ins_uwb_positioning_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | catkin_install_python(PROGRAMS 159 | src/node_sage-husa.py 160 | src/shfaf.py 161 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 162 | ) 163 | 164 | ## Mark executables for installation 165 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 166 | # install(TARGETS ${PROJECT_NAME}_node 167 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark libraries for installation 171 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 172 | # install(TARGETS ${PROJECT_NAME} 173 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 176 | # ) 177 | 178 | ## Mark cpp header files for installation 179 | # install(DIRECTORY include/${PROJECT_NAME}/ 180 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 181 | # FILES_MATCHING PATTERN "*.h" 182 | # PATTERN ".svn" EXCLUDE 183 | # ) 184 | 185 | ## Mark other files for installation (e.g. launch and bag files, etc.) 186 | # install(FILES 187 | # # myfile1 188 | # # myfile2 189 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 190 | # ) 191 | 192 | ############# 193 | ## Testing ## 194 | ############# 195 | 196 | ## Add gtest based cpp test target and link libraries 197 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_ins_uwb_positioning.cpp) 198 | # if(TARGET ${PROJECT_NAME}-test) 199 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 200 | # endif() 201 | 202 | ## Add folders to be run by python nosetests 203 | # catkin_add_nosetests(test) 204 | -------------------------------------------------------------------------------- /data/errore_eskf.txt: -------------------------------------------------------------------------------- 1 | # /gazebo/link_states/recceipt_time, /filter_error/data 2 | 17403.859000000000378, 0.13090825080871582031 3 | 17404.059000000001106, 0.12988480925559997559 4 | 17404.259000000001834, 0.12823000550270080566 5 | 17404.462999999999738, 0.12667837738990783691 6 | 17404.656999999999243, 0.12503695487976074219 7 | 17404.86200000000099, 0.12364888936281204224 8 | 17405.056000000000495, 0.12180174142122268677 9 | 17405.251000000000204, 0.1200693175196647644 10 | 17405.454000000001543, 0.11819701641798019409 11 | 17405.651999999998225, 0.11195751279592514038 12 | 17405.858000000000175, 0.10949284583330154419 13 | 17406.055000000000291, 0.10718416422605514526 14 | 17406.265999999999622, 0.10494848340749740601 15 | 17406.460999999999331, 0.10276186466217041016 16 | 17406.656999999999243, 0.10072005540132522583 17 | 17406.854999999999563, 0.099305957555770874023 18 | 17407.06499999999869, 0.098704636096954345703 19 | 17407.256000000001222, 0.098564289510250091553 20 | 17407.459999999999127, 0.098995670676231384277 21 | 17407.651999999998225, 0.099550575017929077148 22 | 17407.854999999999563, 0.10062524676322937012 23 | 17408.065000000002328, 0.10244356840848922729 24 | 17408.257000000001426, 0.1049764975905418396 25 | 17408.449000000000524, 0.10756498575210571289 26 | 17408.663000000000466, 0.10968472808599472046 27 | 17408.854999999999563, 0.11320888251066207886 28 | 17409.052999999999884, 0.084244243800640106201 29 | 17409.255000000001019, 0.085451811552047729492 30 | 17409.454999999998108, 0.087957844138145446777 31 | 17409.663000000000466, 0.090042769908905029297 32 | 17409.85399999999936, 0.059477467089891433716 33 | 17410.06499999999869, 0.057743538171052932739 34 | 17410.259000000001834, 0.058337550610303878784 35 | 17410.451999999997497, 0.059208977967500686646 36 | 17410.662000000000262, 0.060052350163459777832 37 | 17410.85399999999936, 0.063340552151203155518 38 | 17411.054000000000087, 0.095616996288299560547 39 | 17411.263999999999214, 0.11642089486122131348 40 | 17411.458000000002357, 0.11817637830972671509 41 | 17411.651999999998225, 0.12024102360010147095 42 | 17411.863000000001193, 0.11921670287847518921 43 | 17412.054000000000087, 0.1111604347825050354 44 | 17412.253000000000611, 0.11505638062953948975 45 | 17412.461999999999534, 0.12186597287654876709 46 | 17412.652999999998428, 0.11007315665483474731 47 | 17412.847999999998137, 0.12105556577444076538 48 | 17413.058000000000902, 0.12310864031314849854 49 | 17413.262000000002445, 0.11727081984281539917 50 | 17413.462999999999738, 0.11988015472888946533 51 | 17413.661000000000058, 0.11600479483604431152 52 | 17413.863000000001193, 0.12593190371990203857 53 | 17414.05199999999968, 0.12311812490224838257 54 | 17414.252000000000407, 0.12684120237827301025 55 | 17414.445999999999913, 0.12521816790103912354 56 | 17414.655000000002474, 0.12885056436061859131 57 | 17414.854999999999563, 0.13760477304458618164 58 | 17415.052999999999884, 0.147466316819190979 59 | 17415.256000000001222, 0.1453123241662979126 60 | 17415.457999999998719, 0.15394061803817749023 61 | 17415.654999999998836, 0.15489238500595092773 62 | 17415.849000000001979, 0.16151468455791473389 63 | 17416.057000000000698, 0.17603980004787445068 64 | 17416.261999999998807, 0.17277921736240386963 65 | 17416.457000000002154, 0.17260833084583282471 66 | 17416.658999999999651, 0.1717691570520401001 67 | 17416.856999999999971, 0.17518050968647003174 68 | 17417.057999999997264, 0.17627367377281188965 69 | 17417.254999999997381, 0.17769002914428710938 70 | 17417.461999999999534, 0.18227688968181610107 71 | 17417.657999999999447, 0.18265312910079956055 72 | 17417.851999999998952, 0.17886953055858612061 73 | 17418.052999999999884, 0.17810682952404022217 74 | 17418.261999999998807, 0.34904548525810241699 75 | 17418.452000000001135, 0.35336628556251525879 76 | 17418.665000000000873, 0.35602816939353942871 77 | 17418.854999999999563, 0.33620893955230712891 78 | 17419.063000000001921, 0.33902302384376525879 79 | 17419.261999999998807, 0.32260611653327941895 80 | 17419.461999999999534, 0.30873009562492370605 81 | 17419.654999999998836, 0.31112086772918701172 82 | 17419.864000000001397, 0.32679286599159240723 83 | 17420.059000000001106, 0.32577967643737792969 84 | 17420.253000000000611, 0.31516149640083312988 85 | 17420.452000000001135, 0.3069553077220916748 86 | 17420.664000000000669, 0.30515843629837036133 87 | 17420.863000000001193, 0.31502872705459594727 88 | 17421.06000000000131, 0.5906441807746887207 89 | 17421.256999999997788, 0.57738846540451049805 90 | 17421.451000000000931, 0.55081415176391601563 91 | 17421.652999999998428, 0.5367367863655090332 92 | 17421.860000000000582, 0.53185063600540161133 93 | 17422.063999999998487, 0.52321010828018188477 94 | 17422.252000000000407, 0.50751143693923950195 95 | 17422.463999999999942, 0.47807523608207702637 96 | 17422.657999999999447, 0.47524309158325195313 97 | 17422.851999999998952, 0.45446997880935668945 98 | 17423.054000000000087, 0.44219708442687988281 99 | 17423.252000000000407, 0.4371388554573059082 100 | 17423.450000000000728, 0.41151750087738037109 101 | 17423.65400000000227, 0.42896565794944763184 102 | 17423.860000000000582, 0.44645079970359802246 103 | 17424.062999999998283, 0.42206531763076782227 104 | 17424.255999999997584, 0.43973630666732788086 105 | 17424.450000000000728, 0.41806465387344360352 106 | 17424.65599999999904, 0.40366667509078979492 107 | 17424.852999999999156, 0.39509558677673339844 108 | 17425.065000000002328, 0.38445177674293518066 109 | 17425.257000000001426, 0.37300726771354675293 110 | 17425.455999999998312, 0.34625411033630371094 111 | 17425.652999999998428, 0.33807477355003356934 112 | 17425.863000000001193, 0.32896894216537475586 113 | 17426.061999999998079, 0.33217781782150268555 114 | 17426.255000000001019, 0.32071653008460998535 115 | 17426.461999999999534, 0.3042627871036529541 116 | 17426.652000000001863, 0.29553636908531188965 117 | 17426.849000000001979, 0.55528330802917480469 118 | 17427.050999999999476, 0.54787874221801757813 119 | 17427.254999999997381, 0.54218041896820068359 120 | 17427.453999999997905, 0.55001693964004516602 121 | 17427.648000000001048, 0.53267180919647216797 122 | 17427.852999999999156, 0.51495367288589477539 123 | 17428.052999999999884, 0.49981668591499328613 124 | 17428.251000000000204, 0.8927014470100402832 125 | 17428.44800000000032, 0.86653321981430053711 126 | 17428.662000000000262, 0.88838148117065429688 127 | 17428.854999999999563, 0.86788165569305419922 128 | 17429.056000000000495, 0.82983613014221191406 129 | 17429.248999999999796, 0.84751629829406738281 130 | 17429.462999999999738, 0.82522302865982055664 131 | 17429.661000000000058, 0.89901262521743774414 132 | 17429.860000000000582, 0.87063318490982055664 133 | 17430.048999999999069, 0.88718283176422119141 134 | 17430.261999999998807, 0.90005820989608764648 135 | 17430.461999999999534, 0.87308806180953979492 136 | 17430.651000000001659, 0.85044950246810913086 137 | 17430.852999999999156, 0.15817256271839141846 138 | 17431.054000000000087, 0.14253008365631103516 139 | 17431.247999999999593, 0.12644885480403900146 140 | 17431.454000000001543, 0.12124214321374893188 141 | 17431.658999999999651, 0.11834982037544250488 142 | 17431.849000000001979, 0.41818290948867797852 143 | 17432.061999999998079, 0.4219589531421661377 144 | 17432.253000000000611, 0.41438668966293334961 145 | 17432.461999999999534, 0.4162152707576751709 146 | 17432.656999999999243, 0.62747335433959960938 147 | 17432.852999999999156, 0.62217432260513305664 148 | 17433.05199999999968, 0.60893815755844116211 149 | 17433.261999999998807, 0.60198444128036499023 150 | 17433.454000000001543, 0.60435968637466430664 151 | 17433.649000000001251, 0.60657137632369995117 152 | 17433.852999999999156, 0.61281335353851318359 153 | 17434.060999999997875, 0.59863924980163574219 154 | 17434.256999999997788, 0.93837374448776245117 155 | 17434.461999999999534, 0.92615348100662231445 156 | 17434.664000000000669, 0.90292322635650634766 157 | 17434.856999999999971, 0.89664793014526367188 158 | 17435.055000000000291, 0.88337826728820800781 159 | 17435.25, 0.83855271339416503906 160 | 17435.450000000000728, 0.83253920078277587891 161 | 17435.651999999998225, 0.81378155946731567383 162 | 17435.851999999998952, 0.82123023271560668945 163 | 17436.06499999999869, 0.82524061203002929688 164 | 17436.247999999999593, 0.81021529436111450195 165 | 17436.465000000000146, 0.53388601541519165039 166 | 17436.658999999999651, 0.50941097736358642578 167 | 17436.864000000001397, 0.50158488750457763672 168 | 17437.057000000000698, 0.47679901123046875 169 | 17437.247999999999593, 0.47730058431625366211 170 | 17437.456999999998516, 0.47338956594467163086 171 | 17437.652000000001863, 0.46418395638465881348 172 | 17437.861000000000786, 0.44962507486343383789 173 | 17438.061999999998079, 0.43684211373329162598 174 | 17438.255999999997584, 0.43686592578887939453 175 | 17438.44800000000032, 0.55030244588851928711 176 | 17438.665999999997439, 0.54106175899505615234 177 | 17438.863000000001193, 0.53005349636077880859 178 | 17439.058000000000902, 0.51451605558395385742 179 | 17439.263999999999214, 0.51813143491744995117 180 | 17439.451999999997497, 0.51068156957626342773 181 | 17439.664000000000669, 0.51120561361312866211 182 | 17439.86200000000099, 0.50065279006958007813 183 | 17440.052999999999884, 0.48977383971214294434 184 | 17440.252000000000407, 0.48551604151725769043 185 | 17440.455999999998312, 0.48680791258811950684 186 | 17440.663000000000466, 0.4783808290958404541 187 | 17440.859000000000378, 0.46812587976455688477 188 | 17441.062999999998283, 0.46002057194709777832 189 | 17441.252000000000407, 0.45202985405921936035 190 | 17441.451999999997497, 0.46458107233047485352 191 | 17441.662000000000262, 0.46435716748237609863 192 | 17441.86200000000099, 0.45350253582000732422 193 | 17442.057000000000698, 0.4413197934627532959 194 | 17442.258999999998196, 0.42520910501480102539 195 | 17442.466000000000349, 0.42357751727104187012 196 | 17442.656999999999243, 0.41510447859764099121 197 | 17442.855999999999767, 0.41367855668067932129 198 | 17443.061999999998079, 0.41575416922569274902 199 | 17443.259000000001834, 0.38932070136070251465 200 | 17443.461999999999534, 0.38497412204742431641 201 | 17443.665000000000873, 0.38570284843444824219 202 | 17443.856999999999971, 0.39117828011512756348 203 | 17444.059000000001106, 0.3905257880687713623 204 | 17444.25, 0.51385915279388427734 205 | 17444.450000000000728, 0.49568915367126464844 206 | 17444.663000000000466, 0.50002253055572509766 207 | 17444.855999999999767, 0.48251238465309143066 208 | 17445.048999999999069, 0.47166469693183898926 209 | 17445.253000000000611, 0.46055293083190917969 210 | 17445.467000000000553, 0.44701221585273742676 211 | 17445.654999999998836, 1.1375614404678344727 212 | 17445.86200000000099, 1.1030218601226806641 213 | 17446.054000000000087, 1.0787965059280395508 214 | 17446.254000000000815, 1.1042791604995727539 215 | 17446.46900000000096, 1.0733633041381835938 216 | 17446.666000000001077, 1.0425329208374023438 217 | 17446.867000000002008, 1.0072270631790161133 218 | 17447.064000000002125, 0.67961305379867553711 219 | 17447.261999999998807, 0.6578960418701171875 220 | 17447.459999999999127, 0.64462894201278686523 221 | 17447.667000000001281, 0.63891518115997314453 222 | 17447.851999999998952, 0.63344609737396240234 223 | 17448.063000000001921, 0.61124861240386962891 224 | 17448.252000000000407, 0.59214484691619873047 225 | 17448.465000000000146, 0.57953697443008422852 226 | 17448.653000000002066, 0.55888551473617553711 227 | 17448.861000000000786, 0.54000258445739746094 228 | 17449.052999999999884, 0.52482771873474121094 229 | 17449.261999999998807, 0.50452601909637451172 230 | 17449.461999999999534, 0.51983582973480224609 231 | 17449.662000000000262, 0.51233237981796264648 232 | 17449.86200000000099, 0.48804426193237304688 233 | 17450.056000000000495, 0.4740785062313079834 234 | 17450.252000000000407, 0.47352543473243713379 235 | 17450.47099999999773, 0.4540869295597076416 236 | 17450.665000000000873, 0.44069299101829528809 237 | 17450.863000000001193, 0.43784391880035400391 238 | 17451.061999999998079, 0.41498780250549316406 239 | 17451.261999999998807, 0.51778376102447509766 240 | 17451.461999999999534, 0.40991419553756713867 241 | 17451.658999999999651, 0.39732876420021057129 242 | 17451.85399999999936, 0.39641743898391723633 243 | 17452.063000000001921, 0.37963891029357910156 244 | 17452.26299999999901, 0.36461487412452697754 245 | 17452.453000000001339, 0.34943947196006774902 246 | 17452.664000000000669, 0.34165787696838378906 247 | 17452.859000000000378, 0.3284861147403717041 248 | 17453.052999999999884, 0.3137804865837097168 249 | 17453.253000000000611, 0.31006500124931335449 250 | 17453.463999999999942, 0.47298294305801391602 251 | 17453.662000000000262, 0.4473972618579864502 252 | 17453.86699999999837, 0.43554407358169555664 253 | 17454.066999999999098, 0.42041483521461486816 254 | 17454.259000000001834, 0.4133014380931854248 255 | 17454.466000000000349, 0.40114334225654602051 256 | 17454.672000000002299, 0.38516095280647277832 257 | 17454.868999999998778, 0.36724805831909179688 258 | 17455.069000000003143, 0.34993582963943481445 259 | 17455.261999999998807, 0.33301934599876403809 260 | 17455.45600000000195, 0.3266499936580657959 261 | 17455.673000000002503, 0.32540854811668395996 262 | 17455.863999999997759, 0.3027973473072052002 263 | 17456.066999999999098, 0.28651854395866394043 264 | 17456.271999999997206, 0.26967263221740722656 265 | 17456.472000000001572, 0.25811028480529785156 266 | 17456.665999999997439, 0.25283291935920715332 267 | 17456.856999999999971, 0.24925933778285980225 268 | 17457.06000000000131, 0.23775979876518249512 269 | 17457.257999999997992, 0.21609726548194885254 270 | 17457.472999999998137, 0.21411934494972229004 271 | 17457.66899999999805, 0.19618624448776245117 272 | 17457.862999999997555, 0.1851218491792678833 273 | 17458.062000000001717, 0.1801271289587020874 274 | 17458.273000000001048, 0.17096287012100219727 275 | 17458.472000000001572, 0.14846569299697875977 276 | 17458.663000000000466, 0.13189753890037536621 277 | 17458.870999999999185, 0.1264371722936630249 278 | 17459.057000000000698, 0.13084432482719421387 279 | 17459.268000000000029, 0.12205544859170913696 280 | 17459.462999999999738, 0.10546461492776870728 281 | 17459.666000000001077, 0.096079893410205841064 282 | 17459.872999999999593, 0.085019156336784362793 283 | 17460.063999999998487, 0.077596515417098999023 284 | 17460.257000000001426, 0.076574966311454772949 285 | 17460.465000000000146, 0.75790029764175415039 286 | -------------------------------------------------------------------------------- /data/errore_in.txt: -------------------------------------------------------------------------------- 1 | # /gazebo/link_states/recceipt_time, /filter_error/data 2 | 20191.212999999999738, 0.13273005187511444092 3 | 20191.407999999999447, 0.13342238962650299072 4 | 20191.601999999998952, 0.13479658961296081543 5 | 20191.809999999997672, 0.13623295724391937256 6 | 20192.003000000000611, 0.13700595498085021973 7 | 20192.197000000000116, 0.1383590400218963623 8 | 20192.408000000003085, 0.13982914388179779053 9 | 20192.614000000001397, 0.14130438864231109619 10 | 20192.810999999997875, 0.14356470108032226563 11 | 20193.01299999999901, 0.14551086723804473877 12 | 20193.213999999999942, 0.1474761664867401123 13 | 20193.404999999998836, 0.14874643087387084961 14 | 20193.601000000002387, 0.15037393569946289063 15 | 20193.806000000000495, 0.15211942791938781738 16 | 20193.996999999999389, 0.1534544825553894043 17 | 20194.204000000001543, 0.15457938611507415771 18 | 20194.412000000000262, 0.1553684622049331665 19 | 20194.604000000002998, 0.15542620420455932617 20 | 20194.812999999998283, 0.15483357012271881104 21 | 20195.001000000000204, 0.15401588380336761475 22 | 20195.204999999998108, 0.15339490771293640137 23 | 20195.409000000003289, 0.15387080609798431396 24 | 20195.613000000001193, 0.15541222691535949707 25 | 20195.808000000000902, 0.15704372525215148926 26 | 20196.003000000000611, 0.15870997309684753418 27 | 20196.202999999997701, 0.16079695522785186768 28 | 20196.413000000000466, 0.16346538066864013672 29 | 20196.601999999998952, 0.16627411544322967529 30 | 20196.812999999998283, 0.169841766357421875 31 | 20196.998999999999796, 0.17784191668033599854 32 | 20197.209000000002561, 0.18141593039035797119 33 | 20197.398999999997613, 0.19025258719921112061 34 | 20197.609000000000378, 0.19459140300750732422 35 | 20197.796999999998661, 0.20566941797733306885 36 | 20198.005999999997584, 0.21068938076496124268 37 | 20198.197000000000116, 0.22319220006465911865 38 | 20198.406999999999243, 0.22956395149230957031 39 | 20198.609999999996944, 0.23904252052307128906 40 | 20198.802999999999884, 0.24799069762229919434 41 | 20199.013999999999214, 0.25278621912002563477 42 | 20199.199000000000524, 0.26916840672492980957 43 | 20199.412000000000262, 0.27073428034782409668 44 | 20199.610999999997148, 0.28368404507637023926 45 | 20199.805000000000291, 0.29312032461166381836 46 | 20199.998999999999796, 0.30611449480056762695 47 | 20200.207999999998719, 0.31191450357437133789 48 | 20200.398000000001048, 0.32581481337547302246 49 | 20200.601999999998952, 0.33262899518013000488 50 | 20200.811000000001513, 0.3445517122745513916 51 | 20200.997999999999593, 0.36061212420463562012 52 | 20201.205000000001746, 0.36877751350402832031 53 | 20201.408999999999651, 0.38204428553581237793 54 | 20201.614000000001397, 0.39156284928321838379 55 | 20201.812000000001717, 0.40553641319274902344 56 | 20201.997999999999593, 0.42830100655555725098 57 | 20202.211999999999534, 0.4398123621940612793 58 | 20202.406999999999243, 0.45591583847999572754 59 | 20202.613999999997759, 0.46752598881721496582 60 | 20202.800999999999476, 0.49106717109680175781 61 | 20203.006999999997788, 0.50256860256195068359 62 | 20203.213999999999942, 0.51510977745056152344 63 | 20203.402999999998428, 0.53547674417495727539 64 | 20203.609000000000378, 0.55228883028030395508 65 | 20203.813000000001921, 0.56626206636428833008 66 | 20204.005000000001019, 0.58786356449127197266 67 | 20204.213999999999942, 0.60290229320526123047 68 | 20204.406999999999243, 0.62431347370147705078 69 | 20204.610000000000582, 0.64286410808563232422 70 | 20204.804000000000087, 0.66007506847381591797 71 | 20205.009000000001834, 0.67685091495513916016 72 | 20205.216000000000349, 0.6902497410774230957 73 | 20205.401000000001659, 0.71280348300933837891 74 | 20205.601999999998952, 0.7286739349365234375 75 | 20205.806999999997061, 0.74117457866668701172 76 | 20205.996999999999389, 0.75979101657867431641 77 | 20206.202000000001135, 0.77264493703842163086 78 | 20206.40599999999904, 0.78802472352981567383 79 | 20206.61200000000099, 0.80138123035430908203 80 | 20206.799999999999272, 0.82291507720947265625 81 | 20207.011999999998807, 0.83438020944595336914 82 | 20207.204999999998108, 0.85315877199172973633 83 | 20207.415000000000873, 0.86746239662170410156 84 | 20207.605999999999767, 0.88724571466445922852 85 | 20207.810999999997875, 0.90466231107711791992 86 | 20208.003000000000611, 0.92434239387512207031 87 | 20208.211999999999534, 0.94113308191299438477 88 | 20208.403000000002066, 0.96016359329223632813 89 | 20208.613999999997759, 0.9780070185661315918 90 | 20208.805000000000291, 0.99917370080947875977 91 | 20209.011999999998807, 1.0172196626663208008 92 | 20209.19999999999709, 1.0384823083877563477 93 | 20209.413000000000466, 1.0558176040649414063 94 | 20209.597999999998137, 1.0782148838043212891 95 | 20209.80199999999968, 1.0961605310440063477 96 | 20210.003000000000611, 1.1151995658874511719 97 | 20210.211999999999534, 1.1338759660720825195 98 | 20210.404999999998836, 1.1522208452224731445 99 | 20210.61200000000099, 1.1695604324340820313 100 | 20210.804000000000087, 1.186858057975769043 101 | 20211.001000000000204, 1.2047019004821777344 102 | 20211.199000000000524, 1.2225008010864257813 103 | 20211.406999999999243, 1.2412534952163696289 104 | 20211.613000000001193, 1.2605253458023071289 105 | 20211.797999999998865, 1.2787840366363525391 106 | 20212.004000000000815, 1.2972366809844970703 107 | 20212.211999999999534, 1.3152235746383666992 108 | 20212.402999999998428, 1.3316496610641479492 109 | 20212.605999999999767, 1.3482120037078857422 110 | 20212.812000000001717, 1.3648737668991088867 111 | 20213.002000000000407, 1.3793404102325439453 112 | 20213.208999999998923, 1.3956699371337890625 113 | 20213.398999999997613, 1.4103142023086547852 114 | 20213.608000000000175, 1.4264819622039794922 115 | 20213.797999999998865, 1.4397181272506713867 116 | 20214.002000000000407, 1.455767512321472168 117 | 20214.212000000003172, 1.472747802734375 118 | 20214.400999999998021, 1.4845460653305053711 119 | 20214.612999999997555, 1.5031170845031738281 120 | 20214.80199999999968, 1.5128738880157470703 121 | 20215.011999999998807, 1.5294282436370849609 122 | 20215.204999999998108, 1.5438473224639892578 123 | 20215.415000000000873, 1.559234619140625 124 | 20215.601999999998952, 1.5709291696548461914 125 | 20215.811999999998079, 1.5852646827697753906 126 | 20216.002000000000407, 1.5968263149261474609 127 | 20216.203000000001339, 1.6135363578796386719 128 | 20216.407999999999447, 1.6277335882186889648 129 | 20216.61200000000099, 1.6446703672409057617 130 | 20216.807000000000698, 1.6554243564605712891 131 | 20217.015000000003056, 1.6715210676193237305 132 | 20217.203000000001339, 1.6813036203384399414 133 | 20217.413000000000466, 1.6985071897506713867 134 | 20217.608000000000175, 1.7084642648696899414 135 | 20217.802999999999884, 1.7228547334671020508 136 | 20218.014999999999418, 1.7415499687194824219 137 | 20218.211999999999534, 1.7534137964248657227 138 | 20218.413000000000466, 1.7733496427536010742 139 | 20218.597999999998137, 1.7818289995193481445 140 | 20218.798999999999069, 1.7898424863815307617 141 | 20219.006000000001222, 1.8178633451461791992 142 | 20219.215000000000146, 1.8378676176071166992 143 | 20219.40599999999904, 1.8482930660247802734 144 | 20219.614000000001397, 1.8676617145538330078 145 | 20219.80199999999968, 1.8781936168670654297 146 | 20220.010999999998603, 1.8933024406433105469 147 | 20220.203000000001339, 1.908742070198059082 148 | 20220.407999999999447, 1.9242434501647949219 149 | 20220.598000000001775, 1.9343168735504150391 150 | 20220.805000000000291, 1.9547914266586303711 151 | 20221.01299999999901, 1.9740972518920898438 152 | 20221.202999999997701, 1.9835878610610961914 153 | 20221.408000000003085, 1.997694849967956543 154 | 20221.614000000001397, 2.0163843631744384766 155 | 20221.799999999999272, 2.0197701454162597656 156 | 20222.00800000000163, 2.0380706787109375 157 | 20222.213999999999942, 2.0572662353515625 158 | 20222.402000000001863, 2.06561279296875 159 | 20222.614999999997963, 2.0844960212707519531 160 | 20222.809000000001106, 2.0930330753326416016 161 | 20222.996999999999389, 2.1009297370910644531 162 | 20223.208999999998923, 2.1201655864715576172 163 | 20223.400999999998021, 2.1279904842376708984 164 | 20223.613000000001193, 2.1519727706909179688 165 | 20223.804000000000087, 2.1587316989898681641 166 | 20223.997999999999593, 2.1648459434509277344 167 | 20224.208000000002357, 2.1828978061676025391 168 | 20224.398000000001048, 2.1901571750640869141 169 | 20224.608000000000175, 2.207660675048828125 170 | 20224.811000000001513, 2.2193942070007324219 171 | 20225.001999999996769, 2.2253015041351318359 172 | 20225.208999999998923, 2.2414286136627197266 173 | 20225.414999999997235, 2.2569000720977783203 174 | 20225.605999999999767, 2.2622652053833007813 175 | 20225.811999999998079, 2.2787106037139892578 176 | 20226.001000000000204, 2.2786257266998291016 177 | 20226.207999999998719, 2.2946012020111083984 178 | 20226.397000000000844, 2.2994027137756347656 179 | 20226.604999999999563, 2.3139142990112304688 180 | 20226.813000000001921, 2.3281373977661132813 181 | 20227.01299999999901, 2.3377449512481689453 182 | 20227.203000000001339, 2.3421993255615234375 183 | 20227.40599999999904, 2.3367238044738769531 184 | 20227.613999999997759, 2.3651144504547119141 185 | 20227.802999999999884, 2.3696866035461425781 186 | 20228.010999999998603, 2.3792090415954589844 187 | 20228.203000000001339, 2.3886241912841796875 188 | 20228.411000000000058, 2.3988282680511474609 189 | 20228.597999999998137, 2.4043178558349609375 190 | 20228.812000000001717, 2.4225890636444091797 191 | 20229.002000000000407, 2.4229071140289306641 192 | 20229.210999999999331, 2.4420559406280517578 193 | 20229.399999999997817, 2.4428310394287109375 194 | 20229.609000000000378, 2.4572346210479736328 195 | 20229.799000000002707, 2.4634130001068115234 196 | 20230.003000000000611, 2.4775743484497070313 197 | 20230.211999999999534, 2.4915947914123535156 198 | 20230.396999999997206, 2.4935235977172851563 199 | 20230.601999999998952, 2.5035636425018310547 200 | 20230.809000000001106, 2.5181717872619628906 201 | 20231.014000000002852, 2.5318615436553955078 202 | 20231.204000000001543, 2.5382068157196044922 203 | 20231.397000000000844, 2.5444414615631103516 204 | 20231.601999999998952, 2.5567495822906494141 205 | 20231.809999999997672, 2.5657098293304443359 206 | 20232.008999999998196, 2.5750458240509033203 207 | 20232.19800000000032, 2.5828638076782226563 208 | 20232.40599999999904, 2.5950031280517578125 209 | 20232.598999999998341, 2.601764678955078125 210 | 20232.812000000001717, 2.6162028312683105469 211 | 20232.997000000003027, 2.6199097633361816406 212 | 20233.205000000001746, 2.6310491561889648438 213 | 20233.401999999998225, 2.638950347900390625 214 | 20233.614000000001397, 2.6518549919128417969 215 | 20233.805000000000291, 2.6597626209259033203 216 | 20234.011999999998807, 2.6710338592529296875 217 | 20234.200999999997293, 2.6768767833709716797 218 | 20234.413000000000466, 2.6873846054077148438 219 | 20234.606999999999971, 2.6954884529113769531 220 | 20234.812000000001717, 2.7040705680847167969 221 | 20235.004000000000815, 2.7128033638000488281 222 | 20235.197000000000116, 2.7202608585357666016 223 | 20235.404999999998836, 2.7295081615447998047 224 | 20235.610999999997148, 2.7374067306518554688 225 | 20235.80199999999968, 2.7442994117736816406 226 | 20236.011999999998807, 2.7513370513916015625 227 | 20236.197000000000116, 2.7588646411895751953 228 | 20236.406000000002678, 2.7669816017150878906 229 | 20236.613000000001193, 2.7749419212341308594 230 | 20236.805000000000291, 2.7827696800231933594 231 | 20236.996999999999389, 2.7915482521057128906 232 | 20237.20600000000195, 2.7978663444519042969 233 | 20237.412000000000262, 2.8047323226928710938 234 | 20237.601000000002387, 2.8139503002166748047 235 | 20237.812000000001717, 2.820892333984375 236 | 20238.006999999997788, 2.8283209800720214844 237 | 20238.19800000000032, 2.8375151157379150391 238 | 20238.402999999998428, 2.8421790599822998047 239 | 20238.609000000000378, 2.8495349884033203125 240 | 20238.797999999998865, 2.8587598800659179688 241 | 20239.005000000001019, 2.8648087978363037109 242 | 20239.211999999999534, 2.8695914745330810547 243 | 20239.402999999998428, 2.8824687004089355469 244 | 20239.597000000001572, 2.8887486457824707031 245 | 20239.80199999999968, 2.8941409587860107422 246 | 20240.008999999998196, 2.9021191596984863281 247 | 20240.212999999999738, 2.9069926738739013672 248 | 20240.400999999998021, 2.9213855266571044922 249 | 20240.609000000000378, 2.9271388053894042969 250 | 20240.802999999999884, 2.9364371299743652344 251 | 20241.005999999997584, 2.9465913772583007813 252 | 20241.199000000000524, 2.960750579833984375 253 | 20241.404999999998836, 2.9678771495819091797 254 | 20241.606999999999971, 2.9785740375518798828 255 | 20241.812000000001717, 2.9896523952484130859 256 | 20242.005999999997584, 3.0010786056518554688 257 | 20242.211999999999534, 3.0095729827880859375 258 | 20242.399999999997817, 3.0284616947174072266 259 | 20242.60399999999936, 3.0377652645111083984 260 | 20242.80199999999968, 3.0543527603149414063 261 | 20243.009000000001834, 3.0622246265411376953 262 | 20243.19999999999709, 3.0780072212219238281 263 | 20243.408999999999651, 3.0854229927062988281 264 | 20243.597999999998137, 3.0999929904937744141 265 | 20243.80199999999968, 3.1062369346618652344 266 | 20244.011999999998807, 3.1162278652191162109 267 | 20244.202000000001135, 3.1254131793975830078 268 | 20244.411000000000058, 3.1360831260681152344 269 | 20244.602999999999156, 3.1473393440246582031 270 | 20244.802999999999884, 3.1584038734436035156 271 | 20245.006000000001222, 3.1701576709747314453 272 | 20245.19800000000032, 3.1859469413757324219 273 | 20245.401000000001659, 3.196804046630859375 274 | 20245.605999999999767, 3.2033884525299072266 275 | 20245.809999999997672, 3.2141320705413818359 276 | 20245.997999999999593, 3.2284557819366455078 277 | 20246.211999999999534, 3.2306177616119384766 278 | 20246.40400000000227, 3.2448639869689941406 279 | 20246.610000000000582, 3.2543935775756835938 280 | 20246.811999999998079, 3.2589159011840820313 281 | 20247.003000000000611, 3.2717237472534179688 282 | 20247.212999999999738, 3.2750785350799560547 283 | 20247.403000000002066, 3.2866826057434082031 284 | 20247.613000000001193, 3.2893438339233398438 285 | 20247.80199999999968, 3.3018095493316650391 286 | 20248.009999999998399, 3.3095374107360839844 287 | 20248.200000000000728, 3.3209497928619384766 288 | 20248.413000000000466, 3.3194103240966796875 289 | 20248.604999999999563, 3.331432342529296875 290 | 20248.812999999998283, 3.33400726318359375 291 | 20249.002000000000407, 3.3455660343170166016 292 | 20249.208999999998923, 3.3516793251037597656 293 | 20249.408999999999651, 3.3568072319030761719 294 | 20249.599000000001979, 3.3663928508758544922 295 | 20249.812999999998283, 3.3679127693176269531 296 | 20250.001999999996769, 3.3733670711517333984 297 | 20250.212999999999738, 3.3745634555816650391 298 | 20250.402999999998428, 3.3839101791381835938 299 | 20250.614000000001397, 3.3854603767395019531 300 | 20250.808000000000902, 3.3953418731689453125 301 | 20251.003000000000611, 3.4017057418823242188 302 | 20251.210999999999331, 3.4079630374908447266 303 | 20251.408999999999651, 3.4139254093170166016 304 | 20251.613000000001193, 3.4160065650939941406 305 | 20251.802999999999884, 3.4255881309509277344 306 | 20252.013000000002648, 3.4276328086853027344 307 | 20252.202000000001135, 3.4362506866455078125 308 | 20252.411999999996624, 3.4418363571166992188 309 | 20252.602999999999156, 3.4474015235900878906 310 | 20252.813000000001921, 3.4525997638702392578 311 | 20253.004000000000815, 3.4569363594055175781 312 | 20253.212999999999738, 3.4588396549224853516 313 | 20253.398000000001048, 3.4691913127899169922 314 | 20253.603000000002794, 3.471195220947265625 315 | 20253.813999999998487, 3.4729573726654052734 316 | 20254.008999999998196, 3.4793376922607421875 317 | 20254.205999999998312, 3.4828584194183349609 318 | 20254.407999999999447, 3.4863348007202148438 319 | 20254.604999999999563, 3.4899199008941650391 320 | 20254.800999999999476, 3.4938509464263916016 321 | 20255.006999999997788, 3.4972701072692871094 322 | -------------------------------------------------------------------------------- /data/errore_shaf.txt: -------------------------------------------------------------------------------- 1 | # /gazebo/link_states/recceipt_time, /filter_error/data 2 | 16936.686000000001513, 0.1164553910493850708 3 | 16936.891999999999825, 0.11644693464040756226 4 | 16937.11200000000099, 0.1161373332142829895 5 | 16937.294000000001688, 0.10169070959091186523 6 | 16937.486999999997352, 0.070607006549835205078 7 | 16937.704000000001543, 0.069847434759140014648 8 | 16937.897000000000844, 0.064382344484329223633 9 | 16938.093000000000757, 0.057836018502712249756 10 | 16938.296999999998661, 0.080035991966724395752 11 | 16938.496000000002823, 0.083012089133262634277 12 | 16938.688999999998487, 0.063474781811237335205 13 | 16938.887000000002445, 0.064939260482788085938 14 | 16939.100000000002183, 0.05547490343451499939 15 | 16939.295000000001892, 0.032832175493240356445 16 | 16939.502000000000407, 0.023178603500127792358 17 | 16939.693999999999505, 0.041931401938199996948 18 | 16939.897000000000844, 0.015720264986157417297 19 | 16940.092000000000553, 0.01205022912472486496 20 | 16940.292999999997846, 0.016325891017913818359 21 | 16940.493999999998778, 0.037383370101451873779 22 | 16940.68999999999869, 0.026611359789967536926 23 | 16940.891999999999825, 0.028240390121936798096 24 | 16941.100999999998749, 0.025205299258232116699 25 | 16941.29399999999805, 0.026826735585927963257 26 | 16941.493999999998778, 0.026600353419780731201 27 | 16941.69800000000032, 0.038615748286247253418 28 | 16941.893000000000029, 0.065643228590488433838 29 | 16942.088999999999942, 0.054998181760311126709 30 | 16942.302000000003318, 0.069884680211544036865 31 | 16942.497999999999593, 0.07983433455228805542 32 | 16942.687999999998283, 0.063083864748477935791 33 | 16942.893000000000029, 0.065599560737609863281 34 | 16943.092000000000553, 0.06900069117546081543 35 | 16943.290000000000873, 0.064597524702548980713 36 | 16943.489000000001397, 0.074106551706790924072 37 | 16943.690999999998894, 0.042846765369176864624 38 | 16943.899000000001251, 0.070229604840278625488 39 | 16944.097999999998137, 0.075836263597011566162 40 | 16944.294000000001688, 0.060808159410953521729 41 | 16944.491000000001804, 0.049734026193618774414 42 | 16944.695999999999913, 0.047820806503295898438 43 | 16944.891999999999825, 0.050165630877017974854 44 | 16945.086999999999534, 0.043599139899015426636 45 | 16945.30199999999968, 0.023227792233228683472 46 | 16945.488000000001193, 0.023426465690135955811 47 | 16945.691999999999098, 0.034535463899374008179 48 | 16945.891999999999825, 0.030205035582184791565 49 | 16946.091000000000349, 0.031738370656967163086 50 | 16946.292999999997846, 0.034714974462985992432 51 | 16946.489000000001397, 0.026588996872305870056 52 | 16946.703000000001339, 0.035934869199991226196 53 | 16946.902000000001863, 0.056350886821746826172 54 | 16947.098000000001775, 0.057984873652458190918 55 | 16947.29399999999805, 0.047788679599761962891 56 | 16947.490999999998166, 0.038153115659952163696 57 | 16947.692999999999302, 0.028466267511248588562 58 | 16947.901999999998225, 0.03249816596508026123 59 | 16948.096000000001368, 0.034752503037452697754 60 | 16948.290999999997439, 0.045512091368436813354 61 | 16948.492000000002008, 0.11120362579822540283 62 | 16948.701000000000931, 0.10227373242378234863 63 | 16948.900999999998021, 0.1008203551173210144 64 | 16949.102999999999156, 0.11726002395153045654 65 | 16949.300999999999476, 0.10996278375387191772 66 | 16949.502000000000407, 0.12204651534557342529 67 | 16949.685999999997875, 0.12837006151676177979 68 | 16949.898000000001048, 0.15000560879707336426 69 | 16950.093000000000757, 0.1409588158130645752 70 | 16950.290000000000873, 0.13973206281661987305 71 | 16950.498999999999796, 0.14331836998462677002 72 | 16950.688000000001921, 0.14679256081581115723 73 | 16950.903999999998632, 0.14950636029243469238 74 | 16951.09400000000096, 0.1383714526891708374 75 | 16951.289000000000669, 0.14639046788215637207 76 | 16951.490000000001601, 0.14928145706653594971 77 | 16951.692999999999302, 0.14842125773429870605 78 | 16951.893000000000029, 0.14312559366226196289 79 | 16952.102999999999156, 0.14366276562213897705 80 | 16952.294000000001688, 0.11751458793878555298 81 | 16952.493999999998778, 0.1448014378547668457 82 | 16952.703000000001339, 0.126570090651512146 83 | 16952.891999999999825, 0.10105154663324356079 84 | 16953.09599999999773, 0.097468934953212738037 85 | 16953.294999999998254, 0.077972240746021270752 86 | 16953.498999999999796, 0.074070751667022705078 87 | 16953.704000000001543, 0.069481663405895233154 88 | 16953.915000000000873, 0.071996793150901794434 89 | 16954.098000000001775, 0.07925395667552947998 90 | 16954.299999999999272, 0.10119685530662536621 91 | 16954.503000000000611, 0.10636712610721588135 92 | 16954.693999999999505, 0.1145494505763053894 93 | 16954.88799999999901, 0.11934675276279449463 94 | 16955.095000000001164, 0.12151139974594116211 95 | 16955.30199999999968, 0.12160571664571762085 96 | 16955.501000000000204, 0.125341072678565979 97 | 16955.695999999999913, 0.13773289322853088379 98 | 16955.898000000001048, 0.13367225229740142822 99 | 16956.088999999999942, 0.13215178251266479492 100 | 16956.287000000000262, 0.12099161744117736816 101 | 16956.503000000000611, 0.14587482810020446777 102 | 16956.691999999999098, 0.1447124183177947998 103 | 16956.897000000000844, 0.13486303389072418213 104 | 16957.092999999997119, 0.12663242220878601074 105 | 16957.302999999999884, 0.12239130586385726929 106 | 16957.495999999999185, 0.11058067530393600464 107 | 16957.702999999997701, 0.10406416654586791992 108 | 16957.904999999998836, 0.096129335463047027588 109 | 16958.099000000001979, 0.097104884684085845947 110 | 16958.297999999998865, 0.091543935239315032959 111 | 16958.496999999999389, 0.085012555122375488281 112 | 16958.692999999999302, 0.083519101142883300781 113 | 16958.902000000001863, 0.078034199774265289307 114 | 16959.091999999996915, 0.097728490829467773438 115 | 16959.287000000000262, 0.11115133762359619141 116 | 16959.502000000000407, 0.10986234992742538452 117 | 16959.702000000001135, 0.099150918424129486084 118 | 16959.900999999998021, 0.098255567252635955811 119 | 16960.093000000000757, 0.11587198078632354736 120 | 16960.302000000003318, 0.11126679182052612305 121 | 16960.495999999999185, 0.11537578701972961426 122 | 16960.695999999999913, 0.10482668131589889526 123 | 16960.902999999998428, 0.11717066168785095215 124 | 16961.090000000000146, 0.10497405380010604858 125 | 16961.302999999999884, 0.11261841654777526855 126 | 16961.495000000002619, 0.11496160924434661865 127 | 16961.693000000002939, 0.11739989370107650757 128 | 16961.890999999999622, 0.11460200697183609009 129 | 16962.092000000000553, 0.099669888615608215332 130 | 16962.289000000000669, 0.096397250890731811523 131 | 16962.503000000000611, 0.10111892968416213989 132 | 16962.701999999997497, 0.10506211221218109131 133 | 16962.900000000001455, 0.11890005320310592651 134 | 16963.091000000000349, 0.1177075803279876709 135 | 16963.294000000001688, 0.11158072203397750854 136 | 16963.497999999999593, 0.13079507648944854736 137 | 16963.691999999999098, 0.10900219529867172241 138 | 16963.886000000002241, 0.1192480921745300293 139 | 16964.086999999999534, 0.11976580321788787842 140 | 16964.292999999997846, 0.11718100309371948242 141 | 16964.493999999998778, 0.11569852381944656372 142 | 16964.686999999998079, 0.11263761669397354126 143 | 16964.888999999999214, 0.12324897944927215576 144 | 16965.097000000001572, 0.10943692177534103394 145 | 16965.302999999999884, 0.11522803455591201782 146 | 16965.497999999999593, 0.10639709234237670898 147 | 16965.701000000000931, 0.11847965419292449951 148 | 16965.903000000002066, 0.12066070735454559326 149 | 16966.093000000000757, 0.11712694913148880005 150 | 16966.300999999999476, 0.10065912455320358276 151 | 16966.497999999999593, 0.10624514520168304443 152 | 16966.691999999999098, 0.098594583570957183838 153 | 16966.902999999998428, 0.086249113082885742188 154 | 16967.090000000000146, 0.064745217561721801758 155 | 16967.287000000000262, 0.072452664375305175781 156 | 16967.488000000001193, 0.078387871384620666504 157 | 16967.691999999999098, 0.081792764365673065186 158 | 16967.897000000000844, 0.092305481433868408203 159 | 16968.093000000000757, 0.088648319244384765625 160 | 16968.287000000000262, 0.090643346309661865234 161 | 16968.496999999999389, 0.094076082110404968262 162 | 16968.692999999999302, 0.076976053416728973389 163 | 16968.901999999998225, 0.097758337855339050293 164 | 16969.092000000000553, 0.092690691351890563965 165 | 16969.30199999999968, 0.10386617481708526611 166 | 16969.5, 0.10021848231554031372 167 | 16969.697000000000116, 0.0973757132887840271 168 | 16969.888999999999214, 0.10538855195045471191 169 | 16970.10399999999936, 0.11013892292976379395 170 | 16970.288000000000466, 0.10641685128211975098 171 | 16970.512000000002445, 0.12508772313594818115 172 | 16970.697000000000116, 0.13378834724426269531 173 | 16970.900000000001455, 0.14676928520202636719 174 | 16971.105999999999767, 0.14756509661674499512 175 | 16971.292000000001281, 0.14700876176357269287 176 | 16971.488000000001193, 0.16064742207527160645 177 | 16971.688999999998487, 0.1657657325267791748 178 | 16971.901999999998225, 0.16937847435474395752 179 | 16972.09400000000096, 0.16449150443077087402 180 | 16972.302999999999884, 0.17696274816989898682 181 | 16972.492000000002008, 0.16240142285823822021 182 | 16972.704000000001543, 0.18139243125915527344 183 | 16972.901000000001659, 0.17091950774192810059 184 | 16973.091000000000349, 0.15573620796203613281 185 | 16973.300999999999476, 0.150493621826171875 186 | 16973.494999999998981, 0.14361496269702911377 187 | 16973.68999999999869, 0.15088346600532531738 188 | 16973.891999999999825, 0.1590856015682220459 189 | 16974.092000000000553, 0.16264753043651580811 190 | 16974.295000000001892, 0.18686604499816894531 191 | 16974.501999999996769, 0.18536122143268585205 192 | 16974.691000000002532, 0.1799713820219039917 193 | 16974.902000000001863, 0.18484650552272796631 194 | 16975.096999999997934, 0.18133153021335601807 195 | 16975.30199999999968, 0.18967577815055847168 196 | 16975.504000000000815, 0.18298941850662231445 197 | 16975.702000000001135, 0.16459819674491882324 198 | 16975.889000000002852, 0.1752926558256149292 199 | 16976.10200000000259, 0.1596784740686416626 200 | 16976.302000000003318, 0.12734265625476837158 201 | 16976.5, 0.12386363744735717773 202 | 16976.693999999999505, 0.1181754767894744873 203 | 16976.905000000002474, 0.1249490007758140564 204 | 16977.10200000000259, 0.12062967568635940552 205 | 16977.305000000000291, 0.11990474164485931396 206 | 16977.506000000001222, 0.13018287718296051025 207 | 16977.702000000001135, 0.1235911250114440918 208 | 16977.894000000000233, 0.11832883208990097046 209 | 16978.088999999999942, 0.11933419108390808105 210 | 16978.296999999998661, 0.13312864303588867188 211 | 16978.490999999998166, 0.13141582906246185303 212 | 16978.693000000002939, 0.12947687506675720215 213 | 16978.890999999999622, 0.14581277966499328613 214 | 16979.103000000002794, 0.14104913175106048584 215 | 16979.297999999998865, 0.12931366264820098877 216 | 16979.503000000000611, 0.14088243246078491211 217 | 16979.697000000000116, 0.18789106607437133789 218 | 16979.891999999999825, 0.18114823102951049805 219 | 16980.093000000000757, 0.18329454958438873291 220 | 16980.289000000000669, 0.16259594261646270752 221 | 16980.492999999998574, 0.16101980209350585938 222 | 16980.705999999998312, 0.1563615947961807251 223 | 16980.902000000001863, 0.16827099025249481201 224 | 16981.093000000000757, 0.17710037529468536377 225 | 16981.304000000000087, 0.19015130400657653809 226 | 16981.503000000000611, 0.18605889379978179932 227 | 16981.688999999998487, 0.18722113966941833496 228 | 16981.88799999999901, 0.18349808454513549805 229 | 16982.10399999999936, 0.20707173645496368408 230 | 16982.30199999999968, 0.18926629424095153809 231 | 16982.493999999998778, 0.18544349074363708496 232 | 16982.690999999998894, 0.18697920441627502441 233 | 16982.893000000000029, 0.17987988889217376709 234 | 16983.104999999999563, 0.18016541004180908203 235 | 16983.293000000001484, 0.17872290313243865967 236 | 16983.504000000000815, 0.19150769710540771484 237 | 16983.70600000000195, 0.19449010491371154785 238 | 16983.893000000000029, 0.18129496276378631592 239 | 16984.097000000001572, 0.18499480187892913818 240 | 16984.298000000002503, 0.17018014192581176758 241 | 16984.492999999998574, 0.17059215903282165527 242 | 16984.701999999997497, 0.17522229254245758057 243 | 16984.894000000000233, 0.17006269097328186035 244 | 16985.094999999997526, 0.16477699577808380127 245 | 16985.302999999999884, 0.16184560954570770264 246 | 16985.502000000000407, 0.15160274505615234375 247 | 16985.701000000000931, 0.13690276443958282471 248 | 16985.908000000003085, 0.15868066251277923584 249 | 16986.105999999999767, 0.16428747773170471191 250 | 16986.309999999997672, 0.14935478568077087402 251 | 16986.496999999999389, 0.13195908069610595703 252 | 16986.710999999999331, 0.14315691590309143066 253 | 16986.911000000000058, 0.14421123266220092773 254 | 16987.099000000001979, 0.14077998697757720947 255 | 16987.313000000001921, 0.16174225509166717529 256 | 16987.504000000000815, 0.1469281613826751709 257 | 16987.701000000000931, 0.15329247713088989258 258 | 16987.902000000001863, 0.15556859970092773438 259 | 16988.099999999998545, 0.16071133315563201904 260 | 16988.300999999999476, 0.16161067783832550049 261 | 16988.509999999998399, 0.16653360426425933838 262 | 16988.701000000000931, 0.15901368856430053711 263 | 16988.906999999999243, 0.1710147559642791748 264 | 16989.111000000000786, 0.16582325100898742676 265 | 16989.305999999996857, 0.17092804610729217529 266 | 16989.51299999999901, 0.17735052108764648438 267 | 16989.709000000002561, 0.1868553459644317627 268 | 16989.922999999998865, 0.19142000377178192139 269 | 16990.122999999999593, 0.19922964274883270264 270 | 16990.305000000000291, 0.20039300620555877686 271 | 16990.525000000001455, 0.19364203512668609619 272 | 16990.722000000001572, 0.1922661513090133667 273 | 16990.921999999998661, 0.2056379169225692749 274 | 16991.119999999998981, 0.21406443417072296143 275 | 16991.32300000000032, 0.21639907360076904297 276 | 16991.519000000000233, 0.2019144594669342041 277 | 16991.722000000001572, 0.19915431737899780273 278 | 16991.91899999999805, 0.19226638972759246826 279 | 16992.122999999999593, 0.19808243215084075928 280 | 16992.32300000000032, 0.20088186860084533691 281 | 16992.52299999999741, 0.17843168973922729492 282 | 16992.717000000000553, 0.17427651584148406982 283 | 16992.911000000000058, 0.18687203526496887207 284 | 16993.109000000000378, 0.17454396188259124756 285 | 16993.311000000001513, 0.18686462938785552979 286 | 16993.523000000001048, 0.18111906945705413818 287 | 16993.705999999998312, 0.1684620976448059082 288 | 16993.915000000000873, 0.17334318161010742188 289 | 16994.112999999997555, 0.16841894388198852539 290 | 16994.322000000000116, 0.16104824841022491455 291 | 16994.519000000000233, 0.15156464278697967529 292 | 16994.711999999999534, 0.15436831116676330566 293 | 16994.919999999998254, 0.1534312516450881958 294 | 16995.148000000001048, 0.14615423977375030518 295 | 16995.316999999999098, 0.1266025155782699585 296 | 16995.519000000000233, 0.15201523900032043457 297 | 16995.713999999999942, 0.1632785797119140625 298 | 16995.909999999999854, 0.1744314730167388916 299 | 16996.122999999999593, 0.17452444136142730713 300 | 16996.313000000001921, 0.15551988780498504639 301 | 16996.515999999999622, 0.16335302591323852539 302 | 16996.70600000000195, 0.15792773663997650146 303 | 16996.908000000003085, 0.16398319602012634277 304 | 16997.108000000000175, 0.16667973995208740234 305 | 16997.32300000000032, 0.17466585338115692139 306 | 16997.51299999999901, 0.17841053009033203125 307 | 16997.726000000002387, 0.19296254217624664307 308 | 16997.921999999998661, 0.20509195327758789063 309 | 16998.113000000001193, 0.20533116161823272705 310 | 16998.328000000001339, 0.21354614198207855225 311 | 16998.519000000000233, 0.21317125856876373291 312 | 16998.717000000000553, 0.2158298790454864502 313 | 16998.929000000000087, 0.22266042232513427734 314 | 16999.125, 0.20129592716693878174 315 | 16999.315999999998894, 0.19215135276317596436 316 | 16999.528999999998632, 0.20539760589599609375 317 | 16999.723000000001775, 0.21352507174015045166 318 | 16999.92699999999968, 0.20334105193614959717 319 | 17000.118000000002212, 0.20737174153327941895 320 | 17000.322000000000116, 0.2034467160701751709 321 | -------------------------------------------------------------------------------- /data/errore_shfaf.txt: -------------------------------------------------------------------------------- 1 | # /gazebo/link_states/recceipt_time, /filter_error/data 2 | 20446.295999999998457, 0.064250081777572631836 3 | 20446.496999999999389, 0.061002235859632492065 4 | 20446.699000000000524, 0.070759214460849761963 5 | 20446.897000000000844, 0.073281660676002502441 6 | 20447.095000000001164, 0.059469632804393768311 7 | 20447.304000000000087, 0.079287648200988769531 8 | 20447.492000000002008, 0.089598663151264190674 9 | 20447.691000000002532, 0.09760735929012298584 10 | 20447.898999999997613, 0.10887198895215988159 11 | 20448.104999999999563, 0.10483868420124053955 12 | 20448.305000000000291, 0.10871092230081558228 13 | 20448.5, 0.11572863161563873291 14 | 20448.69800000000032, 0.11780447512865066528 15 | 20448.891999999999825, 0.12580673396587371826 16 | 20449.090000000000146, 0.12941366434097290039 17 | 20449.299999999999272, 0.13446152210235595703 18 | 20449.493999999998778, 0.13875503838062286377 19 | 20449.701000000000931, 0.12185738980770111084 20 | 20449.891999999999825, 0.14245876669883728027 21 | 20450.095000000001164, 0.13373404741287231445 22 | 20450.292000000001281, 0.14146940410137176514 23 | 20450.5, 0.1298647075891494751 24 | 20450.707999999998719, 0.12146271020174026489 25 | 20450.891999999999825, 0.12297887355089187622 26 | 20451.088999999999942, 0.11425387859344482422 27 | 20451.291999999997643, 0.11570419371128082275 28 | 20451.496999999999389, 0.11495985835790634155 29 | 20451.699000000000524, 0.11706130951642990112 30 | 20451.888999999999214, 0.09366858750581741333 31 | 20452.095000000001164, 0.092211358249187469482 32 | 20452.297999999998865, 0.059386793524026870728 33 | 20452.49199999999837, 0.06775073707103729248 34 | 20452.703000000001339, 0.066257067024707794189 35 | 20452.891999999999825, 0.066213458776473999023 36 | 20453.10200000000259, 0.051436025649309158325 37 | 20453.290000000000873, 0.06514479219913482666 38 | 20453.493000000002212, 0.065991766750812530518 39 | 20453.707000000002154, 0.034184195101261138916 40 | 20453.902000000001863, 0.017257066443562507629 41 | 20454.093000000000757, 0.027596846222877502441 42 | 20454.289000000000669, 0.027392709627747535706 43 | 20454.505000000001019, 0.030369861051440238953 44 | 20454.705000000001746, 0.019605765119194984436 45 | 20454.891999999999825, 0.027804009616374969482 46 | 20455.102999999999156, 0.028941638767719268799 47 | 20455.298999999999069, 0.042885113507509231567 48 | 20455.49199999999837, 0.038877751678228378296 49 | 20455.69800000000032, 0.025750812143087387085 50 | 20455.895000000000437, 0.036736104637384414673 51 | 20456.106999999999971, 0.055115390568971633911 52 | 20456.302999999999884, 0.074800737202167510986 53 | 20456.495999999999185, 0.064345598220825195313 54 | 20456.68999999999869, 0.060910388827323913574 55 | 20456.906999999999243, 0.077463209629058837891 56 | 20457.106999999999971, 0.086276128888130187988 57 | 20457.29399999999805, 0.07594057917594909668 58 | 20457.49199999999837, 0.081468120217323303223 59 | 20457.690999999998894, 0.076813586056232452393 60 | 20457.895000000000437, 0.073635406792163848877 61 | 20458.100999999998749, 0.068207517266273498535 62 | 20458.30199999999968, 0.073146902024745941162 63 | 20458.502000000000407, 0.078314125537872314453 64 | 20458.703999999997905, 0.066170305013656616211 65 | 20458.897000000000844, 0.061205171048641204834 66 | 20459.105999999999767, 0.091631554067134857178 67 | 20459.293000000001484, 0.077504880726337432861 68 | 20459.492999999998574, 0.062774159014225006104 69 | 20459.703999999997905, 0.07268584519624710083 70 | 20459.894000000000233, 0.059632662683725357056 71 | 20460.093000000000757, 0.051928784698247909546 72 | 20460.289000000000669, 0.062493793666362762451 73 | 20460.5, 0.065370619297027587891 74 | 20460.70600000000195, 0.068427704274654388428 75 | 20460.900000000001455, 0.066009812057018280029 76 | 20461.093000000000757, 0.077142745256423950195 77 | 20461.305000000000291, 0.068181864917278289795 78 | 20461.502000000000407, 0.067469686269760131836 79 | 20461.704000000001543, 0.091474890708923339844 80 | 20461.894000000000233, 0.093800581991672515869 81 | 20462.091000000000349, 0.078531295061111450195 82 | 20462.292000000001281, 0.083510711789131164551 83 | 20462.506000000001222, 0.087286636233329772949 84 | 20462.701000000000931, 0.049194376915693283081 85 | 20462.897000000000844, 0.032987590879201889038 86 | 20463.093000000000757, 0.026114851236343383789 87 | 20463.294999999998254, 0.026882242411375045776 88 | 20463.497999999999593, 0.02960925176739692688 89 | 20463.692000000002736, 0.017392182722687721252 90 | 20463.907000000002881, 0.017189187929034233093 91 | 20464.103000000002794, 0.014964213594794273376 92 | 20464.300999999999476, 0.010642761364579200745 93 | 20464.497999999999593, 0.014778110198676586151 94 | 20464.705000000001746, 0.018645532429218292236 95 | 20464.898000000001048, 0.039883293211460113525 96 | 20465.090000000000146, 0.039485264569520950317 97 | 20465.305000000000291, 0.024656482040882110596 98 | 20465.492000000002008, 0.038326144218444824219 99 | 20465.702000000001135, 0.041758973151445388794 100 | 20465.901000000001659, 0.024659007787704467773 101 | 20466.101000000002387, 0.027390789240598678589 102 | 20466.300999999999476, 0.030863691121339797974 103 | 20466.493999999998778, 0.019619725644588470459 104 | 20466.692999999999302, 0.016425997018814086914 105 | 20466.903000000002066, 0.012105473317205905914 106 | 20467.092000000000553, 0.013630476780235767365 107 | 20467.290999999997439, 0.031194636598229408264 108 | 20467.503000000000611, 0.023293141275644302368 109 | 20467.702000000001135, 0.03298325464129447937 110 | 20467.895000000000437, 0.042315267026424407959 111 | 20468.096000000001368, 0.058101240545511245728 112 | 20468.292000000001281, 0.059700883924961090088 113 | 20468.497999999999593, 0.02658262103796005249 114 | 20468.701000000000931, 0.038386054337024688721 115 | 20468.888999999999214, 0.043524142354726791382 116 | 20469.097999999998137, 0.046157598495483398438 117 | 20469.304000000000087, 0.048733174800872802734 118 | 20469.493999999998778, 0.053723488003015518188 119 | 20469.704000000001543, 0.050900638103485107422 120 | 20469.893000000000029, 0.048464007675647735596 121 | 20470.106999999999971, 0.04953540116548538208 122 | 20470.305999999996857, 0.036411933600902557373 123 | 20470.498999999999796, 0.024924468249082565308 124 | 20470.692999999999302, 0.046143721789121627808 125 | 20470.891999999999825, 0.044918619096279144287 126 | 20471.092000000000553, 0.047789499163627624512 127 | 20471.305000000000291, 0.05837726593017578125 128 | 20471.495999999999185, 0.07043186575174331665 129 | 20471.703000000001339, 0.05934401974081993103 130 | 20471.89600000000064, 0.028822887688875198364 131 | 20472.101999999998952, 0.033553481101989746094 132 | 20472.292000000001281, 0.015488062985241413116 133 | 20472.502000000000407, 0.013063135556876659393 134 | 20472.705000000001746, 0.021432518959045410156 135 | 20472.902000000001863, 0.01533928699791431427 136 | 20473.099000000001979, 0.035632714629173278809 137 | 20473.295999999998457, 0.034836065024137496948 138 | 20473.496999999999389, 0.04206390678882598877 139 | 20473.702000000001135, 0.031492516398429870605 140 | 20473.906999999999243, 0.025804340839385986328 141 | 20474.101999999998952, 0.031913299113512039185 142 | 20474.309000000001106, 0.033693447709083557129 143 | 20474.509000000001834, 0.030075279995799064636 144 | 20474.702000000001135, 0.017192756757140159607 145 | 20474.902999999998428, 0.037607066333293914795 146 | 20475.096999999997934, 0.03674014657735824585 147 | 20475.311999999998079, 0.030526585876941680908 148 | 20475.502000000000407, 0.031988874077796936035 149 | 20475.701000000000931, 0.034122765064239501953 150 | 20475.907999999999447, 0.037800364196300506592 151 | 20476.096000000001368, 0.032486509531736373901 152 | 20476.29399999999805, 0.016865264624357223511 153 | 20476.502000000000407, 0.0340538807213306427 154 | 20476.707999999998719, 0.038717485964298248291 155 | 20476.898000000001048, 0.047082304954528808594 156 | 20477.09400000000096, 0.06642732769250869751 157 | 20477.311000000001513, 0.065994225442409515381 158 | 20477.511000000002241, 0.064137190580368041992 159 | 20477.693999999999505, 0.062868848443031311035 160 | 20477.907000000002881, 0.087596997618675231934 161 | 20478.113000000001193, 0.086196050047874450684 162 | 20478.311999999998079, 0.075041994452476501465 163 | 20478.506999999997788, 0.07230262458324432373 164 | 20478.693999999999505, 0.061485480517148971558 165 | 20478.913000000000466, 0.060565911233425140381 166 | 20479.093999999997322, 0.054660454392433166504 167 | 20479.302000000003318, 0.060497283935546875 168 | 20479.496999999999389, 0.054629370570182800293 169 | 20479.710999999999331, 0.059738226234912872314 170 | 20479.902999999998428, 0.04561986774206161499 171 | 20480.108000000000175, 0.036142230033874511719 172 | 20480.302999999999884, 0.034002073109149932861 173 | 20480.502000000000407, 0.031228290870785713196 174 | 20480.701000000000931, 0.032346669584512710571 175 | 20480.912000000000262, 0.034375276416540145874 176 | 20481.101999999998952, 0.035380247980356216431 177 | 20481.300999999999476, 0.035442378371953964233 178 | 20481.495999999999185, 0.034729368984699249268 179 | 20481.709999999999127, 0.049724336713552474976 180 | 20481.894000000000233, 0.048666074872016906738 181 | 20482.112999999997555, 0.041282113641500473022 182 | 20482.307999999997264, 0.044919367879629135132 183 | 20482.498999999999796, 0.031451854854822158813 184 | 20482.712999999999738, 0.02163493819534778595 185 | 20482.895000000000437, 0.036591552197933197021 186 | 20483.102999999999156, 0.06041172519326210022 187 | 20483.302999999999884, 0.069785445928573608398 188 | 20483.498999999999796, 0.062591701745986938477 189 | 20483.707000000002154, 0.094993129372596740723 190 | 20483.912000000000262, 0.10172472149133682251 191 | 20484.095000000001164, 0.11020205914974212646 192 | 20484.307000000000698, 0.12138915061950683594 193 | 20484.496999999999389, 0.12657608091831207275 194 | 20484.701000000000931, 0.092021383345127105713 195 | 20484.902000000001863, 0.090145185589790344238 196 | 20485.109000000000378, 0.11340399831533432007 197 | 20485.305000000000291, 0.092155463993549346924 198 | 20485.494999999998981, 0.099881954491138458252 199 | 20485.704000000001543, 0.11828607320785522461 200 | 20485.902000000001863, 0.11036288738250732422 201 | 20486.098999999998341, 0.11042261868715286255 202 | 20486.294000000001688, 0.11169844865798950195 203 | 20486.516999999999825, 0.10894085466861724854 204 | 20486.705999999998312, 0.08690503239631652832 205 | 20486.912000000000262, 0.09990581125020980835 206 | 20487.108000000000175, 0.097051791846752166748 207 | 20487.320999999999913, 0.089052267372608184814 208 | 20487.523000000001048, 0.095001220703125 209 | 20487.712999999999738, 0.087050385773181915283 210 | 20487.912000000000262, 0.083171315491199493408 211 | 20488.106999999999971, 0.084268756210803985596 212 | 20488.32300000000032, 0.084476150572299957275 213 | 20488.519000000000233, 0.086317457258701324463 214 | 20488.704000000001543, 0.092465169727802276611 215 | 20488.913000000000466, 0.08068968355655670166 216 | 20489.104999999999563, 0.078477196395397186279 217 | 20489.316999999999098, 0.083642378449440002441 218 | 20489.506999999997788, 0.071209780871868133545 219 | 20489.711999999999534, 0.069528378546237945557 220 | 20489.906999999999243, 0.062876164913177490234 221 | 20490.106999999999971, 0.06150703132152557373 222 | 20490.315000000002328, 0.056352950632572174072 223 | 20490.51299999999901, 0.045935876667499542236 224 | 20490.709999999999127, 0.044618140906095504761 225 | 20490.907999999999447, 0.047727596014738082886 226 | 20491.104999999999563, 0.053619872778654098511 227 | 20491.307000000000698, 0.069058693945407867432 228 | 20491.518000000000029, 0.082312509417533874512 229 | 20491.71900000000096, 0.073935240507125854492 230 | 20491.916000000001077, 0.07801698148250579834 231 | 20492.108000000000175, 0.06862856447696685791 232 | 20492.318999999999505, 0.07327903062105178833 233 | 20492.507999999997992, 0.090060472488403320313 234 | 20492.722999999998137, 0.090804472565650939941 235 | 20492.919999999998254, 0.11075903475284576416 236 | 20493.11200000000099, 0.08831249922513961792 237 | 20493.309000000001106, 0.090394929051399230957 238 | 20493.519000000000233, 0.10673406720161437988 239 | 20493.709999999999127, 0.08403717726469039917 240 | 20493.922999999998865, 0.080171898007392883301 241 | 20494.10399999999936, 0.093863777816295623779 242 | 20494.317999999999302, 0.089014165103435516357 243 | 20494.513999999999214, 0.10087516903877258301 244 | 20494.72099999999773, 0.11290594190359115601 245 | 20494.922000000002299, 0.1144898533821105957 246 | 20495.109000000000378, 0.11025234311819076538 247 | 20495.31499999999869, 0.078025110065937042236 248 | 20495.51299999999901, 0.083274014294147491455 249 | 20495.706999999998516, 0.087494783103466033936 250 | 20495.915999999997439, 0.097689174115657806396 251 | 20496.104999999999563, 0.09166847914457321167 252 | 20496.312000000001717, 0.084314987063407897949 253 | 20496.505000000001019, 0.086529627442359924316 254 | 20496.715000000000146, 0.082579933106899261475 255 | 20496.908999999999651, 0.075838029384613037109 256 | 20497.116000000001804, 0.062830820679664611816 257 | 20497.304000000000087, 0.041641067713499069214 258 | 20497.52100000000064, 0.055184241384267807007 259 | 20497.703999999997905, 0.059851847589015960693 260 | 20497.904999999998836, 0.051440116018056869507 261 | 20498.113000000001193, 0.052024312317371368408 262 | 20498.306999999997061, 0.054972432553768157959 263 | 20498.516999999999825, 0.050485439598560333252 264 | 20498.709999999999127, 0.050705727189779281616 265 | 20498.911000000000058, 0.053954292088747024536 266 | 20499.121999999999389, 0.045669618993997573853 267 | 20499.320999999999913, 0.037667512893676757813 268 | 20499.52100000000064, 0.041698932647705078125 269 | 20499.706999999998516, 0.045488253235816955566 270 | 20499.921000000002095, 0.024790911003947257996 271 | 20500.104999999999563, 0.029794866219162940979 272 | 20500.320999999999913, 0.023597978055477142334 273 | 20500.505000000001019, 0.028940808027982711792 274 | 20500.711999999999534, 0.040916804224252700806 275 | 20500.915000000000873, 0.047019414603710174561 276 | 20501.133999999998196, 0.023758763447403907776 277 | 20501.305000000000291, 0.027715440839529037476 278 | 20501.52100000000064, 0.027040174230933189392 279 | 20501.703999999997905, 0.030934875831007957458 280 | 20501.914999999997235, 0.037670113146305084229 281 | 20502.104999999999563, 0.039806406944990158081 282 | 20502.316999999999098, 0.047786429524421691895 283 | 20502.513999999999214, 0.042617287486791610718 284 | 20502.707999999998719, 0.021713035181164741516 285 | 20502.911000000000058, 0.039002764970064163208 286 | 20503.113000000001193, 0.02713927440345287323 287 | 20503.31000000000131, 0.017561616376042366028 288 | 20503.506999999997788, 0.020762512460350990295 289 | -------------------------------------------------------------------------------- /dependencies.sh: -------------------------------------------------------------------------------- 1 | pip install numpy 2 | pip install numpy-quaternion 3 | pip install scikit-fuzzy 4 | cd .. 5 | sudo apt-get install ros-melodic-rqt-multiplot 6 | rospack find gazebosensorplugins && echo sensor plugin already downloaded || git clone https://github.com/valentinbarral/gazebosensorplugins.git 7 | rospack find gtec_msgs && echo gtec messages already downloaded || git clone https://github.com/valentinbarral/rosmsgs.git 8 | catkin build -------------------------------------------------------------------------------- /launch/control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /launch/description.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /launch/spawn_husky_uwb.launch: -------------------------------------------------------------------------------- 1 | 2 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | target_frame: base_link # Leave empty to output scan in the pointcloud frame 80 | tolerance: 1.0 81 | min_height: 0.05 82 | max_height: 1.0 83 | 84 | angle_min: -0.52 # -30.0*M_PI/180.0 85 | angle_max: 0.52 # 30.0*M_PI/180.0 86 | angle_increment: 0.005 # M_PI/360.0 87 | scan_time: 0.3333 88 | range_min: 0.45 89 | range_max: 4.0 90 | use_inf: true 91 | 92 | # Concurrency level, affects number of pointclouds queued for processing and number of threads used 93 | # 0 : Detect number of cores 94 | # 1 : Single threaded 95 | # 2->inf : Parallelism level 96 | concurrency_level: 1 97 | 98 | 99 | 100 | 101 | 102 | 103 | 112 | 113 | 114 | 115 | 116 | -------------------------------------------------------------------------------- /launch/uwb.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /launch/uwb_empty.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 33 | 34 | -------------------------------------------------------------------------------- /launch/world_uwb.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 49 | 50 | 51 | 52 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /models/husky_uwb.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | $(arg robot_namespace) 169 | true 170 | 171 | 172 | 173 | 174 | 175 | $(arg robot_namespace) 176 | 100.0 177 | base_link 178 | imu/data 179 | 0.1 0.07 0.12 180 | 0.06 0.06 0.06 181 | 0.0 0.0 0.0 182 | 0.0 0.0 0.1 183 | 0.00 184 | 0.00 185 | 186 | 187 | 188 | 189 | 190 | 191 | 5.0 192 | 0.25 193 | 0.0 194 | base_link 195 | uwb_anchor 196 | true 197 | 0 198 | 199 | 200 | 201 | 202 | 203 | $(arg robot_namespace) 204 | 40 205 | base_link 206 | base_link 207 | navsat/fix 208 | navsat/vel 209 | 49.9 210 | 8.9 211 | 0 212 | 0 213 | 0.0001 0.0001 0.0001 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ins_uwb_positioning 4 | 0.0.0 5 | The ins_uwb_positioning package 6 | 7 | 8 | 9 | 10 | lorenzo 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # INS/UWB Integrated Positioning 2 | This ROS package is the implementation of the navigation algorithm illustrated in 3 | the article [**An Approach to Robust INS/UWB IntegratedPositioning for 4 | Autonomous Indoor Mobile Robots**](https://www.ncbi.nlm.nih.gov/pmc/articles/PMC6412300/pdf/sensors-19-00950.pdf), *Liu J, Pu J, Sun L, He Z.*, *Sensors (Basel). 5 | 2019;19(4):950. Published 2019 Feb 23. doi:10.3390/s19040950* using Husky platform 6 | as benchmark 7 | 8 | ## Dependencies 9 | In order to use this package you should have [ROS](http://wiki.ros.org/melodic/Installation/Ubuntu) installed in your system. 10 | Additionally this package depends on some standard python libraries and two other packages used for the UWB Gazebo plugin which are [gazebosensorplugins](https://github.com/valentinbarral/gazebosensorplugins) and [gtech_msgs](https://github.com/valentinbarral/rosmsgs) both by valentinbarral. 11 | Script dependencies.sh should set everything you need 12 | ``` 13 | chmod +x ./dependencies.sh && ./dependencies.sh 14 | ``` 15 | ## Run the simulation 16 | You should first set all the environmental variables for Gazebo to work correctly by running *setup.bash* in your workspace root folder 17 | ``` 18 | chmod +x ./setup.bash && ./setup.bash 19 | ``` 20 | And then launch the script *start_all.sh* which will start: 21 | * Gazebo with Husky model in an empty world with UWB antennas 22 | * The navigation node *node_sage-husa.py* 23 | * rqt_multiplot with *rqt_multiplot.xml* config in order to monitor the filter behaviour 24 | * Publisher for Husky velocity command in order to keep the robot in a circular uniform motion 25 | ``` 26 | chmod +x ./start_all.sh && ./start_all.sh 27 | ``` 28 | In order to start visualizing data in rqt plot you should hit the play button on the top right of each graph 29 | ## Folders 30 | 31 | ### src 32 | Contains Python scripts which performs navigation. In particular *node_sage-husa.py* sets the node up and *shfaf.py* contains the filter class. 33 | 34 | ### data 35 | Contains data exported as csv of simulation run with different filter configurations 36 | 37 | ## launch 38 | Contains roslaunch script to spawn husky with IMU and UWB tag (*spawn_husky_uwb.launch, description.launch*), 39 | Husky controls (*control.launch*), and to launch the empty world with UWB antennas in place (*uwb_empty.launch*) 40 | 41 | ## models 42 | Contains Husky urdf xacro model 43 | 44 | ## worlds 45 | Contains different world configuration -------------------------------------------------------------------------------- /rqt_multiplot.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | #ffffff 5 | #000000 6 | false 7 | false 8 | 9 | 10 | 11 | 12 | 13 | 14 | x(m) 15 | 1 16 | true 17 | 18 | 19 | y(m) 20 | 1 21 | true 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | pose/pose/position/x 30 | 0 31 | 32 | 1000 33 | 0 34 | 0 35 | -1000 36 | 0 37 | 38 | /filter_position 39 | nav_msgs/Odometry 40 | 41 | 42 | pose/pose/position/y 43 | 0 44 | 45 | 1000 46 | 0 47 | 0 48 | -1000 49 | 0 50 | 51 | /filter_position 52 | nav_msgs/Odometry 53 | 54 | 55 | 56 | #000000 57 | 0 58 | 59 | 60 | 1000 61 | 10 62 | 2 63 | 64 | 74 | 100 75 | Estimated 76 | 77 | 78 | 79 | 80 | pose/5/position/x 81 | 0 82 | 83 | 1000 84 | 0 85 | 0 86 | -1000 87 | 0 88 | 89 | /gazebo/model_states 90 | gazebo_msgs/ModelStates 91 | 92 | 93 | pose/5/position/y 94 | 0 95 | 96 | 1000 97 | 0 98 | 0 99 | -1000 100 | 0 101 | 102 | /gazebo/model_states 103 | gazebo_msgs/ModelStates 104 | 105 | 106 | 107 | #000000 108 | 0 109 | 110 | 111 | 1000 112 | 10 113 | 2 114 | 115 | 125 | 100 126 | True 127 | 128 | 129 | 130 | 131 | pose/pose/position/x 132 | 0 133 | 134 | 1000 135 | 0 136 | 0 137 | -1000 138 | 0 139 | 140 | /uwb_position 141 | nav_msgs/Odometry 142 | 143 | 144 | pose/pose/position/y 145 | 0 146 | 147 | 1000 148 | 0 149 | 0 150 | -1000 151 | 0 152 | 153 | /uwb_position 154 | nav_msgs/Odometry 155 | 156 | 157 | 158 | #000000 159 | 0 160 | 161 | 162 | 300 163 | 10 164 | 2 165 | 166 | 176 | 100 177 | UWB 178 | 179 | 180 | 181 | true 182 | 183 | 30 184 | Plan view 185 | 186 | 187 | 188 | 189 | 190 | Time(s) 191 | 1 192 | true 193 | 194 | 195 | bias(m/s^2) 196 | 1 197 | true 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 1 207 | 208 | 1000 209 | 0 210 | 0 211 | -1000 212 | 0 213 | 214 | /imu/data/bias 215 | sensor_msgs/Imu 216 | 217 | 218 | linear_acceleration/x 219 | 0 220 | 221 | 1000 222 | 0 223 | 0 224 | -1000 225 | 0 226 | 227 | /imu/data/bias 228 | sensor_msgs/Imu 229 | 230 | 231 | 232 | #000000 233 | 0 234 | 235 | 236 | 1000 237 | 10 238 | 0 239 | 240 | 250 | 100 251 | bias x 252 | 253 | 254 | 255 | 256 | 257 | 1 258 | 259 | 1000 260 | 0 261 | 0 262 | -1000 263 | 0 264 | 265 | /filter_bias 266 | geometry_msgs/Vector3 267 | 268 | 269 | x 270 | 0 271 | 272 | 1000 273 | 0 274 | 0 275 | -1000 276 | 0 277 | 278 | /filter_bias 279 | geometry_msgs/Vector3 280 | 281 | 282 | 283 | #000000 284 | 0 285 | 286 | 287 | 1000 288 | 10 289 | 0 290 | 291 | 301 | 100 302 | Estimated bias x 303 | 304 | 305 | 306 | 307 | 308 | 1 309 | 310 | 1000 311 | 0 312 | 0 313 | -1000 314 | 0 315 | 316 | /filter_bias 317 | geometry_msgs/Vector3 318 | 319 | 320 | y 321 | 0 322 | 323 | 1000 324 | 0 325 | 0 326 | -1000 327 | 0 328 | 329 | /filter_bias 330 | geometry_msgs/Vector3 331 | 332 | 333 | 334 | #000000 335 | 0 336 | 337 | 338 | 1000 339 | 10 340 | 0 341 | 342 | 352 | 100 353 | Estimated bias y 354 | 355 | 356 | 357 | 358 | 359 | 1 360 | 361 | 1000 362 | 0 363 | 0 364 | -1000 365 | 0 366 | 367 | /imu/data/bias 368 | sensor_msgs/Imu 369 | 370 | 371 | linear_acceleration/y 372 | 0 373 | 374 | 1000 375 | 0 376 | 0 377 | -1000 378 | 0 379 | 380 | /imu/data/bias 381 | sensor_msgs/Imu 382 | 383 | 384 | 385 | #000000 386 | 0 387 | 388 | 389 | 1000 390 | 10 391 | 0 392 | 393 | 403 | 100 404 | bias y 405 | 406 | 407 | 408 | 409 | 410 | 1 411 | 412 | 1000 413 | 0 414 | 0 415 | -1000 416 | 0 417 | 418 | /imu/data/bias 419 | sensor_msgs/Imu 420 | 421 | 422 | linear_acceleration/z 423 | 0 424 | 425 | 1000 426 | 0 427 | 0 428 | -1000 429 | 0 430 | 431 | /imu/data/bias 432 | sensor_msgs/Imu 433 | 434 | 435 | 436 | #000000 437 | 0 438 | 439 | 440 | 1000 441 | 10 442 | 0 443 | 444 | 454 | 100 455 | Estimated bias z 456 | 457 | 458 | 459 | 460 | 461 | 1 462 | 463 | 1000 464 | 0 465 | 0 466 | -1000 467 | 0 468 | 469 | /filter_bias 470 | geometry_msgs/Vector3 471 | 472 | 473 | z 474 | 0 475 | 476 | 1000 477 | 0 478 | 0 479 | -1000 480 | 0 481 | 482 | /filter_bias 483 | geometry_msgs/Vector3 484 | 485 | 486 | 487 | #000000 488 | 0 489 | 490 | 491 | 1000 492 | 10 493 | 0 494 | 495 | 505 | 100 506 | bias z 507 | 508 | 509 | 510 | true 511 | 512 | 30 513 | Bias estimation 514 | 515 | 516 | 517 | 518 | 519 | Untitled Axis 520 | 0 521 | true 522 | 523 | 524 | Untitled Axis 525 | 0 526 | true 527 | 528 | 529 | 530 | 531 | 532 | 533 | 534 | 535 | 1 536 | 537 | 1000 538 | 0 539 | 0 540 | -1000 541 | 0 542 | 543 | /gazebo/model_states 544 | gazebo_msgs/ModelStates 545 | 546 | 547 | twist/5/linear/x 548 | 0 549 | 550 | 1000 551 | 0 552 | 0 553 | -1000 554 | 0 555 | 556 | /gazebo/model_states 557 | gazebo_msgs/ModelStates 558 | 559 | 560 | 561 | #000000 562 | 0 563 | 564 | 565 | 1000 566 | 10 567 | 0 568 | 569 | 579 | 100 580 | Vx 581 | 582 | 583 | 584 | 585 | 586 | 1 587 | 588 | 1000 589 | 0 590 | 0 591 | -1000 592 | 0 593 | 594 | /filter_position 595 | nav_msgs/Odometry 596 | 597 | 598 | twist/twist/linear/x 599 | 0 600 | 601 | 1000 602 | 0 603 | 0 604 | -1000 605 | 0 606 | 607 | /filter_position 608 | nav_msgs/Odometry 609 | 610 | 611 | 612 | #000000 613 | 0 614 | 615 | 616 | 1000 617 | 10 618 | 0 619 | 620 | 630 | 100 631 | Estimated Vx 632 | 633 | 634 | 635 | 636 | 637 | 1 638 | 639 | 1000 640 | 0 641 | 0 642 | -1000 643 | 0 644 | 645 | /gazebo/model_states 646 | gazebo_msgs/ModelStates 647 | 648 | 649 | twist/5/linear/y 650 | 0 651 | 652 | 1000 653 | 0 654 | 0 655 | -1000 656 | 0 657 | 658 | /gazebo/model_states 659 | gazebo_msgs/ModelStates 660 | 661 | 662 | 663 | #000000 664 | 0 665 | 666 | 667 | 1000 668 | 10 669 | 0 670 | 671 | 681 | 100 682 | Vy 683 | 684 | 685 | 686 | 687 | 688 | 1 689 | 690 | 1000 691 | 0 692 | 0 693 | -1000 694 | 0 695 | 696 | /filter_position 697 | nav_msgs/Odometry 698 | 699 | 700 | twist/twist/linear/y 701 | 0 702 | 703 | 1000 704 | 0 705 | 0 706 | -1000 707 | 0 708 | 709 | /filter_position 710 | nav_msgs/Odometry 711 | 712 | 713 | 714 | #000000 715 | 0 716 | 717 | 718 | 1000 719 | 10 720 | 0 721 | 722 | 732 | 100 733 | Estimated Vy 734 | 735 | 736 | 737 | 738 | 739 | 1 740 | 741 | 1000 742 | 0 743 | 0 744 | -1000 745 | 0 746 | 747 | /uwb_position 748 | nav_msgs/Odometry 749 | 750 | 751 | twist/twist/linear/x 752 | 0 753 | 754 | 1000 755 | 0 756 | 0 757 | -1000 758 | 0 759 | 760 | /uwb_position 761 | nav_msgs/Odometry 762 | 763 | 764 | 765 | #000000 766 | 0 767 | 768 | 769 | 1000 770 | 10 771 | 0 772 | 773 | 783 | 100 784 | Vx UWB 785 | 786 | 787 | 788 | 789 | 790 | 1 791 | 792 | 1000 793 | 0 794 | 0 795 | -1000 796 | 0 797 | 798 | /uwb_position 799 | nav_msgs/Odometry 800 | 801 | 802 | twist/twist/linear/y 803 | 0 804 | 805 | 1000 806 | 0 807 | 0 808 | -1000 809 | 0 810 | 811 | /uwb_position 812 | nav_msgs/Odometry 813 | 814 | 815 | 816 | #000000 817 | 0 818 | 819 | 820 | 1000 821 | 10 822 | 0 823 | 824 | 834 | 100 835 | Vy UWB 836 | 837 | 838 | 839 | true 840 | 841 | 30 842 | Velocity 843 | 844 | 845 | 846 | 847 | 848 | 849 | 850 | Time(s) 851 | 1 852 | true 853 | 854 | 855 | error(m) 856 | 1 857 | true 858 | 859 | 860 | 861 | 862 | 863 | 864 | 865 | pose/5/position/x 866 | 1 867 | 868 | 1000 869 | 0 870 | 0 871 | -1000 872 | 0 873 | 874 | /gazebo/link_states 875 | gazebo_msgs/LinkStates 876 | 877 | 878 | data 879 | 0 880 | 881 | 1000 882 | 0 883 | 0 884 | -1000 885 | 0 886 | 887 | /filter_error 888 | std_msgs/Float32 889 | 890 | 891 | 892 | #000000 893 | 0 894 | 895 | 896 | 1000 897 | 10 898 | 0 899 | 900 | 910 | 100 911 | Error 912 | 913 | 914 | 915 | true 916 | 917 | 30 918 | Error norm 919 | 920 | 921 | 922 | 923 | 924 | Time(s) 925 | 1 926 | true 927 | 928 | 929 | heading(rad) 930 | 1 931 | true 932 | 933 | 934 | 935 | 936 | 937 | 938 | 939 | data 940 | 1 941 | 942 | 1000 943 | 0 944 | 0 945 | -1000 946 | 0 947 | 948 | /filter_heading 949 | std_msgs/Float32 950 | 951 | 952 | data 953 | 0 954 | 955 | 1000 956 | 0 957 | 0 958 | -1000 959 | 0 960 | 961 | /filter_heading 962 | std_msgs/Float32 963 | 964 | 965 | 966 | #000000 967 | 0 968 | 969 | 970 | 1000 971 | 10 972 | 0 973 | 974 | 984 | 100 985 | Estimated 986 | 987 | 988 | 989 | 990 | 991 | 1 992 | 993 | 1000 994 | 0 995 | 0 996 | -1000 997 | 0 998 | 999 | /true_heading 1000 | std_msgs/Float32 1001 | 1002 | 1003 | data 1004 | 0 1005 | 1006 | 1000 1007 | 0 1008 | 0 1009 | -1000 1010 | 0 1011 | 1012 | /true_heading 1013 | std_msgs/Float32 1014 | 1015 | 1016 | 1017 | #000000 1018 | 0 1019 | 1020 | 1021 | 1000 1022 | 10 1023 | 0 1024 | 1025 | 1035 | 100 1036 | True 1037 | 1038 | 1039 | 1040 | true 1041 | 1042 | 30 1043 | Heading 1044 | 1045 | 1046 | 1047 | 1048 | 1049 | Untitled Axis 1050 | 0 1051 | true 1052 | 1053 | 1054 | Untitled Axis 1055 | 0 1056 | true 1057 | 1058 | 1059 | 1060 | 1061 | true 1062 | 1063 | 30 1064 | Untitled Plot 1065 | 1066 | 1067 | 1068 | false 1069 |
1070 | 1071 | -------------------------------------------------------------------------------- /src/fuzzy_demo.py: -------------------------------------------------------------------------------- 1 | 2 | import skfuzzy as fuzz 3 | from skfuzzy import control as ctrl 4 | import numpy as np 5 | 6 | r = 0.6 7 | 8 | # Fuzzy inference 9 | 10 | input = ctrl.Antecedent(np.arange(0, 0.9, 0.1), 'r') 11 | output = ctrl.Consequent(np.arange(0.8, 2.1, 0.1), 's') 12 | 13 | # Define membership functions 14 | input['less'] = fuzz.trimf(input.universe, [0, 0, 0.3]) 15 | input['equal'] = fuzz.trimf(input.universe, [0.1, 0.4, 0.7]) 16 | input['more'] = fuzz.trimf(input.universe, [0.5, 0.8, 0.8]) 17 | 18 | output['less'] = fuzz.trimf(output.universe, [0.8, 0.8, 1.2]) 19 | output['equal'] = fuzz.trimf(output.universe, [1, 1.4, 1.8]) 20 | output['more'] = fuzz.trimf(output.universe, [1.6, 2.0, 2.0]) 21 | 22 | # Fuzzy rules 23 | rule1 = ctrl.Rule(input['equal'], output['equal']) 24 | rule2 = ctrl.Rule(input['more'], output['more']) 25 | rule3 = ctrl.Rule(input['less'], output['less']) 26 | 27 | fuzzy_control = ctrl.ControlSystem([rule1, rule2, rule3]) 28 | fuzzy = ctrl.ControlSystemSimulation(fuzzy_control) 29 | 30 | fuzzy.input['r'] = r 31 | fuzzy.compute() 32 | 33 | input.view() 34 | output.view() 35 | 36 | output.view(sim = fuzzy) 37 | input.view(sim = fuzzy) 38 | 39 | -------------------------------------------------------------------------------- /src/node_sage-husa.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # coding=utf-8 3 | # /gtec/toa/ranging /imu/data 4 | 5 | 6 | import rospy 7 | import quaternion 8 | import numpy as np 9 | from sensor_msgs.msg import Imu 10 | from nav_msgs.msg import Odometry 11 | from gazebo_msgs.msg import LinkStates 12 | from geometry_msgs.msg import Vector3 13 | from gtec_msgs.msg import Ranging 14 | from std_msgs.msg import Int8 15 | from std_msgs.msg import Float32 16 | from shfaf import shfaf 17 | 18 | pub_filter = rospy.Publisher('filter_position', Odometry, queue_size=1) 19 | pub_uwb = rospy.Publisher('uwb_position', Odometry, queue_size=1) 20 | pub_heading = rospy.Publisher('filter_heading', Float32 , queue_size=1) 21 | pub_heading_true = rospy.Publisher('true_heading', Float32 , queue_size=1) 22 | pub_error = rospy.Publisher('filter_error', Float32 , queue_size=1) 23 | pub_bias = rospy.Publisher('filter_bias', Vector3 , queue_size=1) 24 | init = 1 25 | count = 0 26 | 27 | def imu_callback(data): 28 | 29 | if init == 0: 30 | 31 | w = np.array([data.angular_velocity.x, data.angular_velocity.y, data.angular_velocity.z]) 32 | a = np.array([data.linear_acceleration.x, data.linear_acceleration.y, data.linear_acceleration.z]) 33 | t = data.header.stamp.secs + data.header.stamp.nsecs*1e-9 34 | filter.prediction(a,w,t) 35 | x = filter.x_ 36 | q = filter.q_ 37 | # rospy.loginfo(filter.posOld) 38 | odom = Odometry() 39 | odom.pose.pose.position.x = x[0] 40 | odom.pose.pose.position.y = x[1] 41 | odom.pose.pose.position.z = x[2] 42 | odom.pose.pose.orientation.x = q[0] 43 | odom.pose.pose.orientation.y = q[1] 44 | odom.pose.pose.orientation.z = q[2] 45 | odom.pose.pose.orientation.w = q[3] 46 | odom.twist.twist.linear.x = x[3] 47 | odom.twist.twist.linear.y = x[4] 48 | odom.twist.twist.linear.z = x[5] 49 | odom.twist.twist.angular.x = 0 50 | odom.twist.twist.angular.y = 0 51 | odom.twist.twist.angular.z = 0 52 | pub_filter.publish(odom) 53 | 54 | bias = Vector3() 55 | bias.x = filter.x_[6,0] 56 | bias.y = filter.x_[7,0] 57 | bias.z = filter.x_[8,0] 58 | pub_bias.publish(bias) 59 | 60 | heading = quaternion.as_euler_angles(quaternion.from_float_array(q)) 61 | pub_heading.publish(heading[2]) 62 | 63 | x_uwb = filter.posOld 64 | vel_uwb = filter.velOld 65 | odom_uwb = Odometry() 66 | odom_uwb.pose.pose.position.x = x_uwb[0] 67 | odom_uwb.pose.pose.position.y = x_uwb[1] 68 | odom_uwb.pose.pose.position.z = x_uwb[2] 69 | odom_uwb.twist.twist.linear.x = vel_uwb[0] 70 | odom_uwb.twist.twist.linear.y = vel_uwb[1] 71 | odom_uwb.twist.twist.linear.z = vel_uwb[2] 72 | odom_uwb.twist.twist.angular.x = 0 73 | odom_uwb.twist.twist.angular.y = 0 74 | odom_uwb.twist.twist.angular.z = 0 75 | pub_uwb.publish(odom_uwb) 76 | 77 | 78 | 79 | def uwb_callback(data): 80 | if init == 2: 81 | i = data.anchorId 82 | r = data.range*0.001 83 | filter.range[i] = r 84 | filter.uwbCeck[i] = 1 85 | if sum(filter.uwbCeck) == filter.nAnchors: 86 | filter.correction() 87 | filter.uwbCeck = np.zeros(4) 88 | 89 | def activation_callback(data): 90 | global filter 91 | filter.mode = data.data 92 | # if data.data == 3: 93 | # filter.x_[6:9, :] = 0 94 | 95 | 96 | def model_callback(data): 97 | global init 98 | global filter 99 | global count 100 | count = count + 1 101 | pos_true = np.zeros(3) 102 | if init == 1: 103 | q = np.zeros(4) 104 | x = np.zeros(12) 105 | x[0] = data.pose[5].position.x 106 | x[1] = data.pose[5].position.y 107 | x[3] = data.twist[5].linear.x 108 | x[4] = data.twist[5].linear.y 109 | x[6:9] = 0.0 110 | q[0] = data.pose[5].orientation.w 111 | q[1] = 0 112 | q[2] = 0 113 | q[3] = data.pose[5].orientation.z 114 | filter = shfaf(R=None, Q=None, P=None, x=x, q=q, window_width=100, a=0.9784) 115 | filter.nAnchors = 4 116 | filter.anchorPos = np.array([[-1, -1], 117 | [9, -1], 118 | [-1, 9], 119 | [9, 9], 120 | ]) 121 | rospy.loginfo('Filter initialized at x: ') 122 | rospy.loginfo(filter.x) 123 | rospy.loginfo('q: ') 124 | rospy.loginfo(filter.q) 125 | init = 0 126 | true_q = np.array([data.pose[5].orientation.w, 0, 0, data.pose[5].orientation.z]) 127 | heading_true = quaternion.as_euler_angles(quaternion.from_float_array(true_q)) 128 | pub_heading_true.publish(heading_true[2]) 129 | if count % 200 == 0 and init==0: 130 | out = np.greater(np.random.rand(), 0.95).astype(int) 131 | pos_outlier = out * (np.random.rand(3)-0.5)*4 132 | pos_true[0] = data.pose[5].position.x 133 | pos_true[1] = data.pose[5].position.y 134 | pos_true[2] = data.pose[5].position.z 135 | uwb_noise = np.random.normal(0,0.1,3) 136 | filter.posOld = pos_true + uwb_noise + pos_outlier 137 | filter.velOld[0] = data.twist[5].linear.x + np.random.normal(0.0, 0.1) + uwb_noise[0]*5 + pos_outlier[0]*5 138 | filter.velOld[1] = data.twist[5].linear.y + np.random.normal(0.0, 0.1) + uwb_noise[1]*5 + pos_outlier[1]*5 139 | filter.velOld[2] = data.twist[5].linear.z + np.random.normal(0.0, 0.1) + uwb_noise[2]*5 + pos_outlier[2]*5 140 | error = np.linalg.norm(pos_true - filter.x_[:3,0]) 141 | pub_error.publish(error) 142 | if filter.mode != 3: 143 | filter.correction() 144 | count = 0 145 | 146 | 147 | 148 | 149 | 150 | 151 | def node(): 152 | 153 | rospy.init_node('estimator', anonymous=True) 154 | 155 | rospy.Subscriber("/imu/data", Imu, imu_callback) 156 | rospy.Subscriber("/gtec/toa/ranging", Ranging, uwb_callback) 157 | rospy.Subscriber("/filter_mode", Int8, activation_callback) 158 | rospy.Subscriber("/gazebo/model_states", LinkStates, model_callback) 159 | 160 | # spin() simply keeps python from exiting until this node is stopped 161 | rospy.spin() 162 | 163 | if __name__ == '__main__': 164 | node() 165 | 166 | -------------------------------------------------------------------------------- /src/shfaf.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import quaternion 3 | import rospy 4 | import skfuzzy as fuzz 5 | from skfuzzy import control as ctrl 6 | import scipy as sp 7 | 8 | 9 | def skew(x): 10 | return np.array([[0, -x[2], x[1]], 11 | [x[2], 0, -x[0]], 12 | [-x[1], x[0], 0]]) 13 | 14 | 15 | def push(x, y): 16 | x[:, :-1] = x[:, 1:] 17 | x[:, -1:] = y # .reshape(-1,1) 18 | return x 19 | 20 | 21 | class shfaf(object): 22 | def __init__(self, R=None, Q=None, P=None, x=None, q=None, window_width=12, a=0.95): 23 | 24 | if R is None: 25 | R = np.diag([1, 1, 1, 1 * 5, 1 * 5, 1 * 5]) * 1e-1 26 | if Q is None: 27 | Q = np.diag([0.06 ** 2, 0.06 ** 2, 0.06 ** 2, 0, 0, 0, 0.1, 0.1, 0.1, 0, 0, 0]) 28 | if P is None: 29 | P = np.diag([0.1, 0.1, 0.1, 0.05, 0.05, 0.05, 0, 0, 0, 0.01, 0.01, 0.01, 0, 0, 0]) 30 | if x is None: 31 | x = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 32 | np.float) # position, velocity, acceleration bias, omega bias 33 | if q is None: 34 | q = quaternion.from_rotation_vector(np.array([0, 0, 0.701], np.float)) 35 | q = quaternion.as_float_array(q) 36 | 37 | ''' 38 | Filter mode must be set to: 39 | 0 - for Sage-Husa Fuzzy Adaptive Filter 40 | 1 - for Sage-Husa Adaptive Filter 41 | 2 - for EKF 42 | 3 - for Inertial Navigation 43 | ''' 44 | self.mode = 0 45 | self.on = True 46 | 47 | self.lamb = np.block([[np.zeros((3, 12), np.float)], 48 | [np.identity(12, np.float)]]) 49 | self.H = np.block([[np.identity(6, np.float), np.zeros((6, 9), np.float)]]) 50 | self.threshold = 4.5 51 | a_v = np.ones(window_width, np.float) * a 52 | j = np.arange(0, window_width) 53 | s = np.power(a_v, j) * (1 - a_v) / (1 - np.power(a_v, window_width)) 54 | self.sigma = np.diag(s) 55 | self.time = 0 56 | self.uwbTime = 0 57 | self.nAnchors = 4 58 | self.anchorPos = np.zeros((2, self.nAnchors), np.float) 59 | self.range = np.ones(self.nAnchors, np.float) * 99 60 | self.uwbCeck = np.zeros(self.nAnchors) # check if all anchors' signals arrived 61 | self.uwbInit = 0 # flag for uwb velocity initialization 62 | self.pred = 0 # flag which states if prediction or correction has been performed last 63 | beta = 0 64 | self.G = 1 - np.square(beta) 65 | self.H_uwb = np.square(1 - beta) 66 | 67 | self.R = R 68 | self.Q = Q 69 | self.P = P 70 | self.P_ = P 71 | self.x = x[np.newaxis].T 72 | self.x_ = x[np.newaxis].T 73 | self.posOld = x[:3] 74 | self.velOld = x[3:6] 75 | self.q = q 76 | self.q_ = q 77 | self.dx = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], np.float)[ 78 | np.newaxis].T # position, velocity, orientation, acceleration bias, omega bias 79 | self.dx_ = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], np.float)[ 80 | np.newaxis].T # position, velocity, orientation, acceleration bias, omega bias 81 | self.innovation = np.random.normal(0.0, 0.1, (6, window_width)) 82 | self.error = (np.random.rand(15, window_width) - 1) * 0.1 83 | # self.innovation_ = np.zeros((6,window_width), np.float) 84 | self.k = 0 # Iteration counter 85 | self.lambda_ = 1.0119 # Sage Huza ICW coefficent 86 | self.b = 0.9976 # Sage Huza ICW coefficent 87 | self.alpha = 0.48 # Sage Huza ICW coefficent 88 | H = self.H 89 | self.K = H.T.dot(np.linalg.inv(H.dot(H.T))) # Kalman gain 90 | 91 | # Fuzzy inference 92 | 93 | input = ctrl.Antecedent(np.arange(0, 0.9, 0.1), 'r') 94 | output = ctrl.Consequent(np.arange(0.8, 2.1, 0.1), 's') 95 | 96 | # Define membership functions 97 | input['less'] = fuzz.trimf(input.universe, [0, 0, 0.3]) 98 | input['equal'] = fuzz.trimf(input.universe, [0.1, 0.4, 0.7]) 99 | input['more'] = fuzz.trimf(input.universe, [0.5, 0.8, 0.8]) 100 | 101 | output['less'] = fuzz.trimf(output.universe, [0.8, 0.8, 1.2]) 102 | output['equal'] = fuzz.trimf(output.universe, [1, 1.4, 1.8]) 103 | output['more'] = fuzz.trimf(output.universe, [1.6, 2.0, 2.0]) 104 | 105 | # Fuzzy rules 106 | rule1 = ctrl.Rule(input['equal'], output['equal']) 107 | rule2 = ctrl.Rule(input['more'], output['more']) 108 | rule3 = ctrl.Rule(input['less'], output['less']) 109 | 110 | fuzzy_control = ctrl.ControlSystem([rule1, rule2, rule3]) 111 | self.fuzzy = ctrl.ControlSystemSimulation(fuzzy_control) 112 | 113 | def _rotationMatrix(self, q): 114 | q = quaternion.from_float_array(q) 115 | C = quaternion.as_rotation_matrix(q) 116 | return C 117 | 118 | def prediction(self, am, wm, t): 119 | 120 | am = am[np.newaxis].T 121 | wm = wm[np.newaxis].T 122 | wm[:2, :] = 0 123 | if self.uwbInit: 124 | dt = t - self.time 125 | else: 126 | dt = 0.01 127 | 128 | if self.pred: 129 | x = self.x_ 130 | P = self.P_ 131 | q = self.q_ 132 | else: 133 | x = self.x 134 | P = self.P 135 | q = self.q 136 | C = self._rotationMatrix(q) 137 | self.Q = sp.linalg.block_diag(np.eye(3) * ((0.06 * dt) ** 2), np.eye(3) * ((0.0 * dt) ** 2), 138 | np.eye(3) * ((0.001 * dt) ** 2), np.eye(3) * ((0.0 * dt) ** 2)) 139 | Q = self.Q 140 | w = wm * dt 141 | g = np.array([0, 0, 9.8])[np.newaxis].T 142 | x_dot = np.concatenate([x[3:6, :], 143 | C.dot((am - x[6:9, :])) - g, 144 | np.zeros(6)[np.newaxis].T 145 | ]) 146 | x_next = x + x_dot * dt 147 | 148 | if np.linalg.norm(w) != 0: 149 | qq = quaternion.as_quat_array(q) 150 | wq = quaternion.from_rotation_vector(w[:, 0] * np.array((0, 0, 1))) 151 | q_next = quaternion.as_float_array(qq * wq) 152 | else: 153 | q_next = q 154 | F = np.block([[np.identity(3, np.float), np.identity(3, np.float) * dt, np.zeros((3, 9))], 155 | [np.zeros((3, 3), np.float), np.identity(3, np.float), -(C.dot(skew(am - x[6:9, :]))) * dt, 156 | -C * dt, np.zeros((3, 3), np.float)], 157 | [np.zeros((3, 6), np.float), self._rotationMatrix( 158 | quaternion.as_float_array(quaternion.from_euler_angles(((wm - x[9:12, :]) * dt)[:, 0]))), 159 | np.zeros((3, 3), np.float), 160 | -np.identity(3, np.float) * dt], 161 | [np.zeros((3, 9), np.float), np.identity(3, np.float), np.zeros((3, 3), np.float)], 162 | [np.zeros((3, 12), np.float), np.identity(3, np.float)]]) 163 | self.P_ = (F.dot(P.dot(F.T)) + self.lamb.dot(Q.dot(self.lamb.T))) 164 | self.dx_ = F.dot(self.dx) 165 | self.k = self.k + 1 166 | self.x_ = x_next 167 | self.q_ = q_next 168 | self.time = t 169 | self.pred = 1 170 | self.uwbInit = 1 171 | 172 | return x_next, q_next 173 | 174 | def fuzzyInference(self, r): 175 | 176 | self.fuzzy.input['r'] = r 177 | self.fuzzy.compute() 178 | 179 | return self.fuzzy.output['s'] 180 | 181 | def correction(self): 182 | pos = self.posOld 183 | vel = self.velOld 184 | measure = np.concatenate((pos, vel))[np.newaxis].T 185 | H = self.H 186 | z = measure - self.x_[:6, :] 187 | check = np.ones(6, int)[np.newaxis].T 188 | S_teo = H.dot(self.P_.dot(H.T)) + self.R 189 | 190 | # Filter outliers 191 | while sum(check)[0] != 0: 192 | inn = z - H.dot(self.dx_) 193 | ep = push(self.innovation, inn) 194 | S = ep.dot(self.sigma.dot(ep.T)) 195 | if self.mode == 2: 196 | break 197 | D = S_teo + H.dot(self.dx_.dot(self.dx_.T.dot(H.T))) 198 | G = S + H.dot(self.dx_.dot(self.dx_.T.dot(H.T))) 199 | m = (np.abs(np.diagonal(G) / np.diagonal(D)))[np.newaxis].T 200 | check = np.greater(m, self.threshold).astype(int)[np.newaxis].T 201 | z = (z * (check * (1 / np.sqrt(m) - 1) + 1)).reshape(-1, 1) 202 | 203 | self.innovation = ep 204 | if self.mode == 0: 205 | r = np.abs(np.trace(S) / np.trace(S_teo) - 1) 206 | s = self.fuzzyInference(r) 207 | elif self.mode == 1: 208 | s = 1 209 | elif self.mode == 2: 210 | s = 0 211 | self.R = np.diag([1, 1, 1, 1, 1, 1]) * 1e-1 212 | 213 | d = (self.lambda_ - self.b) / (self.lambda_ - np.power(self.b, self.k + 1)) 214 | 215 | # Estimate measurement noise covariance 216 | self.R = (1 - np.power(s, self.alpha) * d) * self.R + np.power(s, self.alpha) * d * ( 217 | S - H.dot(self.P_.dot(H.T))) 218 | 219 | # Calculate Kalman gain 220 | self.K = self.P_.dot(H.T.dot(np.linalg.inv(H.dot(self.P_.dot(H.T)) + self.R))) 221 | 222 | # Update error state and its covariance 223 | self.dx = self.K.dot(inn) 224 | self.error = push(self.error, self.dx) 225 | self.P = (np.identity(15, float) - self.K.dot(H)).dot(self.P_) 226 | 227 | # Update the nominal state 228 | dx_pos = np.concatenate((self.dx[:6], self.dx[9:])) 229 | self.x = self.x_ + dx_pos 230 | dq = quaternion.from_euler_angles(np.concatenate((np.zeros(2), self.dx[8]))) 231 | self.q = quaternion.as_float_array(quaternion.from_float_array(self.q_) * dq) 232 | 233 | 234 | # Reset the error state 235 | self.dx = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], np.float)[np.newaxis].T 236 | self.dx_ = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], np.float)[np.newaxis].T 237 | self.pred = 0 238 | self.k = 0 239 | 240 | def uwbRange2Pos(self): 241 | r = self.range 242 | anc = self.anchorPos 243 | t = rospy.get_time() 244 | dt = t - self.uwbTime 245 | G = anc[0, :] - anc[1:, :] 246 | b = np.square(r[1:]) - np.square(r[0]) + np.square(anc[0, 0]) - np.square(anc[1:, 0]) + np.square( 247 | anc[0, 1]) - np.square(anc[1:, 1]) # + np.square(anc[0,2]) - np.square(anc[1:,2]) 248 | pos_ = 0.5 * np.linalg.inv(G.T.dot(G)).dot(G.T.dot(b)) 249 | pos = self.posOld + self.velOld * dt + self.G * (pos_ - (self.posOld + self.velOld * dt)) 250 | vel = self.velOld + self.H_uwb / dt * (pos_ - (self.posOld + self.velOld * dt)) 251 | self.uwbTime = t 252 | self.posOld = pos 253 | self.velOld = vel 254 | return pos, vel 255 | -------------------------------------------------------------------------------- /src/shfaf.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lorenzoferrini/ins_uwb_positioning/57b9cd87d93a1eb03be54c8b79fd1a10547252a1/src/shfaf.pyc -------------------------------------------------------------------------------- /start_all.sh: -------------------------------------------------------------------------------- 1 | gnome-terminal -- roslaunch ins_uwb_positioning uwb_empty.launch 2 | sleep 5 3 | gnome-terminal -- rosrun ins_uwb_positioning node_sage-husa.py 4 | gnome-terminal -- rosrun rqt_multiplot rqt_multiplot --multiplot-config file://$(rospack find ins_uwb_positioning)/rqt_multiplot.xml 5 | gnome-terminal -- rostopic pub /husky_velocity_controller/cmd_vel geometry_msgs/Twist "linear: 6 | x: 0.5 7 | y: 0.0 8 | z: 0.0 9 | angular: 10 | x: 0.0 11 | y: 0.0 12 | z: 0.2" -r10 13 | 14 | 15 | -------------------------------------------------------------------------------- /worlds/uwb_empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | model://ground_plane 11 | 12 | 13 | 14 | -1 -1 0.5 0 0 0 15 | 1 16 | 17 | 18 | 19 | 20 | 0.2 0.01 0.2 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 9 -1 0.5 0 0 0 29 | 1 30 | 31 | 32 | 33 | 34 | 0.2 0.01 0.2 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -1 9 0.5 0 0 0 43 | 1 44 | 45 | 46 | 47 | 48 | 0.2 0.01 0.2 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 9 9 0.5 0 0 0 57 | 1 58 | 59 | 60 | 61 | 62 | 0.2 0.01 0.2 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | --------------------------------------------------------------------------------