From 27eda640d317b87f004f211a320fdb9b5070d773 Mon Sep 17 00:00:00 2001
From: William Chen <billyskc@ucla.edu>
Date: Thu, 16 May 2019 18:25:43 -0700
Subject: [PATCH] add dataset analysis to realworld dataset manager
---
CoLo-AT/Cooperative_Localization_Journal1.py | 12 +++---
.../__pycache__/data_analyzer.cpython-36.pyc | Bin 6943 -> 6989 bytes
.../__pycache__/realtime_plot.cpython-36.pyc | Bin 5825 -> 5753 bytes
CoLo-AT/data_analysis/data_analyzer.py | 1 -
CoLo-AT/data_analysis/dataset_analyzer.py | 35 ------------------
CoLo-AT/dataset_analysis.py | 24 ++++++++++++
.../realworld_dataset_manager.cpython-36.pyc | Bin 13030 -> 14098 bytes
.../realworld_dataset_manager.py | 33 +++++++++++++++++
CoLo-AT/localization_algos/ekf_gs_bound.py | 26 ++++++++-----
9 files changed, 78 insertions(+), 53 deletions(-)
delete mode 100644 CoLo-AT/data_analysis/dataset_analyzer.py
create mode 100644 CoLo-AT/dataset_analysis.py
diff --git a/CoLo-AT/Cooperative_Localization_Journal1.py b/CoLo-AT/Cooperative_Localization_Journal1.py
index d48eb2e..445f809 100644
--- a/CoLo-AT/Cooperative_Localization_Journal1.py
+++ b/CoLo-AT/Cooperative_Localization_Journal1.py
@@ -43,8 +43,6 @@ state_recorder = StatesRecorder('GS_CI', robot_labels)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False)
loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
-#animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
-
##############################################################################
testing_dataset.dataset_reset()
@@ -54,7 +52,7 @@ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
sim = SimulationManager('sim gs_ci_bound')
state_recorder_bound = StatesRecorder('GS_CI_bound', robot_labels, state_var_only = True)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_bound, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = True)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = False)
##############################################################################
@@ -66,7 +64,7 @@ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim cen_ekf')
state_recorder_LS_cen = StatesRecorder('LS_cen', robot_labels)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_cen, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_cen, plot_graphs = True)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_cen, plot_graphs = False)
##############################################################################
@@ -77,7 +75,7 @@ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim ls_bda')
state_recorder_LS_BDA = StatesRecorder('LS_BDA', robot_labels)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_BDA, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_BDA, plot_graphs = True)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_BDA, plot_graphs = False)
##############################################################################
@@ -89,7 +87,7 @@ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim ls_ci')
state_recorder_LS_CI = StatesRecorder('LS_CI', robot_labels)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_CI, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_CI, plot_graphs = True)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_CI, plot_graphs = False)
##############################################################################
@@ -100,6 +98,6 @@ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
sim = SimulationManager('sim gs_sci')
state_recorder_GS_SCI = StatesRecorder('GS_SCI', robot_labels)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_GS_SCI, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_SCI, plot_graphs = True)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_SCI, plot_graphs = False)
analyzer.algos_comparison([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI, state_recorder_bound], only_trace = ['GS_CI_bound'])
diff --git a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc
index dc6edb64e90bd2206b2c9cd8468823513f13048e..dffc9d42f386adb855d92b044eb516afee063aa8 100644
GIT binary patch
delta 567
zcmY+9u}|AT7{&cvh%t^2f<%LpO085>Pyn$&5dx&NNn2o}Ekz6<Ai~f_NgQ1aES55O
zWk9+=0I{?)@&p6_K`V7YLLOMF{0$w-y-Szg@Vn>tz4ys?G5=%Up5@#wI}h90i7T5~
zv=DRanyNv6%?bx2(l?K2)X-`uBG-tBjKB=3HgJ%M81#V?*rB7+8Sts`&*jR^YFDx6
zhN*KVW<sOpNYnmBT;ALMK*E|Wkv0q?Udh}N32E{#DfWV5pZ{i^k7^)30UN+)g(THH
zZfqo(hPU577GA?^1|o%_&dx!h(A6GVU2NCYt_igXFoj}Yuk-)@?q79^{o<8M$h+)x
zv4s3<pbS)ix4=69i^O|iC7vAq!{{&`A6aGeM*6vl>^cSxun4r|x7@RW57_{kfZo3f
zwH80*&dBUNTDIINv-fIw>Atj%RH8`YQ2unc#!m1tC^%X!kXK03!PlVfi7Je3xt3oV
z!|ucpe$@|l>-$x&-WJoC?f@3BC%@-U>4SV;C_T}CtJ^J%+CWFP3zf83R$AT^yhkS&
BezO1o
delta 545
zcmY+=Jx|*}7zc3Q1%mMlY9ydZI-&v=EC|Q|Qh6(F1|&)e0*OeL(;R{*iK8bDU7~i#
z+5zd-AovQ!Q<r{&Rz+g6#7r~r1<FwF@37$E=g0qhp0n<L<t}o@yloqQ?r)1%liv-l
zv+>Z@f3Vdys|=OR4rf}BEL(xzwmK{rhOe}z)K=+>I92x3cH)ea)z&J8oHnkRDgPOM
zNajz*8gkA)W{I$C-|??Inx@f5unP)`C{b_K3Xxe8Ub97_Ch7qtuo(9)pu#{e&M?&*
zM268`64f5y3TL1{^L+o_fAu&+w5U2HPt7`S(7yuKz&h9fZvi%<P4F(<Pd#uZ!rjq4
zXRES1K9%3bf(4d<uW*KGM7^SvN92~hz;j7_@~JF{>gcJ08i;@V9#uXxUw&iB{%y$f
z^h~mU9rAbjwSJ^J97aMuqz@)q_!uD@Q)!S<M1#ReP!;4MIFzTE<q7N^S>%`ez^!^E
qQEk#JR*OIfuDr>7W*_8Mc4aI+Mm&uZ5P>i9GP^Ml@p?o4&5B1xGJtOY
diff --git a/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc
index 3a8dd54cec228a7daaec9f21364b2a5091cbcb31..61c63f3de2a7898ac54f31c321a5c93a82e23461 100644
GIT binary patch
delta 754
zcmZ9K%}*0i6vgMZWhm{;V5cqGn2?HAY%JJ>qCyF_kQUg~sL_qap>!r?w4IVS&qx*Q
zf|ZFzlDojYace@FxN+sm|6vlNJC_)D?tFKuXk5%M_niAaCg;A7Q$KT&8C8wU9euv{
zdg3sWm%+oLd>0D+1Nkh#41O41g+&a7521wb!(U;ge=uSNdfrgWuw3qiT1Jz5VYgA|
zz8>}~VJWSS@a&G`+tFjy<g6k|5}On7l(y{{+-_S&oiR2~evxp6aFtLM@Pd4s3$(YG
zMsmn<@u@Ni3;0H9LIa~}MK06gCA_0fhnGc}4YCTpQ0KwIPpS#)nA1+3u8GET@wtkC
z1R;n(SWp30Lqki!4tBKy<nWy~J35t;_Fy;4<p;`6#GLN`(2gK=$qbmPdGTckB+)pt
zCxK*2x1?+yBe5csu^O9%HFRQWSjAVdeGz3^&j?oaHQ2>{eS9PvU`=s1+r)4Bc=Q>S
zJ%Q{5o``S2GP?1{a)I1M{2ou9D^a;Z2nu*=^^wykJ9gU-mNlqhVK$M*>xoR168)VV
z_7W$1v!pK*@&aC*bv8QO_%jICpNsy#DaMnb)uXm!b7QMxJAANG`_~ZjRR_AxH~oW@
zzfv><c9Sg}k5fF7wz2KA>qT32$86WxL&G*%jQ&n1_;2Di-5jhtwud*87vMfVNv;QF
p_7=Y+Z!TT)n{L--W3(YfNE6Nz2BA?^BKVmN@*zo+7st3`wO{!j)1CkT
delta 821
zcmZ9K%TLr$6vlfxLm8$In2uv41_XiNgonrsC?L-O;sO?$m{`e_GA*?*(~{fEOEH+R
zGJ(Y8nD7U<aAzbWZd|!AZe5QXx41EJrz`L6L?JHjFXw#cwD;zG=U4Z~o@7o{lls`#
z#ZRrr$vz1T9FZPD?&L^n1L(&OwR13v@x(EV<NL%nm_0eFbHqKPrer!HHR2`HrA8tw
zS~SXQqligLDPWox_>nCC%UnvP1wkOU7%EEH^G)iN9n&I&3~_y!W02z}#|%RyMz<+L
zd7WrHt#Lwps&v8#zE)gVL|L7YCV2BCK2UoTQ!LEJ$TS|P!%)D_stpgZTf205o)uoO
z6DkY>#32a@Mg_<`npz_q;EtAuK76YU)Zb_n_TiaKrB!7&Y4@M})P6uVXN$IKcOAw-
zV1?Fw0fgNvt5@xAyT|UeuOG%hu!Z|VHjlp>MqmO5Q=Kq}g;WdN!M)T=HrfWggK<#5
z3lyL0%|lPrVo(Z9+bIXVuD4Q=y|U#x?0Chs)7$KoS+OE*5sx~g5?fghvm#kyDzWg5
z-YjqMaFchn;TL@wrZAV@l5$)f#dqmU`#2A0IbsZkI=kT)Cw+DYH-;?oLIF?HEm9>O
zTAL2mUFg{3#+);N&x$m0BpE8A8<ITF1~~c{j5G<B18V+tIE)O}{}-9>9lX2V^F3;=
z2cA!>o!WW9r3FK+3Ko4F{Yg1X*;<L7d$uQR`^x5a#2rH}S-xGi$eQWdB*kB+b41Iw
yz;BLOzGtAExduy^&n(r536Z?Qqs%=R!|$1<_UhdDH)mt3j)-e~d02bb#f3j)ecwa?
diff --git a/CoLo-AT/data_analysis/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py
index e1d352c..2de013b 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -146,7 +146,6 @@ class Analyzer():
time_arr.append((interval_start_time+unit_time_interval+interval_start_time)/2)
interval_start_time = interval_start_time+unit_time_interval
-
print(data_recorder.name,': ')
print('Avg location deviation errors per run: ', sum(loc_err_per_run)/len(loc_err_per_run))
print('Avg trace of state variances per run: ', sum(trace_per_run)/len(trace_per_run))
diff --git a/CoLo-AT/data_analysis/dataset_analyzer.py b/CoLo-AT/data_analysis/dataset_analyzer.py
deleted file mode 100644
index a9e77f8..0000000
--- a/CoLo-AT/data_analysis/dataset_analyzer.py
+++ /dev/null
@@ -1,35 +0,0 @@
-import numpy as np
-import os
-
-import initialization_datasets
-
-def dataset_analysis(dataset_name, duration, sampling_period):
- # this function is to find the characteristic of the dataset. Given name of the dataset and period of sampling (in second),
- # it will output v_max, r_max of for each robot during each period, also the mean and the STD of observation time interval
- dataset_labels = [1,2,3,4,5] #all robots
- [landmark_map, start_time, start_state_arr, measurement_data, odometry_data, groundtruth_data] = initialization_datasets.initialization_MRCLAMDatasets_multirobots(dataset_name, dataset_labels, duration)
-
- print 'robot\t\tv_max\t\tr_mean\t\tr_std\tnum_obser\ttime'
-
- for i, label in enumerate(dataset_labels):
- t = start_time
- while t <= start_time+duration-sampling_period:
- v_seq = [obs['velocity'] for obs in odometry_data[i] if t+sampling_period > obs['time'] >= t] # create a list of velocity data for each sampling period
- v_max = max(v_seq)
-
- r_seq = [obs['measurment_range'] for obs in measurement_data[i] if t+sampling_period > obs['time'] >= t] # create a list of measurement_range data for each sampling period
- try:
- r_max = max(r_seq)
- r_mean = np.mean(r_seq)
- r_std = np.std(r_seq)
-
- if r_std < 0.8:
- print str(label)+'\t'+ str(v_max) + '\t\t'+ str(r_max) +'\t' +str(r_std) + '\t'+ str(len(r_seq)) + '\t' + str(t-start_time) + 's~' + str(t+sampling_period-start_time) +'s'
-
- except ValueError:
- print '\tempty array'
-
- t = t + sampling_period
-
- return 1
-
diff --git a/CoLo-AT/dataset_analysis.py b/CoLo-AT/dataset_analysis.py
new file mode 100644
index 0000000..2bd5338
--- /dev/null
+++ b/CoLo-AT/dataset_analysis.py
@@ -0,0 +1,24 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Thu May 16 16:26:29 2019
+
+@author: william
+"""
+
+import os, sys
+sys.path.append(os.path.join(os.path.dirname(__file__), "."))
+from dataset_manager.realworld_dataset_manager import RW_Dataset_Manager
+
+
+# load algorithms
+sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
+
+# dataset_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/"
+dataset_path = "/home/william/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/" # for desktop Ubuntu
+
+robot_labels = [1,2,3]
+duration = 180 # duration for the simulation in sec
+testing_dataset = RW_Dataset_Manager('testing')
+start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datasets(dataset_path, robot_labels, duration)
+testing_dataset.dataset_analysis(duration)
\ No newline at end of file
diff --git a/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc
index f73b2cf653e3579d4ded1a002c64063cda1cb593..de81e6ebe727461593d79fb78612610ee8c62a80 100644
GIT binary patch
delta 3920
zcmbtXYitzP6`ngYyF0t~;`Ia8>)2+oAuu*}Y(S+rkeUQj2!S?0>Xu?nhP`8Z@V<O!
zY}qTjOA8W<BGSU8Dp3=K^riG6qSVcw@~i#PhlB)`sv=!Qk@}}CQKU+hDm7L5IOooq
zT@azIy3%}e?!D)pIj?)p-9I>&^Reymc<iR}-pO4PZ^uq(5urPIIwbgv_PNJ;6h+Cv
zbML1CH42aSWB#tzNe20c+N{}2H=a{Yw9+T&<8<>ma-xk6QIqxowU+kNt+2L^4jfa=
zVg8bSh=`Rt`U2_N1X6@vwEbxj&)RvKv)PO_>1G{4Jh74uAB=YF0v6Ve(0mVCkQ(8)
zn#LZrJeYXo^2Nkh_s0u|FO2g`iM@~dz@g?HMDR1sJ?SmLURra^x0#!tO8aik8A;zo
zH?cMZlmEPV7umx<YVIJTJlWEp*o)IoBRqq!n;&iIgQew`^hTTz5$a@p+w$3yFfxe<
zQGc=Mn5rBD`_Ec3q@QnU9XPZT_Zzq;mY>U6tZ=H}dm_Z_{FEaSImh<qn3HqzzC~@{
z7V!ct<Q$(Z2E&G_VBUgh!C!0L4hH?EHQlfkr9Oc$$UkcR6dC5b+xEa(scm-(gU0$1
z4k5gN@Em~Px7ymvA}Up|KpBykVuiUp_1T<1ZOJI#HzNbBXWhKRMp4N9PPu=#o1ZL*
zD4G)pYOGo&vxV)$?PCa`{eY0SYrCju%GrKq+MyO0Dfb&-Ww5>G2B|~{mGFdtFUzG1
zg81yyC`hxdc@fP!3%-SGRT2Zt4&eIA2WtmNSp^b*@#=$-_jIBtuClCBLe(jCS)&@&
z;jJ&c?<-5nX>ykAQzqU$sV;|>!!TAaXbU$h8dC6RNu{E;D>H=sec||$2At1bAm_<>
z^}IHz(GU%vGk{B93Z;~NO8G2Vj`*Ra2)qqHd^#ejsBh3nMWe=2xI$=j$pDNkMbE~F
zGW+bf0%CP3RynU!#WqyMMgy@REe29!@NSS&k!q}ys({pnibBh=rA8_C+PG5DD(X(<
zf;KxeuFRG{`AK7K@cvy_0OoXgrQJ?~@-8P{J7Vum5hfVboieHntM6&`N~z!Z3E
zV(T)gs1=e_mIzf+imqtNj9%!es57Cw4*n+!)0apEmM*EYI((7@DTOryP7R)O5epI%
zkAbO)M$CnIII#99C^t=>ohw3wr>vsG+yZ5bup_8AL9v119e6xcFRYYy9v!##fgsQE
zt;76J$)>WMx3i0$>kaM()9kGG6cDcGX9~IEQ>AcF5=z0@NFUrcQVLl)djVgbPe=YA
z>jUG<|G|1z2F1~Ad4$kzFq?fDCLd<%do@#K1v6u899iXo{~}WI)PEMK69`chFEp@)
zJ&WVd0GM$mO9g&JG_Cr_VvaYL^=tkIJv<SwSIZ0P{WR{2V3<5-HXy)d)ft}(L+-&f
zpvq7faJ4!KddSf0>?^3w8HAd#7*dhIM8Np1YIYN5y^R2hs;dSu)HV_(>qv}r5`)Cl
zZnZ`wv{G+<2H1K2eli}ZO5(|6X2(I0V`|lU4K&+>6CoBw`V_)pgkgjX0A%bYem%K?
z+~v2EM-6ncoa$@uAs_I=?Z=FX)u}hzH<Lf}+wGq>Fba}uD3vCE;m@a@H~LnmevsNo
z{>pEqE^o#jB9DWq*M;#5IO`yHNXAL<N=Gl5=0ED_BY)#}I&Qr%iX3Z^BT}~ym1S~s
zEfJ)ca3Cg*ezh_HQw6rjjw8G5-xyDJg%73Lqj(%Fo3VfK&dwVL(KDrngL&#Kq{ogP
z9Z7>mc>IICD4VTAz|1nMU#WH(sMWzi{|2lHp6)sTQR1h&e(gx3cjGo}!S^~RPDqnN
z><H2_2-2h!4pH0HPc&xc7{kVi+6v>IMXw-Z65$HJ+PwkNgx~0XDJg@f1wJsCGizbT
zX0Pyx^}S79P~AKowkT=~DfRH%-JL5xT%S_A?$=V9vj4cy(S>Xe|NVxo<SJj&)0L4L
zN>7}`4^IM^jls7x_?C#|9M7|%<IA$P>fY+HS{*W}`85=PcGmG?%i}NitS49a^`0$c
z<I0_$i+cH6$T^CDE|dxUK#zumlIweM{b__}5TsSuKOa)@!4sHqqsaF$EK<e+1xgD0
zHnPd?{z;@vfV*LU)eqMiKQ<9F$-aYZ-$g(ZgS?TsbPR{+H`aq7eTio!sA=*76>>2J
z*{e7U$CRJi(o!CTfe7adEN5q#hASb21_1MM836VsPFw*HYh*=o^TjzITMPR>u3SSv
z&x;@{Fqz^-=FH2ylDCa`vET)1CWop&BvgNx%_`sn#&Exf``5rdrodcCN!xb|dDew1
z>k+yU(f~p&z}~{JQ0L)2KOEd*bhyO~3B+MmLRRTv^qq)+o)&ezfGAG8Ld`C~vnZm4
zQ>U%wz$M?rO=AfA`B`&-9AA0c>?Vh52_oYpE1X^z0_kL#tC0s>^?+;GJG`TRfP8P|
zK>t5UUEX1`{EvC}-{lsFMU@|#MrGyrezW00Gh`CUoT_CKW`Vr=vR@#DN7GEyEEP@i
z<WYBW5^j$<*RQksC%`K&<8U)wO&57Q-%&PzC<uLB<k?SgcpX9J8FvPsByX*}G58oM
zkHRd}XPNh}BL}uI_7;M4{~JJw2JCay5@AMb8nV}LwI%PsCD|)F8F$jn$h4H2iuk;p
zbt!i2Vj)j!nJl#kZbz-yW|?fLkt*Ns_>rMA{W2M}x;l<-TWvKF$Yk^l!weGq!=X=*
zKk}jBJqbMJ(9nJ*U;E7@Umo7{;$|4Jod{n-xQOsF!dDT#hHwetGQ!spt|G{DEM0;=
z2wWoL^h+E{H{C|+xBMT&?WtZMx5MLMQ7}|PGZL|~5;D5@)@|$Ms+kZW_*YY040^bV
Vdtc;q+elbelwJ^%<TtkU{~L46$LIh6
delta 2772
zcmZ`*Yitx%6yCd!?rz&{w=Y`gcG<R2I`sXd@-9LF@xiNrx9T|UOxuConeE-_x=Yj&
z0}0U}$kjjmG%+R`e~|rSLgG&oG*L*5#DOUOAu;i%n3y1X&Yj)vZlfgq_MExro_o)C
z9+&IJBdu~I6sml@@%}5XpT1d1J$~xd40Vw5bDmSJF_+6V2Y;*oJRt3bztm^^fhR(G
z`Cp!;_C9r!y7?lZ4Qju-MePQuQI*uK7hSYT?K$C+1~~T~AvE`kH$&PwVYlr~D%K(m
z#T7$MDD0d(ug5i;q-f5nI2<IS{Hxj)GRE)K9&lve7;Kv6_iHCh^iyl}(?uQjo~Roo
zlwYpvAe;H;by3pAzo~mBG68<h*UK+xN@|&D3C*zNOuwpFl9z4g8|!<*JHUYLMA(I}
zjnn$AWIMl9AKQc<Y`?0-Ek(B2IA{{FeX5mBYLZ*%V1B!Pj&$?BhCVXPXBt`uXgE=F
z7M!Hekg1vp&0<+us8tJ=5M=mF`6mq{aP6NOVpS5J-;2=0y^VWFKi}WD3)bFg-1!o6
z$hr{@ARIxM0-#lJJtZrrEQMLD1-{t6xH7B7X^@$-rX?H82^wBt=CYw$Y}s0rMILyl
zsbyt5Iw=po+*CK_O&Y{-oL$xV$VBqkZrr{HVJ|@54!7981zA@!win`bFiUF_iZ!>W
zsWN;MZUg2~)pXnz=@CWYi|dA#q5=F7eu%{;;RM^G$Z0!hXc<e!HR_0#p3vAnyg}}d
zrar=Vw<@F0f+~6P83LJM6-Yi7MENwrL4<yUGXN4{o&0XJfj?@kA>Z=Ht;YjLOFl<x
z7BtJDSjOYNgSC_!z(vltbJ7;!HzGZO<0U)z!$>2!%O6M11Ta;r{ukRq<OiN^I~Led
z@;Mqt|EsN$P?GmxwQxcwoY2ghqvjT@K!#a^;(Z=vQ424k^fG?+F)S0xzle5{8Hi^q
zvH6qe_tPTLO=#gS*@uBMu^D4^N=mq?5N_-c7OOl5uwb$*dk&4Tz}O7JS>&sx8ZVBN
z$O>b>^2yjOGQt0jwG0Yt^v2d9><5tYe)EM16(V9`30R~2K>H#0?=F6|{kA5;+=km4
zaa*C&@Pi1##}1(^QYeC0k1C#6IHG!PnK5j2yc!Lx#$cjhGlIB2ztd6U{?o-DcD&at
z%E0499$?y%j9t!5Q57F`hTZ?Ta-VkYbGMaWPDC(u()J=(Y=T#G^{yD;-}WSQ!zs6F
zR0ApG2|bDn3u2DpJt%-w?HucYV{LyzOQn<rjaI{SSf!SRL>CsAW+D&^9bnicwz}Gp
zDK*AEtVt_zT@@n-`z9azqpl`Wk$cj0#k(hsjzp0l;i6gA%mtj5xKutZoCcfZH;2~o
zpL_fj!bGSmVwPf8vNsUKtQbM53*fg3fP%~NF(Ngy44P#TL=lPlX+ZTb0ushz2!cdx
zBc}}lAlW|MP_+zO;um_?lOBGfx9y;qoNuDpS+uATMCwLz*^<*xv~S^xU=lgCoz!F1
zgpy24I|%?1DSz7AT=gm%zlI?5SYP8xD~fFh5rilJb(@2>+Z?jpY4}YKiP1&9FuDc>
z4zXD@75rf6)DO3iRXybh$wl3E$1~I&&(feddlrTz29(4-tFxBXp<3tS5268nt8W7t
z&;8gJB}a;R6r@jp6|h|7Q!s`E!W<~4WepANlBoOR0~^TYTw>rUnJT&_cs9CTQw)WC
z{zr+xVEW(EC~PY)gOgKI)!K`T8W1H>EQBh&2fl;s3Iev26fVX_YemPX`usdR842Bj
zIAIhy`MwCo;%Nqs3)LX{>JqvtNj0jM5#C0?Dx&<|;T`0I+}FdcL_DKFsGvE87S|9~
z5Cq}NAlX$o0P?LZ1&cRi3|48qp~2IbN@{cZygnz2PGp3JoYrMpDrp*Ou};M;oHF2c
zv@+<=TYSk=qZfKa;ytXKu)^!$hZh)GVVF|NqhsUbHlH5b6&?W{9;&P2XG&VfKOWmW
zvk5ddjG!WD2=fRF2#W|h!Z`$-d~69p@PL6kJc#7HkE-D2I!Yh%JLAnQtQ}-LIn@mi
ta0fhraOH~2%g>B&T#su~*!IEyVUk&An&PX=runt;?G-T;+ri7ee*vVitS<ln
diff --git a/CoLo-AT/dataset_manager/realworld_dataset_manager.py b/CoLo-AT/dataset_manager/realworld_dataset_manager.py
index b527385..ca35dc4 100755
--- a/CoLo-AT/dataset_manager/realworld_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/realworld_dataset_manager.py
@@ -200,6 +200,36 @@ class RW_Dataset_Manager:
print("self.starting_states ", self.starting_states)
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
+
+ def dataset_analysis(self, sampling_period):
+ # this function is to find the characteristic of the dataset. Given name of the dataset and period of sampling (in second),
+ analysis_result = {}
+
+ odo_varibles = ['velocity', 'angular velocity']
+ meas_varibles = ['measurment_range', 'bearing']
+
+ for i, label in enumerate(self.robot_lables):
+ t = self.start_time
+ while t <= self.start_time+self.duration-sampling_period:
+ for odo_variable in odo_varibles:
+ odo_seq = [robot_data[odo_variable] for robot_data in self.odometry_data[i] if t+sampling_period > robot_data['time'] >= t] # create a list of velocity data for each sampling period
+ odo_max = max(odo_seq)
+ odo_std = np.std(odo_seq)
+
+ print('time: ', t, 'robot ID:', label, odo_variable, '_max:', odo_max, odo_variable, '_std:', odo_std)
+
+ for meas_varible in meas_varibles:
+ meas_seq = [robot_data[meas_varible] for robot_data in self.measurement_data[i] if t+sampling_period > robot_data['time'] >= t] # create a list of velocity data for each sampling period
+ meas_max = max(meas_seq)
+ meas_std = np.std(meas_seq)
+
+ print('time: ', t, 'robot ID:', label, meas_varible, '_max:', meas_max, meas_varible, '_std:', meas_std)
+
+ t+=sampling_period
+
+
+ return 1
+
def dataset_reset(self):
self.data_trackers = {'odometry': np.ones((self.num_robots,), dtype=np.int),'measurement':np.ones((self.num_robots,), dtype=np.int), 'groundtruth':np.ones((self.num_robots,), dtype=np.int)}
@@ -460,3 +490,6 @@ class RW_Dataset_Manager:
return gt
+
+
+
diff --git a/CoLo-AT/localization_algos/ekf_gs_bound.py b/CoLo-AT/localization_algos/ekf_gs_bound.py
index cece257..fe91db4 100644
--- a/CoLo-AT/localization_algos/ekf_gs_bound.py
+++ b/CoLo-AT/localization_algos/ekf_gs_bound.py
@@ -10,6 +10,10 @@ def rot_mtx(theta):
class EKF_GS_BOUND(ekf_algo_framework):
def __init__(self, algo_name):
ekf_algo_framework.__init__(self, algo_name)
+ self.var_v = 0.04
+ self.var_range = 0.7
+ self.var_bearing = 0.3
+ self.max_range = 3.5
def state_variance_init(self, num_robots):
return 0.1*np.matrix(np.identity(2*num_robots), dtype = float)
@@ -25,7 +29,7 @@ class EKF_GS_BOUND(ekf_algo_framework):
[measurement_data, sensor_covariance] = sensor_data
sigma_odo = sensor_covariance
- var_v = 0.1 # variance of the velocity
+ var_v = self.var_v # variance of the velocity
i = 2*index
num_robots = int(len(s)/2)
@@ -66,11 +70,12 @@ class EKF_GS_BOUND(ekf_algo_framework):
H_i[1, i+1] = -1
H = rot_mtx(self_theta).getT()*H_i
- var_range = 0.1
- d_max = pow(0.05,2)
- var_bearing = pow(2.0 / 180, 2)
+ var_range = self.var_range
+ var_range = self.var_range
+ range_max = self.range_max
+ var_bearing = self.var_bearing
- R = np.matrix(max(var_range, d_max, var_bearing)*np.identity(2))
+ R = np.matrix(max(var_range, range_max, var_bearing)*np.identity(2))
sigma_s = (sigma_s.getI()+H.getT()*R.getI()*H).getI()
return [s, orinetations, sigma_s]
@@ -101,11 +106,12 @@ class EKF_GS_BOUND(ekf_algo_framework):
H_ij[1, j+1] = 1
H = rot_mtx(self_theta).getT()*H_ij
- var_range = 0.1
- d_max = pow(0.05,2)
- var_bearing = pow(2.0 / 180, 2)
-
- R = np.matrix(max(var_range, d_max, var_bearing)*np.identity(2))
+ var_range = self.var_range
+ var_range = self.var_range
+ range_max = self.range_max
+ var_bearing = self.var_bearing
+
+ R = np.matrix(max(var_range, range_max, var_bearing)*np.identity(2))
sigma_s = (sigma_s.getI()+H.getT()*R.getI()*H).getI()
return [s, orinetations, sigma_s]
--
GitLab