Added simple Kitti example
Richard Roberts
parent
bb4ad176da
commit
7f5de1c3bc
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,2 @@
|
|||
BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT
|
||||
0 0 0 0 0 0 0.01 0.000175 0 0.000167 2.91e-006 0.0100395199348279
|
||||
|
|
@ -0,0 +1,468 @@
|
|||
Time Latitude Longitude Altitude PositionSigma
|
||||
46534.47837579 48.982530923568 8.3903459539796 116.43446350098 0.072138755187486
|
||||
46536.397971133 48.982637737646 8.3904395756801 116.39415740967 0.077077882690173
|
||||
46537.40790448 48.982638403644 8.3904400660267 116.39430236816 0.077077882690173
|
||||
46538.417854613 48.982639054039 8.3904406077418 116.39440917969 0.077077882690173
|
||||
46539.427814058 48.982639701333 8.3904411470687 116.39470672607 0.077077882690173
|
||||
46540.437704157 48.982640368632 8.3904416416362 116.39482116699 0.077077882690173
|
||||
46541.447571055 48.982641020649 8.3904421826205 116.39489746094 0.077077882690173
|
||||
46542.45750414 48.982641670789 8.3904427238241 116.39487457275 0.077077882690173
|
||||
46543.467420571 48.98264233992 8.3904432196704 116.39485168457 0.077077882690173
|
||||
46544.477186399 48.98264299402 8.3904437631157 116.39492034912 0.077077882690173
|
||||
46545.487146318 48.982643646028 8.3904443016455 116.39477539062 0.077077882690173
|
||||
46546.497019056 48.982644318256 8.390444792625 116.39532470703 0.077077882690173
|
||||
46547.506781063 48.982644974973 8.3904453372047 116.39517974854 0.077077882690173
|
||||
46548.516637843 48.9826456288 8.3904458799874 116.39517211914 0.077077882690173
|
||||
46549.52655747 48.982646302123 8.3904463742274 116.39556884766 0.077077882690173
|
||||
46550.536380144 48.982646960728 8.3904469172003 116.39527130127 0.077077882690173
|
||||
46551.546297813 48.982647617358 8.3904474633594 116.39515686035 0.077077882690173
|
||||
46552.556134492 48.982648291715 8.390447951888 116.39591217041 0.077077882690173
|
||||
46553.556620892 48.982648952976 8.3904484941506 116.39554595947 0.077077882690173
|
||||
46554.565888081 48.98264961178 8.3904490383148 116.39540100098 0.077077882690173
|
||||
46555.575787206 48.982650288455 8.3904495289266 116.39614868164 0.077077882690173
|
||||
46556.585675987 48.982650952186 8.3904500721994 116.39587402344 0.077077882690173
|
||||
46557.595615531 48.982651613635 8.3904506146677 116.39552307129 0.077077882690173
|
||||
46558.605522807 48.982652292496 8.3904511069163 116.39621734619 0.077077882690173
|
||||
46559.615373617 48.982652958406 8.3904516515705 116.39595794678 0.077077882690173
|
||||
46560.625244993 48.98265362289 8.3904521942027 116.39569091797 0.077077882690173
|
||||
46561.635143034 48.982654303591 8.3904526887944 116.3963470459 0.077077882690173
|
||||
46562.644985218 48.982654971673 8.3904532335878 116.39611053467 0.077077882690173
|
||||
46563.654853868 48.982655638509 8.3904537785253 116.39585113525 0.077794601355107
|
||||
46564.664840181 48.982656320994 8.3904542803316 116.39681243896 0.077794601355107
|
||||
46565.67472556 48.982656991926 8.3904548257033 116.39672088623 0.077794601355107
|
||||
46566.684600982 48.982657660454 8.3904553709421 116.39641571045 0.077794601355107
|
||||
46567.69445871 48.982658345315 8.3904558745861 116.39734649658 0.077794601355107
|
||||
46568.694752963 48.98265901787 8.3904564227676 116.39722442627 0.077794601355107
|
||||
46569.704205622 48.982659689239 8.3904569673181 116.39707946777 0.077794601355107
|
||||
46570.714115042 48.982660375331 8.3904574734588 116.39790344238 0.077794601355107
|
||||
46571.7240302097 48.982661049731 8.3904580208474 116.3977355957 0.077794601355107
|
||||
46572.733803953 48.98266172296 8.3904585678131 116.39752197266 0.077794601355107
|
||||
46573.743778622 48.982662410871 8.390459075055 116.39833068848 0.077794601355107
|
||||
46574.75366829 48.982663088101 8.39045962353 116.39832305908 0.077794601355107
|
||||
46575.763502718 48.982663762721 8.3904601698486 116.39807891846 0.077794601355107
|
||||
46576.773425433 48.982664457595 8.3904606793942 116.39806365967 0.077794601355107
|
||||
46577.783263421 48.982665137122 8.3904612276202 116.39820098877 0.077794601355107
|
||||
46578.793129246 48.982665814413 8.3904617778371 116.39828491211 0.077794601355107
|
||||
46579.803019797 48.982666511107 8.3904622895775 116.39846801758 0.077794601355107
|
||||
46580.812971397 48.982667192196 8.3904628384972 116.39878845215 0.077794601355107
|
||||
46581.812973661 48.98266787212 8.3904633886502 116.39900970459 0.077794601355107
|
||||
46582.822724791 48.982668570161 8.390463902026 116.3994140625 0.077794601355107
|
||||
46583.834990833 48.982669253451 8.3904644522753 116.40001678467 0.077794601355107
|
||||
46584.842505808 48.982669935469 8.390465001081 116.40031433105 0.077794601355107
|
||||
46585.852380614 48.982670635089 8.3904655169598 116.40087127686 0.077794601355107
|
||||
46586.862289581 48.982671320324 8.3904660691134 116.40154266357 0.077794601355107
|
||||
46587.872126551 48.982672004277 8.3904666182046 116.40216064453 0.077794601355107
|
||||
46588.881994553 48.982672705124 8.3904671356213 116.40279388428 0.077794601355107
|
||||
46589.891872729 48.982673391568 8.3904676893295 116.40348052979 0.077794601355107
|
||||
46590.901792241 48.982674077002 8.3904682416209 116.40419006348 0.077794601355107
|
||||
46591.911956265 48.982674778546 8.3904687629936 116.40480804443 0.077794601355107
|
||||
46592.921705223 48.98267546617 8.3904693180609 116.40550231934 0.077794601355107
|
||||
46593.931564271 48.982676152869 8.3904698731146 116.40628814697 0.077794601355107
|
||||
46594.941532522 48.982676854847 8.3904703986159 116.40700531006 0.077794601355107
|
||||
46595.951309096 48.982677544021 8.3904709557016 116.40765380859 0.077794601355107
|
||||
46596.961135064 48.982678232159 8.390471510579 116.408203125 0.077794601355107
|
||||
46597.96117884 48.982678935435 8.390472038199 116.40875244141 0.077794601355107
|
||||
46598.970937122 48.982679625896 8.3904725992791 116.40939331055 0.077794601355107
|
||||
46599.980772432 48.982680315502 8.390473152759 116.4098815918 0.077794601355107
|
||||
46600.990674182 48.982681019694 8.3904736808386 116.41033172607 0.077794601355107
|
||||
46602.000546794 48.982681712314 8.3904742392497 116.41079711914 0.077794601355107
|
||||
46603.010428562 48.98268240283 8.3904747981971 116.41142272949 0.077794601355107
|
||||
46604.010461353 48.982683108505 8.390475326022 116.4118270874 0.077794601355107
|
||||
46605.020209853 48.982683803362 8.3904758829458 116.41233062744 0.077794601355107
|
||||
46606.030071765 48.982684495722 8.3904764410565 116.41277313232 0.077794601355107
|
||||
46607.039984219 48.982685203059 8.3904769692928 116.41313934326 0.077794601355107
|
||||
46608.049829571 48.982685899049 8.390477526392 116.41352081299 0.077794601355107
|
||||
46609.059760065 48.982686593859 8.3904780844487 116.41412353516 0.077794601355107
|
||||
46610.069659515 48.982687302187 8.3904786145822 116.41454315186 0.077794601355107
|
||||
46611.079473921 48.982687999556 8.390479170931 116.41506958008 0.077794601355107
|
||||
46612.089363448 48.98268869621 8.3904797292032 116.41542816162 0.077794601355107
|
||||
46613.099313545 48.982689405787 8.3904802605124 116.41582489014 0.077794601355107
|
||||
46614.102154941 48.982690104595 8.3904808185335 116.41622924805 0.077794601355107
|
||||
46615.109145674 48.982690801524 8.3904813772106 116.4167175293 0.077794601355107
|
||||
46616.118929637 48.982691516842 8.3904819149795 116.41648864746 0.077794601355107
|
||||
46617.128767067 48.982692216175 8.390482472939 116.41668701172 0.077794601355107
|
||||
46618.128793577 48.982692914656 8.3904830330916 116.41716003418 0.077794601355107
|
||||
46619.138653093 48.982693630101 8.3904835718059 116.41682434082 0.077794601355107
|
||||
46620.148433572 48.982694329952 8.39048413237 116.41715240479 0.077794601355107
|
||||
46621.158409799 48.982695029829 8.3904846892525 116.4174118042 0.077794601355107
|
||||
46622.168254522 48.982695746041 8.3904852284736 116.41710662842 0.077794601355107
|
||||
46623.178126988 48.982696447894 8.3904857887985 116.41732025146 0.077794601355107
|
||||
46624.18797132 48.982697147566 8.3904863495611 116.4176940918 0.077794601355107
|
||||
46625.19785557 48.982697862274 8.3904868923042 116.41771697998 0.077794601355107
|
||||
46626.207807668 48.982698564572 8.3904874531129 116.41793060303 0.077794601355107
|
||||
46627.217644285 48.982699266509 8.3904880136621 116.41813659668 0.077794601355107
|
||||
46628.22749807 48.982699982365 8.3904885568117 116.41814422607 0.077794601355107
|
||||
46629.237868029 48.982700686765 8.3904891177477 116.41827392578 0.077794601355107
|
||||
46630.247291053 48.982701388413 8.3904896796788 116.41846466064 0.077794601355107
|
||||
46631.257256005 48.982702104738 8.390490224735 116.41840362549 0.077794601355107
|
||||
46632.267067061 48.982702810016 8.3904907878194 116.4185256958 0.077794601355107
|
||||
46633.27788599 48.982703514427 8.3904913485087 116.41865539551 0.077794601355107
|
||||
46634.28681835 48.982704232022 8.390491893636 116.41865539551 0.077794601355107
|
||||
46635.296706212 48.982704938802 8.3904924550809 116.41886901855 0.077794601355107
|
||||
46636.306687485 48.982705643692 8.3904930190657 116.4189453125 0.077794601355107
|
||||
46637.306702181 48.98270635393 8.3904935580737 116.41944885254 0.077794601355107
|
||||
46638.316398116 48.982707062371 8.3904941215543 116.41974639893 0.077794601355107
|
||||
46639.326233059 48.982707768731 8.3904946831525 116.41985321045 0.077794601355107
|
||||
46640.336191046 48.982708480501 8.3904952233969 116.42046356201 0.077794601355107
|
||||
46641.346018681 48.98270918972 8.3904957886068 116.42091369629 0.077794601355107
|
||||
46642.35589624 48.982709898293 8.3904963515336 116.42092132568 0.077794601355107
|
||||
46643.365804099 48.982710611396 8.3904968929765 116.42154693604 0.077794601355107
|
||||
46644.375727679 48.982711323247 8.3904974571766 116.42179870605 0.077794601355107
|
||||
46645.38556534 48.982712031755 8.3904980234355 116.42211914062 0.077794601355107
|
||||
46646.395462191 48.982712746212 8.3904985774143 116.42224121094 0.077794601355107
|
||||
46647.405516799 48.98271345917 8.3904991421973 116.422706604 0.077794601355107
|
||||
46648.415648639 48.982714170634 8.3904997058756 116.42281341553 0.077794601355107
|
||||
46649.425187386 48.982714887069 8.3905002574485 116.42311096191 0.077794601355107
|
||||
46650.435206854 48.98271560178 8.3905008217311 116.42374420166 0.077794601355107
|
||||
46651.445116298 48.98271631494 8.3905013857879 116.42406463623 0.077794601355107
|
||||
46652.455663295 48.982717033231 8.3905019384399 116.42446136475 0.077794601355107
|
||||
46653.464935523 48.982717750267 8.3905025026417 116.42520141602 0.077794601355107
|
||||
46654.474778807 48.98271846498 8.3905030658218 116.42575073242 0.077794601355107
|
||||
46655.484473355 48.982719184701 8.3905036182469 116.42624664307 0.077794601355107
|
||||
46656.49435535 48.982719902697 8.3905041826247 116.42699432373 0.077794601355107
|
||||
46657.504209941 48.98272061995 8.3905047464476 116.4275894165 0.077794601355107
|
||||
46658.514078193 48.982721342179 8.3905052991025 116.42905426025 0.077794601355107
|
||||
46659.52399239 48.982722061747 8.3905058640669 116.42966461182 0.077794601355107
|
||||
46660.533888423 48.982722779964 8.3905064287348 116.43035888672 0.077794601355107
|
||||
46661.54378329 48.982723503072 8.3905069832569 116.43170166016 0.077794601355107
|
||||
46662.553619734 48.982724223218 8.3905075505968 116.43222045898 0.077794601355107
|
||||
46663.56379414 48.9827249431 8.3905081163512 116.43286895752 0.077794601355107
|
||||
46664.573380882 48.982725667174 8.3905086725439 116.43411254883 0.080622577482985
|
||||
46665.583266361 48.982726388557 8.390509240528 116.43452453613 0.080622577482985
|
||||
46666.593140868 48.982727108761 8.390509810321 116.43518066406 0.080622577482985
|
||||
46667.603103764 48.982727830071 8.3905103611481 116.43701171875 0.080622577482985
|
||||
46668.612934287 48.982728552569 8.3905109291148 116.43733215332 0.080622577482985
|
||||
46669.613003869 48.982729274304 8.3905114990437 116.4377822876 0.080622577482985
|
||||
46670.622748418 48.982729996671 8.390512052271 116.43941497803 0.080622577482985
|
||||
46671.632607071 48.982730719421 8.3905126248211 116.43965911865 0.080622577482985
|
||||
46672.642467511 48.982731442801 8.3905131910452 116.43985748291 0.080622577482985
|
||||
46673.652359638 48.982732166213 8.3905137453647 116.44117736816 0.080622577482985
|
||||
46674.662247872 48.982732890397 8.3905143198525 116.44119262695 0.080622577482985
|
||||
46675.672122831 48.982733613499 8.3905148908076 116.44144439697 0.080622577482985
|
||||
46676.682018227 48.982734344095 8.390515453764 116.44216918945 0.080622577482985
|
||||
46677.691919221 48.982735069854 8.3905160275404 116.44203186035 0.080622577482985
|
||||
46678.701803419 48.982735794025 8.3905166032058 116.4421081543 0.080622577482985
|
||||
46679.711725398 48.982736525492 8.3905171684714 116.44271087646 0.080622577482985
|
||||
46680.721551119 48.982737252224 8.390517743962 116.44248199463 0.080622577482985
|
||||
46681.731437022 48.982737977929 8.3905183197274 116.44247436523 0.080622577482985
|
||||
46682.731523463 48.982738710292 8.3905188872024 116.44300842285 0.080622577482985
|
||||
46683.741223296 48.982739438336 8.3905194629077 116.44267272949 0.080622577482985
|
||||
46684.751092799 48.982740164973 8.3905200409068 116.44255065918 0.080622577482985
|
||||
46685.760970043 48.982740898293 8.3905206104841 116.44291687012 0.080622577482985
|
||||
46686.771997213 48.982741627001 8.39052118928 116.44258117676 0.080622577482985
|
||||
46687.780750598 48.98274235533 8.3905217648773 116.4421081543 0.080622577482985
|
||||
46688.790608805 48.982743080328 8.3905223225781 116.44178771973 0.080622577482985
|
||||
46689.800647325 48.982743810502 8.3905229031653 116.44135284424 0.080622577482985
|
||||
46690.811736869 48.982744539367 8.3905234810354 116.44080352783 0.080622577482985
|
||||
46691.820296146 48.98274526574 8.3905240413504 116.44033813477 0.080622577482985
|
||||
46692.830162569 48.982745997378 8.3905246227375 116.43975830078 0.080622577482985
|
||||
46693.840049869 48.9827467273 8.3905252025325 116.43918609619 0.080622577482985
|
||||
46694.849967565 48.982747454663 8.3905257645901 116.43859863281 0.080622577482985
|
||||
46695.859846428 48.98274818697 8.3905263473977 116.43801879883 0.080622577482985
|
||||
46696.869725717 48.982748918708 8.3905269278921 116.4372177124 0.080622577482985
|
||||
46697.879597785 48.98274964756 8.3905274914763 116.43655395508 0.080622577482985
|
||||
46698.88948373 48.982750381739 8.3905280749159 116.43585205078 0.080622577482985
|
||||
46699.899595957 48.982751113725 8.390528657902 116.43515777588 0.080622577482985
|
||||
46700.909405283 48.982751844104 8.3905292237077 116.43448638916 0.080622577482985
|
||||
46701.919305179 48.982752579636 8.3905298073703 116.43374633789 0.080622577482985
|
||||
46702.929154081 48.982753313249 8.3905303911038 116.43297576904 0.080622577482985
|
||||
46703.939099211 48.9827540451 8.3905309576531 116.43218994141 0.080622577482985
|
||||
46704.948851642 48.982754781208 8.3905315422152 116.43134307861 0.080622577482985
|
||||
46705.958749245 48.98275551619 8.3905321274067 116.43042755127 0.080622577482985
|
||||
46706.968621846 48.982756254793 8.390532702215 116.43008422852 0.080622577482985
|
||||
46707.978487585 48.982756992215 8.3905332883045 116.42916870117 0.080622577482985
|
||||
46708.988386354 48.982757727889 8.3905338743741 116.4281463623 0.080622577482985
|
||||
46709.98885519 48.982758467237 8.3905344514471 116.42772674561 0.080622577482985
|
||||
46710.998316553 48.982759205108 8.3905350400643 116.42688751221 0.080622577482985
|
||||
46712.008057874 48.982759942394 8.3905356257311 116.42567443848 0.080622577482985
|
||||
46713.017859623 48.982760682746 8.3905362038632 116.4252166748 0.080622577482985
|
||||
46714.027834517 48.982761421614 8.390536795143 116.42449188232 0.080622577482985
|
||||
46715.037669162 48.982762158986 8.3905373825201 116.42335510254 0.080622577482985
|
||||
46716.047525746 48.982762899914 8.3905379621873 116.42309570312 0.080622577482985
|
||||
46717.057423688 48.982763639767 8.3905385529117 116.42258453369 0.080622577482985
|
||||
46718.067294693 48.982764378796 8.3905391439387 116.42183685303 0.080622577482985
|
||||
46719.077221407 48.982765120405 8.3905397167105 116.42156982422 0.080622577482985
|
||||
46720.087086385 48.982765861944 8.3905403089978 116.42120361328 0.080622577482985
|
||||
46721.096971349 48.982766601925 8.3905408998404 116.42071533203 0.080622577482985
|
||||
46722.106850907 48.982767344795 8.3905414751178 116.42057037354 0.080622577482985
|
||||
46723.116788501 48.982768087474 8.3905420703077 116.42043304443 0.080622577482985
|
||||
46724.126656325 48.982768829317 8.3905426637012 116.42024230957 0.080622577482985
|
||||
46725.136521327 48.982769573788 8.3905432417753 116.42042541504 0.080622577482985
|
||||
46726.146444588 48.982770317331 8.3905438374788 116.42044067383 0.080622577482985
|
||||
46727.15631358 48.982771059963 8.390544436255 116.4204864502 0.080622577482985
|
||||
46728.166175031 48.982771804898 8.3905450183055 116.42080688477 0.080622577482985
|
||||
46729.176043595 48.982772549197 8.3905456176303 116.42095947266 0.080622577482985
|
||||
46730.185902329 48.982773293658 8.3905462144165 116.42107391357 0.080622577482985
|
||||
46731.195981882 48.982774039986 8.3905467983712 116.42158508301 0.080622577482985
|
||||
46732.205726467 48.982774785338 8.3905473988558 116.42194366455 0.080622577482985
|
||||
46733.215597349 48.982775530703 8.3905480001049 116.42219543457 0.080622577482985
|
||||
46734.2254705084 48.982776278075 8.3905485866776 116.42295074463 0.080622577482985
|
||||
46735.235344035 48.982777025389 8.3905491888619 116.4235534668 0.080622577482985
|
||||
46736.245271197 48.982777771861 8.3905497891934 116.42386627197 0.080622577482985
|
||||
46737.255117766 48.982778516832 8.3905503657002 116.4239730835 0.080622577482985
|
||||
46738.2650876519 48.982779264985 8.3905509706213 116.42463684082 0.080622577482985
|
||||
46739.274940029 48.982780013031 8.3905515715727 116.42515563965 0.080622577482985
|
||||
46740.284789218 48.982780759009 8.3905521500366 116.4253692627 0.080622577482985
|
||||
46741.294848806 48.982781509121 8.3905527535196 116.42593383789 0.080622577482985
|
||||
46742.304675941 48.982782258002 8.3905533576055 116.42655181885 0.080622577482985
|
||||
46743.314413093 48.98278300548 8.3905539370032 116.42666625977 0.080622577482985
|
||||
46744.32430281 48.982783755614 8.3905545427999 116.42716217041 0.080622577482985
|
||||
46745.334199889 48.982784506718 8.3905551465813 116.42783355713 0.080622577482985
|
||||
46746.344135909 48.982785254644 8.3905557296408 116.4280166626 0.080622577482985
|
||||
46747.353994077 48.98278600651 8.3905563333539 116.42870330811 0.080622577482985
|
||||
46748.363884985 48.982786757263 8.3905569395728 116.42921447754 0.080622577482985
|
||||
46749.374031768 48.982787507124 8.3905575218474 116.42951202393 0.080622577482985
|
||||
46750.383688991 48.982788259217 8.3905581282131 116.43003845215 0.080622577482985
|
||||
46751.393653253 48.982789011669 8.3905587318394 116.43068695068 0.080622577482985
|
||||
46752.403473971 48.982789760939 8.390559317251 116.43074035645 0.080622577482985
|
||||
46753.403602427 48.982790510813 8.3905599249955 116.43103027344 0.080622577482985
|
||||
46754.413188468 48.982791263546 8.3905605333162 116.43153381348 0.080622577482985
|
||||
46755.423061958 48.982792012038 8.3905611229085 116.43146514893 0.080622577482985
|
||||
46756.423204203 48.982792764413 8.3905617318241 116.43164825439 0.080622577482985
|
||||
46757.432983107 48.982793513558 8.390562340213 116.43164825439 0.080622577482985
|
||||
46758.4429261 48.982794263593 8.3905629296594 116.43125152588 0.080622577482985
|
||||
46759.452664919 48.982795014317 8.3905635432651 116.43118286133 0.080622577482985
|
||||
46760.462763171 48.982795765668 8.3905641497346 116.43118286133 0.080622577482985
|
||||
46761.472614316 48.982796514489 8.3905647410515 116.43064117432 0.080622577482985
|
||||
46762.482352886 48.982797267491 8.3905653514117 116.43017578125 0.080622577482985
|
||||
46763.492344408 48.982798016499 8.3905659661196 116.4301071167 0.080622577482985
|
||||
46764.502100082 48.9827987666 8.3905665590305 116.42923736572 0.080622577482985
|
||||
46765.511960054 48.982799519896 8.3905671714197 116.42854309082 0.082036577207975
|
||||
46766.521836002 48.9828002724 8.3905677820126 116.42807006836 0.082036577207975
|
||||
46767.531690085 48.982801023669 8.3905683748596 116.42696380615 0.082036577207975
|
||||
46768.54155003 48.982801777889 8.3905689851418 116.4260559082 0.082036577207975
|
||||
46769.551447192 48.982802530802 8.3905695985423 116.42537689209 0.082036577207975
|
||||
46770.56136825 48.982803283213 8.3905701908568 116.42419433594 0.082036577207975
|
||||
46771.5712437513 48.982804038583 8.3905707999903 116.42325592041 0.082036577207975
|
||||
46772.581105489 48.982804793329 8.3905714110531 116.42241668701 0.082036577207975
|
||||
46773.590988261 48.982805548339 8.3905720025241 116.42127990723 0.082036577207975
|
||||
46774.590998862 48.982806307235 8.3905726081534 116.42029571533 0.082036577207975
|
||||
46775.601510578 48.982807062474 8.3905732182174 116.41941833496 0.082036577207975
|
||||
46776.610638865 48.982807819364 8.3905738073227 116.41822052002 0.082036577207975
|
||||
46777.620520041 48.982808578608 8.3905744129125 116.41734313965 0.082036577207975
|
||||
46778.630385861 48.982809337892 8.3905750184521 116.41647338867 0.082036577207975
|
||||
46779.640309875 48.982810096311 8.3905756068121 116.41547393799 0.082036577207975
|
||||
46780.650185468 48.982810857417 8.3905762113437 116.41484832764 0.082036577207975
|
||||
46781.660044356 48.982811616118 8.3905768176856 116.41408538818 0.082036577207975
|
||||
46782.66994556 48.982812374742 8.3905774081454 116.41355895996 0.082036577207975
|
||||
46783.679818911 48.982813139676 8.390578011424 116.41313934326 0.082036577207975
|
||||
46784.689851967 48.982813900691 8.3905786160058 116.41234588623 0.082036577207975
|
||||
46785.69960609 48.982814661447 8.3905792069628 116.41174316406 0.082036577207975
|
||||
46786.709503172 48.982815424186 8.390579818954 116.41158294678 0.082036577207975
|
||||
46787.71939825 48.982816193451 8.390580415035 116.41095733643 0.082036577207975
|
||||
46788.729256235 48.982816957308 8.3905810051983 116.4103012085 0.082036577207975
|
||||
46789.739177859 48.982817720312 8.3905816117547 116.41031646729 0.082036577207975
|
||||
46790.749042094 48.982818486234 8.390582224189 116.41033172607 0.082036577207975
|
||||
46791.758898514 48.982819250474 8.3905828171536 116.40995025635 0.082036577207975
|
||||
46792.768806224 48.982820017421 8.3905834214398 116.40942382812 0.082036577207975
|
||||
46793.778675536 48.982820781437 8.3905840350413 116.41022491455 0.082036577207975
|
||||
46794.788603495 48.982821683775 8.3905847362337 116.41995239258 0.082036577207975
|
||||
46795.798501681 48.98282245199 8.3905853420887 116.41973114014 0.082036577207975
|
||||
46796.808396108 48.982823218967 8.3905859504922 116.41987609863 0.082036577207975
|
||||
46797.818215132 48.982824114563 8.3905866468519 116.42946624756 0.082036577207975
|
||||
46798.818308841 48.982824883413 8.3905872550729 116.42949676514 0.082036577207975
|
||||
46799.827987497 48.982825652685 8.3905878609011 116.42971801758 0.082036577207975
|
||||
46800.837876469 48.982826545301 8.3905885508369 116.43894958496 0.082036577207975
|
||||
46801.847773501 48.982827318719 8.3905891563965 116.43909454346 0.082036577207975
|
||||
46802.857826602 48.982828086178 8.3905897659379 116.4393081665 0.082036577207975
|
||||
46803.867533794 48.98282897243 8.3905904527253 116.4481048584 0.082036577207975
|
||||
46804.877459869 48.982829744407 8.3905910609147 116.44842529297 0.082036577207975
|
||||
46805.887474934 48.98283051894 8.3905916639599 116.44847106934 0.082036577207975
|
||||
46806.897418464 48.982831402135 8.3905923435632 116.45683288574 0.082036577207975
|
||||
46807.907157586 48.982832177739 8.3905929478529 116.45709991455 0.082036577207975
|
||||
46808.90717593 48.982832949218 8.3905935556382 116.45735168457 0.082036577207975
|
||||
46809.907869815 48.982833827302 8.390594232297 116.46536254883 0.082036577207975
|
||||
46810.91699441 48.982834601363 8.3905948400989 116.46605682373 0.082036577207975
|
||||
46811.926819439 48.982835377671 8.3905954415824 116.46617126465 0.082036577207975
|
||||
46812.936591528 48.982836251578 8.3905961137986 116.47401428223 0.082036577207975
|
||||
46813.9464878608 48.982837028567 8.3905967178607 116.47451019287 0.082036577207975
|
||||
46814.9562604769 48.982837801725 8.3905973252199 116.47496795654 0.082036577207975
|
||||
46815.966211682 48.98283867111 8.3905979941926 116.48223114014 0.082036577207975
|
||||
46816.976103058 48.98283944856 8.390598598945 116.48239135742 0.082036577207975
|
||||
46817.986151499 48.98284022461 8.3905992015345 116.48271179199 0.082036577207975
|
||||
46818.995867047 48.982841089189 8.3905998658641 116.48942565918 0.082036577207975
|
||||
46820.005725162 48.982841864474 8.3906004728921 116.48993682861 0.082036577207975
|
||||
46821.015592639 48.982842641977 8.3906010738912 116.48977661133 0.082036577207975
|
||||
46822.025463197 48.982843502182 8.3906017340953 116.49605560303 0.082036577207975
|
||||
46823.035411626 48.982844279508 8.3906023395073 116.49614715576 0.082036577207975
|
||||
46824.045259766 48.982845053976 8.3906029461288 116.49662017822 0.082036577207975
|
||||
46825.055180106 48.982845910342 8.3906036063315 116.50254821777 0.082036577207975
|
||||
46826.065022328 48.982846685889 8.3906042124802 116.50285339355 0.082036577207975
|
||||
46827.07499507 48.982847462609 8.3906048189716 116.50286102295 0.082036577207975
|
||||
46828.084843058 48.982848313451 8.3906054770162 116.50853729248 0.082036577207975
|
||||
46829.085060376 48.982849088622 8.3906060849486 116.50869750977 0.082036577207975
|
||||
46830.094557513 48.982849864478 8.3906066910434 116.5089263916 0.082036577207975
|
||||
46831.104428817 48.98285071254 8.3906073455063 116.5142364502 0.082036577207975
|
||||
46832.1143589 48.98285148857 8.3906079521653 116.51425933838 0.082036577207975
|
||||
46833.12420917 48.982852264213 8.3906085603972 116.51451873779 0.082036577207975
|
||||
46834.134202648 48.982853109148 8.390609210975 116.51948547363 0.082036577207975
|
||||
46835.143965301 48.982853885196 8.3906098133192 116.51908111572 0.082036577207975
|
||||
46836.153847916 48.982854660168 8.3906104256572 116.51945495605 0.082036577207975
|
||||
46837.163757531 48.9828555 8.3906110763584 116.52411651611 0.082036577207975
|
||||
46838.173681359 48.982856277426 8.3906116787851 116.52375030518 0.082036577207975
|
||||
46839.183652268 48.982857052609 8.3906122820438 116.52353668213 0.082036577207975
|
||||
46840.1934873715 48.982857889966 8.390612927567 116.52803039551 0.082036577207975
|
||||
46841.2033212864 48.982858664365 8.3906135370946 116.528465271 0.082036577207975
|
||||
46842.213171913 48.982859443718 8.3906141327327 116.52767181396 0.082036577207975
|
||||
46843.2230505753 48.982860278347 8.3906147760637 116.53214263916 0.082036577207975
|
||||
46844.233021359 48.982861056432 8.3906153813738 116.53266906738 0.082036577207975
|
||||
46845.242827709 48.982861831067 8.390615987121 116.53270721436 0.082036577207975
|
||||
46846.252707418 48.982862666806 8.390616631132 116.5368347168 0.082036577207975
|
||||
46847.262581497 48.982863444114 8.3906172347912 116.53695678711 0.082036577207975
|
||||
46848.263775179 48.982864222396 8.3906178400113 116.53759002686 0.082036577207975
|
||||
46849.272421171 48.982865060887 8.390618472133 116.54067993164 0.082036577207975
|
||||
46850.282387429 48.982865838632 8.3906190702191 116.54047393799 0.082036577207975
|
||||
46851.292143649 48.982866617193 8.3906196734496 116.54051208496 0.082036577207975
|
||||
46852.302018107 48.982867453848 8.3906202999398 116.54296875 0.082036577207975
|
||||
46853.311916991 48.982868230241 8.3906209019735 116.54236602783 0.082036577207975
|
||||
46854.321779125 48.982869009414 8.3906214966155 116.54215240479 0.082036577207975
|
||||
46855.331680705 48.982869841349 8.3906221216796 116.54433441162 0.082036577207975
|
||||
46856.341607069 48.982870617754 8.3906227181505 116.54376220703 0.082036577207975
|
||||
46857.351482383 48.982871395325 8.390623319339 116.54332733154 0.082036577207975
|
||||
46858.36137011 48.982872225359 8.3906239405806 116.54535675049 0.082036577207975
|
||||
46859.371230785 48.982873003951 8.3906245372592 116.54467010498 0.082036577207975
|
||||
46860.38128337 48.982873779954 8.3906251339677 116.54420471191 0.082036577207975
|
||||
46861.391097064 48.982874608784 8.3906257519197 116.54597473145 0.082036577207975
|
||||
46862.400959735 48.982875384727 8.3906263484643 116.54532623291 0.082036577207975
|
||||
46863.401052759 48.982876162561 8.3906269461274 116.54484558105 0.082036577207975
|
||||
46864.410741881 48.982876986259 8.3906275638686 116.54658508301 0.082036577207975
|
||||
46865.420591932 48.98287776098 8.3906281614249 116.54601287842 0.082036577207975
|
||||
46866.42062093 48.982878537151 8.3906287584467 116.54550170898 0.051107729356723
|
||||
46867.430552878 48.982879357927 8.3906293769422 116.54716491699 0.051107729356723
|
||||
46868.440466026 48.982880131379 8.390629978178 116.54672241211 0.051107729356723
|
||||
46869.450253478 48.982880906417 8.3906305748197 116.54621887207 0.051107729356723
|
||||
46870.46005645 48.982881722937 8.3906311931494 116.54778289795 0.051107729356723
|
||||
46871.469953376 48.982882495414 8.3906317945865 116.54707336426 0.051107729356723
|
||||
46872.479820573 48.98288326942 8.3906323951183 116.54679107666 0.051107729356723
|
||||
46873.489660312 48.982884083298 8.3906330123468 116.54796600342 0.051107729356723
|
||||
46874.499567335 48.982884854689 8.3906336132853 116.54708099365 0.051107729356723
|
||||
46875.509389077 48.982885629144 8.3906342116196 116.5464553833 0.051107729356723
|
||||
46876.519260651 48.982886441349 8.390634824916 116.54724121094 0.051107729356723
|
||||
46877.529171242 48.982887213032 8.3906354211233 116.54600524902 0.051107729356723
|
||||
46878.53905883 48.982887986134 8.3906360224312 116.54542541504 0.051107729356723
|
||||
46879.548911895 48.982888796636 8.3906366324532 116.54594421387 0.051107729356723
|
||||
46880.558793117 48.982889568954 8.3906372251448 116.54445648193 0.051107729356723
|
||||
46881.568737272 48.982890342191 8.390637823128 116.54376983643 0.051107729356723
|
||||
46882.57860618 48.982891151555 8.3906384299057 116.54433441162 0.051107729356723
|
||||
46883.588504241 48.982891923664 8.3906390190492 116.54323577881 0.051107729356723
|
||||
46884.598350895 48.982892696528 8.3906396147454 116.54219055176 0.051107729356723
|
||||
46885.608245086 48.982893502987 8.3906402195885 116.54307556152 0.051107729356723
|
||||
46886.608962596 48.982894275782 8.3906408091756 116.54232025146 0.051107729356723
|
||||
46887.618000059 48.982895047893 8.3906413990678 116.54134368896 0.051107729356723
|
||||
46888.627911017 48.982895853122 8.3906420014676 116.54220581055 0.051107729356723
|
||||
46889.63778101 48.982896624338 8.3906425945668 116.54165649414 0.051107729356723
|
||||
46890.647729544 48.98289739711 8.3906431830809 116.54082489014 0.051107729356723
|
||||
46891.657573441 48.982898187537 8.3906437891796 116.54415893555 0.051107729356723
|
||||
46892.667424349 48.982898958013 8.3906443826796 116.54379272461 0.051107729356723
|
||||
46893.677449523 48.982899728351 8.3906449761701 116.54307556152 0.051107729356723
|
||||
46894.687816687 48.982900516959 8.3906455824252 116.5463104248 0.051107729356723
|
||||
46895.697074962 48.982901287142 8.390646176352 116.54584503174 0.051107729356723
|
||||
46896.706958146 48.982902056827 8.3906467708509 116.54550170898 0.051107729356723
|
||||
46897.716839862 48.982902844001 8.3906473788662 116.54862976074 0.051107729356723
|
||||
46898.72674478 48.982903612441 8.3906479753366 116.54859161377 0.051107729356723
|
||||
46899.73665572 48.982904382145 8.3906485681821 116.54795837402 0.051107729356723
|
||||
46900.746541701 48.982905167443 8.3906491746946 116.55098724365 0.051107729356723
|
||||
46901.756401271 48.982905936067 8.3906497741015 116.5509185791 0.051107729356723
|
||||
46902.766336295 48.982906703654 8.3906503685681 116.55074310303 0.051107729356723
|
||||
46903.776295577 48.982907488848 8.3906509747703 116.55358886719 0.051107729356723
|
||||
46904.786063798 48.982908258745 8.3906515694027 116.55320739746 0.051107729356723
|
||||
46905.795921386 48.982909025279 8.3906521701153 116.55303955078 0.051107729356723
|
||||
46906.805820038 48.982909807752 8.3906527770638 116.55569458008 0.051107729356723
|
||||
46907.81569973 48.982910573984 8.3906533739064 116.55563354492 0.051107729356723
|
||||
46908.825612749 48.982911345127 8.3906539675577 116.55488586426 0.051107729356723
|
||||
46909.835528661 48.982912128347 8.3906545698534 116.55732727051 0.051107729356723
|
||||
46910.845371598 48.982912896961 8.3906551646956 116.55702972412 0.051107729356723
|
||||
46911.855258862 48.982913662591 8.3906557612821 116.55693054199 0.051107729356723
|
||||
46912.865207373 48.982914444259 8.3906563621292 116.55935668945 0.051107729356723
|
||||
46913.875217009 48.982915211568 8.3906569523297 116.55922698975 0.051107729356723
|
||||
46914.885120318 48.982915979685 8.3906575480123 116.55889892578 0.051107729356723
|
||||
46915.894922607 48.98291675962 8.3906581462098 116.56122589111 0.051107729356723
|
||||
46916.90474484 48.982917526374 8.3906587357421 116.56113433838 0.051107729356723
|
||||
46917.904794336 48.982918293947 8.3906593238035 116.56090545654 0.051107729356723
|
||||
46918.914687072 48.982919074075 8.3906599162375 116.5630645752 0.051107729356723
|
||||
46919.924363772 48.982919840744 8.3906605018862 116.5629119873 0.051107729356723
|
||||
46920.934238628 48.982920607274 8.3906610897463 116.56280517578 0.051107729356723
|
||||
46921.944148956 48.982921385183 8.3906616804399 116.5648651123 0.051107729356723
|
||||
46922.954041151 48.982922149771 8.3906622646921 116.56478881836 0.051107729356723
|
||||
46923.963896322 48.982922916584 8.3906628498759 116.56467437744 0.051107729356723
|
||||
46924.963914916 48.982923693477 8.3906634381239 116.56662750244 0.051107729356723
|
||||
46925.973907879 48.982924458805 8.3906640185973 116.56633758545 0.051107729356723
|
||||
46926.98360197 48.982925222803 8.3906646047301 116.56642913818 0.051107729356723
|
||||
46927.993410546 48.982925999121 8.3906651916631 116.56811523438 0.051107729356723
|
||||
46929.00332416 48.982926763011 8.3906657719421 116.5676651001 0.051107729356723
|
||||
46930.013212187 48.982927528198 8.3906663498705 116.567237854 0.051107729356723
|
||||
46931.023073382 48.982928303727 8.3906669308441 116.56831359863 0.051107729356723
|
||||
46932.032955131 48.982929068023 8.3906675097025 116.56716156006 0.051107729356723
|
||||
46933.042933769 48.982929829753 8.3906680890171 116.56691741943 0.051107729356723
|
||||
46934.052725383 48.982930602284 8.3906686719863 116.56777191162 0.051107729356723
|
||||
46935.062646001 48.982931366342 8.3906692442368 116.56658172607 0.051107729356723
|
||||
46936.072520176 48.982932129351 8.390669826704 116.5657043457 0.051107729356723
|
||||
46937.082411589 48.982932901609 8.3906704077574 116.56665802002 0.051107729356723
|
||||
46938.092271378 48.982933662859 8.3906709841994 116.56603240967 0.051107729356723
|
||||
46939.102148658 48.982934427132 8.3906715581898 116.56452178955 0.051107729356723
|
||||
46940.112289119 48.982935197978 8.3906721384476 116.56531524658 0.051107729356723
|
||||
46941.121952338 48.982935958395 8.3906727173536 116.56481933594 0.051107729356723
|
||||
46942.131875512 48.982936719487 8.3906732932631 116.56387329102 0.051107729356723
|
||||
46943.141680875 48.98293748497 8.390673887836 116.5650100708 0.051107729356723
|
||||
46944.151564554 48.982938244467 8.3906744652488 116.56477355957 0.051107729356723
|
||||
46945.161504412 48.982939004431 8.3906750442956 116.56379699707 0.051107729356723
|
||||
46946.171454778 48.982939769483 8.390675637866 116.56485748291 0.051107729356723
|
||||
46947.181257826 48.982940530955 8.3906762123217 116.56439971924 0.051107729356723
|
||||
46948.191114611 48.982941288677 8.3906767919343 116.56402587891 0.051107729356723
|
||||
46949.20102244 48.982942052746 8.3906773866092 116.56523132324 0.051107729356723
|
||||
46950.210877036 48.982942812279 8.3906779630193 116.56516265869 0.051107729356723
|
||||
46951.220789111 48.982943572185 8.3906785378907 116.56446838379 0.051107729356723
|
||||
46952.230758043 48.982944335186 8.3906791304873 116.56582641602 0.051107729356723
|
||||
46953.24055257 48.982945093931 8.3906797106438 116.56645965576 0.051107729356723
|
||||
46954.250444413 48.98294585271 8.3906802836285 116.56587219238 0.051107729356723
|
||||
46955.260295656 48.982946615954 8.3906808743532 116.5673828125 0.051107729356723
|
||||
46956.270232748 48.982947374619 8.3906814556262 116.56803894043 0.051107729356723
|
||||
46957.280117293 48.982948132675 8.3906820322656 116.56848144531 0.051107729356723
|
||||
46958.289976632 48.982948894621 8.3906826232925 116.57034301758 0.051107729356723
|
||||
46959.299886859 48.982949652104 8.3906831989033 116.57125091553 0.051107729356723
|
||||
46960.309765589 48.982950410373 8.3906837808762 116.57192230225 0.051107729356723
|
||||
46961.319655022 48.982951171478 8.3906843699989 116.57392120361 0.051107729356723
|
||||
46962.329479781 48.982951929485 8.3906849447623 116.57461547852 0.051107729356723
|
||||
46963.339424565 48.982952686833 8.3906855229563 116.57586669922 0.051107729356723
|
||||
46964.349275317 48.982953447369 8.3906861125618 116.5778427124 0.051107729356723
|
||||
46965.359168118 48.982954201875 8.3906866891254 116.57861328125 0.051107729356723
|
||||
46966.369233692 48.982954960951 8.3906872643341 116.57935333252 0.051107729356723
|
||||
46967.379188594 48.982955719952 8.390687853023 116.58097839355 0.033600595232823
|
||||
46968.388966176 48.982956474316 8.3906884312723 116.58137512207 0.033600595232823
|
||||
46969.398909085 48.982957229088 8.3906890059952 116.58237457275 0.033600595232823
|
||||
46970.408716186 48.982957987257 8.3906895936359 116.58386993408 0.033600595232823
|
||||
46971.418726445 48.982958742188 8.390690164902 116.58389282227 0.033600595232823
|
||||
46972.428590244 48.982959496062 8.3906907446756 116.58442687988 0.033600595232823
|
||||
46973.438313836 48.982960252931 8.3906913311975 116.58554077148 0.033600595232823
|
||||
46974.438340286 48.982961004717 8.3906919069974 116.5856552124 0.033600595232823
|
||||
46975.448185205 48.982961760777 8.3906924757347 116.58567810059 0.033600595232823
|
||||
46976.45797584 48.982962517438 8.390693058565 116.58670043945 0.033600595232823
|
||||
46977.467846198 48.982963270234 8.3906936304815 116.58670043945 0.033600595232823
|
||||
46978.467898342 48.982964021779 8.3906942050281 116.58692932129 0.033600595232823
|
||||
46979.477596612 48.982964776881 8.3906947869431 116.58779144287 0.033600595232823
|
||||
46980.487476114 48.982965528223 8.39069535983 116.58764648438 0.033600595232823
|
||||
46981.497362615 48.982966280339 8.390695930908 116.58778381348 0.033600595232823
|
||||
46982.507202152 48.982967033776 8.390696512084 116.58861541748 0.033600595232823
|
||||
46983.517118638 48.982967785002 8.3906970814261 116.58840942383 0.033600595232823
|
||||
46984.526948992 48.982968535949 8.390697654384 116.58837127686 0.033600595232823
|
||||
46985.536857947 48.982969289053 8.3906982332511 116.58909606934 0.033600595232823
|
||||
46986.546737498 48.982970038133 8.3906988038223 116.58887481689 0.033600595232823
|
||||
46987.556657323 48.982970790436 8.3906993715514 116.58875274658 0.033600595232823
|
||||
46988.566715759 48.982971542455 8.3906999499718 116.58930206299 0.033600595232823
|
||||
46989.576409117 48.982972290445 8.3907005207007 116.58917999268 0.033600595232823
|
||||
46990.58628031 48.982973040192 8.39070109028 116.5890045166 0.033600595232823
|
||||
46991.596233106 48.982973791475 8.3907016679585 116.58950805664 0.033600595232823
|
||||
46992.60605425 48.982974539908 8.3907022362086 116.58917999268 0.033600595232823
|
||||
46993.616026659 48.982975287272 8.3907028082783 116.5890045166 0.033600595232823
|
||||
46994.625821268 48.982976035033 8.3907034039939 116.58915710449 0.033600595232823
|
||||
46995.635751899 48.982976780843 8.3907039749871 116.58876800537 0.033600595232823
|
||||
46996.645591573 48.982977528466 8.3907045449079 116.58850860596 0.033600595232823
|
||||
46997.655484934 48.982978274477 8.3907051410204 116.58868408203 0.033600595232823
|
||||
46998.665530916 48.982979020186 8.3907057102071 116.58827972412 0.033600595232823
|
||||
46999.675265207 48.982979765191 8.3907062832539 116.58805847168 0.033600595232823
|
||||
47000.685218422 48.982980510851 8.3907068771266 116.5881729126 0.033600595232823
|
||||
47001.695029865 48.982981256295 8.3907074448941 116.58788299561 0.033600595232823
|
||||
47002.704901935 48.982982001613 8.3907080148377 116.58746337891 0.033600595232823
|
||||
47003.715449642 48.982982746538 8.390708606401 116.58760070801 0.033600595232823
|
||||
47004.724710736 48.982983491137 8.3907091727507 116.58738708496 0.033600595232823
|
||||
47005.734604637 48.982984236533 8.3907097406197 116.58688354492 0.033600595232823
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz UpdatePeriod
|
||||
-0.0147725597895258 7.22876903268996e-006 -1.49304056640669e-005 -0.809818318663195 0.32628214806349 -0.797376731139142 1
|
||||
|
|
@ -0,0 +1,126 @@
|
|||
%close all
|
||||
%clc
|
||||
|
||||
import gtsam.*;
|
||||
|
||||
%% Read data
|
||||
IMU_metadata = importdata(gtsam.findExampleDataFile('KittiEquivBiasedImu_metadata.txt'));
|
||||
IMU_data = importdata(gtsam.findExampleDataFile('KittiEquivBiasedImu.txt'));
|
||||
% Make text file column headers into struct fields
|
||||
IMU_metadata = cell2struct(num2cell(IMU_metadata.data), IMU_metadata.colheaders, 2);
|
||||
IMU_data = cell2struct(num2cell(IMU_data.data), IMU_data.colheaders, 2);
|
||||
|
||||
GPS_metadata = importdata(gtsam.findExampleDataFile('KittiGps_metadata.txt'));
|
||||
GPS_data = importdata(gtsam.findExampleDataFile('KittiGps.txt'));
|
||||
% Make text file column headers into struct fields
|
||||
GPS_metadata = cell2struct(num2cell(GPS_metadata.data), GPS_metadata.colheaders, 2);
|
||||
GPS_data = cell2struct(num2cell(GPS_data.data), GPS_data.colheaders, 2);
|
||||
|
||||
%% Convert GPS from lat/long to meters
|
||||
[ x, y, ~ ] = deg2utm( [GPS_data.Latitude], [GPS_data.Longitude] );
|
||||
for i = 1:numel(x)
|
||||
GPS_data(i).Position = gtsam.Point3(x(i), y(i), GPS_data(i).Altitude);
|
||||
end
|
||||
|
||||
% % Calculate GPS sigma in meters
|
||||
% [ xSig, ySig, ~ ] = deg2utm( [GPS_data.Latitude] + [GPS_data.PositionSigma], ...
|
||||
% [GPS_data.Longitude] + [GPS_data.PositionSigma]);
|
||||
% xSig = xSig - x;
|
||||
% ySig = ySig - y;
|
||||
|
||||
%% Start at time of first GPS measurement
|
||||
firstGPSPose = 2;
|
||||
|
||||
%% Get initial conditions for the estimated trajectory
|
||||
currentPoseGlobal = Pose3(Rot3, GPS_data(firstGPSPose).Position); % initial pose is the reference frame (navigation frame)
|
||||
currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning
|
||||
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
|
||||
|
||||
%% Solver object
|
||||
isamParams = ISAM2Params;
|
||||
isamParams.setFactorization('QR');
|
||||
isamParams.setRelinearizeSkip(1);
|
||||
isam = gtsam.ISAM2(isamParams);
|
||||
newFactors = NonlinearFactorGraph;
|
||||
newValues = Values;
|
||||
|
||||
%% Create initial estimate and prior on initial pose, velocity, and biases
|
||||
newValues.insert(symbol('x',firstGPSPose), currentPoseGlobal);
|
||||
newValues.insert(symbol('v',firstGPSPose), currentVelocityGlobal);
|
||||
newValues.insert(symbol('b',1), currentBias);
|
||||
|
||||
sigma_init_x = noiseModel.Diagonal.Precisions([0;0;0; 1;1;1]);
|
||||
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0);
|
||||
sigma_init_b = noiseModel.Isotropic.Sigma(6, 100000.0);
|
||||
|
||||
newFactors.add(PriorFactorPose3(symbol('x',firstGPSPose), currentPoseGlobal, sigma_init_x));
|
||||
newFactors.add(PriorFactorLieVector(symbol('v',firstGPSPose), currentVelocityGlobal, sigma_init_v));
|
||||
newFactors.add(PriorFactorConstantBias(symbol('b',1), currentBias, sigma_init_b));
|
||||
|
||||
%% Main loop:
|
||||
% (1) we read the measurements
|
||||
% (2) we create the corresponding factors in the graph
|
||||
% (3) we solve the graph to obtain and optimal estimate of robot trajectory
|
||||
|
||||
for poseIndex = firstGPSPose:length(GPS_data)
|
||||
% At each non=IMU measurement we initialize a new node in the graph
|
||||
currentPoseKey = symbol('x',poseIndex);
|
||||
currentVelKey = symbol('v',poseIndex);
|
||||
currentBiasKey = symbol('b',1);
|
||||
|
||||
if poseIndex > firstGPSPose
|
||||
% Summarize IMU data between the previous GPS measurement and now
|
||||
IMUindices = find([IMU_data.Time] > GPS_data(poseIndex-1).Time ...
|
||||
& [IMU_data.Time] <= GPS_data(poseIndex).Time);
|
||||
|
||||
currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
|
||||
currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
|
||||
IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3));
|
||||
|
||||
for imuIndex = IMUindices
|
||||
accMeas = [ IMU_data(imuIndex).accelX; IMU_data(imuIndex).accelY; IMU_data(imuIndex).accelZ ];
|
||||
omegaMeas = [ IMU_data(imuIndex).omegaX; IMU_data(imuIndex).omegaY; IMU_data(imuIndex).omegaZ ];
|
||||
deltaT = IMU_data(imuIndex).dt;
|
||||
currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT);
|
||||
end
|
||||
|
||||
% Create IMU factor
|
||||
newFactors.add(ImuFactor( ...
|
||||
currentPoseKey-1, currentVelKey-1, ...
|
||||
currentPoseKey, currentVelKey, ...
|
||||
currentBiasKey, currentSummarizedMeasurement, [0;0;-9.8], [0;0;0]));
|
||||
|
||||
% Create GPS factor
|
||||
newFactors.add(PriorFactorPose3(currentPoseKey, Pose3(currentPoseGlobal.rotation, GPS_data(poseIndex).Position), ...
|
||||
noiseModel.Diagonal.Precisions([ zeros(3,1); 1./(GPS_data(poseIndex).PositionSigma).^2*ones(3,1) ])));
|
||||
|
||||
% Add initial value
|
||||
newValues.insert(currentPoseKey, Pose3(currentPoseGlobal.rotation, GPS_data(poseIndex).Position));
|
||||
newValues.insert(currentVelKey, currentVelocityGlobal);
|
||||
%newValues.insert(currentBiasKey, currentBias);
|
||||
|
||||
% Update solver
|
||||
% =======================================================================
|
||||
isam.update(newFactors, newValues);
|
||||
newFactors = NonlinearFactorGraph;
|
||||
newValues = Values;
|
||||
|
||||
cla;
|
||||
plot3DTrajectory(isam.calculateEstimate, 'g-');
|
||||
drawnow;
|
||||
% =======================================================================
|
||||
|
||||
currentPoseGlobal = isam.calculateEstimate(currentPoseKey);
|
||||
currentVelocityGlobal = isam.calculateEstimate(currentVelKey);
|
||||
currentBias = isam.calculateEstimate(currentBiasKey);
|
||||
|
||||
end
|
||||
end
|
||||
|
||||
disp('TODO: display results')
|
||||
% figure(1)
|
||||
% hold on;
|
||||
% plot(positions(1,:), positions(2,:), '-b');
|
||||
% plot3DTrajectory(isam.calculateEstimate, 'g-');
|
||||
% axis equal;
|
||||
% legend('true trajectory', 'traj integrated in body', 'traj integrated in nav')
|
||||
|
|
@ -0,0 +1,133 @@
|
|||
function [x,y,utmzone] = deg2utm(Lat,Lon)
|
||||
% -------------------------------------------------------------------------
|
||||
% [x,y,utmzone] = deg2utm(Lat,Lon)
|
||||
%
|
||||
% Description: Function to convert lat/lon vectors into UTM coordinates (WGS84).
|
||||
% Some code has been extracted from UTM.m function by Gabriel Ruiz Martinez.
|
||||
%
|
||||
% Inputs:
|
||||
% Lat: Latitude vector. Degrees. +ddd.ddddd WGS84
|
||||
% Lon: Longitude vector. Degrees. +ddd.ddddd WGS84
|
||||
%
|
||||
% Outputs:
|
||||
% x, y , utmzone. See example
|
||||
%
|
||||
% Example 1:
|
||||
% Lat=[40.3154333; 46.283900; 37.577833; 28.645650; 38.855550; 25.061783];
|
||||
% Lon=[-3.4857166; 7.8012333; -119.95525; -17.759533; -94.7990166; 121.640266];
|
||||
% [x,y,utmzone] = deg2utm(Lat,Lon);
|
||||
% fprintf('%7.0f ',x)
|
||||
% 458731 407653 239027 230253 343898 362850
|
||||
% fprintf('%7.0f ',y)
|
||||
% 4462881 5126290 4163083 3171843 4302285 2772478
|
||||
% utmzone =
|
||||
% 30 T
|
||||
% 32 T
|
||||
% 11 S
|
||||
% 28 R
|
||||
% 15 S
|
||||
% 51 R
|
||||
%
|
||||
% Example 2: If you have Lat/Lon coordinates in Degrees, Minutes and Seconds
|
||||
% LatDMS=[40 18 55.56; 46 17 2.04];
|
||||
% LonDMS=[-3 29 8.58; 7 48 4.44];
|
||||
% Lat=dms2deg(mat2dms(LatDMS)); %convert into degrees
|
||||
% Lon=dms2deg(mat2dms(LonDMS)); %convert into degrees
|
||||
% [x,y,utmzone] = deg2utm(Lat,Lon)
|
||||
%
|
||||
% Author:
|
||||
% Rafael Palacios
|
||||
% Universidad Pontificia Comillas
|
||||
% Madrid, Spain
|
||||
% Version: Apr/06, Jun/06, Aug/06, Aug/06
|
||||
% Aug/06: fixed a problem (found by Rodolphe Dewarrat) related to southern
|
||||
% hemisphere coordinates.
|
||||
% Aug/06: corrected m-Lint warnings
|
||||
%-------------------------------------------------------------------------
|
||||
|
||||
% Argument checking
|
||||
%
|
||||
error(nargchk(2, 2, nargin)); %2 arguments required
|
||||
n1=length(Lat);
|
||||
n2=length(Lon);
|
||||
if (n1~=n2)
|
||||
error('Lat and Lon vectors should have the same length');
|
||||
end
|
||||
|
||||
|
||||
% Memory pre-allocation
|
||||
%
|
||||
x=zeros(n1,1);
|
||||
y=zeros(n1,1);
|
||||
utmzone(n1,:)='60 X';
|
||||
|
||||
% Main Loop
|
||||
%
|
||||
for i=1:n1
|
||||
la=Lat(i);
|
||||
lo=Lon(i);
|
||||
|
||||
sa = 6378137.000000 ; sb = 6356752.314245;
|
||||
|
||||
%e = ( ( ( sa ^ 2 ) - ( sb ^ 2 ) ) ^ 0.5 ) / sa;
|
||||
e2 = ( ( ( sa ^ 2 ) - ( sb ^ 2 ) ) ^ 0.5 ) / sb;
|
||||
e2cuadrada = e2 ^ 2;
|
||||
c = ( sa ^ 2 ) / sb;
|
||||
%alpha = ( sa - sb ) / sa; %f
|
||||
%ablandamiento = 1 / alpha; % 1/f
|
||||
|
||||
lat = la * ( pi / 180 );
|
||||
lon = lo * ( pi / 180 );
|
||||
|
||||
Huso = fix( ( lo / 6 ) + 31);
|
||||
S = ( ( Huso * 6 ) - 183 );
|
||||
deltaS = lon - ( S * ( pi / 180 ) );
|
||||
|
||||
if (la<-72), Letra='C';
|
||||
elseif (la<-64), Letra='D';
|
||||
elseif (la<-56), Letra='E';
|
||||
elseif (la<-48), Letra='F';
|
||||
elseif (la<-40), Letra='G';
|
||||
elseif (la<-32), Letra='H';
|
||||
elseif (la<-24), Letra='J';
|
||||
elseif (la<-16), Letra='K';
|
||||
elseif (la<-8), Letra='L';
|
||||
elseif (la<0), Letra='M';
|
||||
elseif (la<8), Letra='N';
|
||||
elseif (la<16), Letra='P';
|
||||
elseif (la<24), Letra='Q';
|
||||
elseif (la<32), Letra='R';
|
||||
elseif (la<40), Letra='S';
|
||||
elseif (la<48), Letra='T';
|
||||
elseif (la<56), Letra='U';
|
||||
elseif (la<64), Letra='V';
|
||||
elseif (la<72), Letra='W';
|
||||
else Letra='X';
|
||||
end
|
||||
|
||||
a = cos(lat) * sin(deltaS);
|
||||
epsilon = 0.5 * log( ( 1 + a) / ( 1 - a ) );
|
||||
nu = atan( tan(lat) / cos(deltaS) ) - lat;
|
||||
v = ( c / ( ( 1 + ( e2cuadrada * ( cos(lat) ) ^ 2 ) ) ) ^ 0.5 ) * 0.9996;
|
||||
ta = ( e2cuadrada / 2 ) * epsilon ^ 2 * ( cos(lat) ) ^ 2;
|
||||
a1 = sin( 2 * lat );
|
||||
a2 = a1 * ( cos(lat) ) ^ 2;
|
||||
j2 = lat + ( a1 / 2 );
|
||||
j4 = ( ( 3 * j2 ) + a2 ) / 4;
|
||||
j6 = ( ( 5 * j4 ) + ( a2 * ( cos(lat) ) ^ 2) ) / 3;
|
||||
alfa = ( 3 / 4 ) * e2cuadrada;
|
||||
beta = ( 5 / 3 ) * alfa ^ 2;
|
||||
gama = ( 35 / 27 ) * alfa ^ 3;
|
||||
Bm = 0.9996 * c * ( lat - alfa * j2 + beta * j4 - gama * j6 );
|
||||
xx = epsilon * v * ( 1 + ( ta / 3 ) ) + 500000;
|
||||
yy = nu * v * ( 1 + ta ) + Bm;
|
||||
|
||||
if (yy<0)
|
||||
yy=9999999+yy;
|
||||
end
|
||||
|
||||
x(i)=xx;
|
||||
y(i)=yy;
|
||||
utmzone(i,:)=sprintf('%02d %c',Huso,Letra);
|
||||
end
|
||||
|
||||
Loading…
Reference in New Issue