From 8bcd4901303ce5e2789707b192496c1a76dd6cde Mon Sep 17 00:00:00 2001
From: William Chen <billyskc@ucla.edu>
Date: Tue, 14 May 2019 18:52:54 -0700
Subject: [PATCH] added isrr2017 sim
---
.../__pycache__/data_analyzer.cpython-36.pyc | Bin 5570 -> 5882 bytes
CoLo-AT/data_analysis/data_analyzer.py | 27 +++--
CoLo-AT/gs_sci_vs_ls_ci.py | 70 +++++++++++++
CoLo-AT/isrr2017_v2_2019.py | 93 ++++++++++++++++++
.../__pycache__/ekf_ls_bda.cpython-36.pyc | Bin 4743 -> 4770 bytes
.../__pycache__/ekf_ls_ci.cpython-36.pyc | Bin 3801 -> 3807 bytes
.../__pycache__/gs_ci_bound.cpython-36.pyc | Bin 3400 -> 3409 bytes
CoLo-AT/localization_algos/ekf_ls_ci.py | 6 +-
CoLo-AT/localization_algos/gs_ci_bound.py | 2 +-
9 files changed, 185 insertions(+), 13 deletions(-)
create mode 100644 CoLo-AT/gs_sci_vs_ls_ci.py
create mode 100644 CoLo-AT/isrr2017_v2_2019.py
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 71e848bdc21f0e4085fd1f8b06af55246286a44c..98deeb48ab51fd14206fa66498458fe60f18dfe4 100644
GIT binary patch
delta 1800
zcmZWp%a0UA7_aKr^lNsWJ+tfr@`h%4gDyTmU{;6{*JXE=3uGO(nss`1W_D|O36Pp>
z;&L;Kkh=IM7!N+e)r%fHC>lLF8sfnV#+Z0A@t}TRO``<6^Zn}gJ*uj^>R0vo*q{5|
zquH$ceCvlxvs*uLH|eWBA;bc!fprg1LWFTi>sh@_LSni`dN#Bgqid8BVaGbt)@aXB
zOb6yFW&rbq6B|MoRG7?M*P+@~zV@&0#U|u4%u4ghUlkUFGR$5#C`qg&6Wb!QMpbLA
z(P5${c4CJ)DRY^dGLLyF^O?`8EVr(yX&shVaW{5ZA!QybrVML`(^@aCIjp1-9uyA-
z_F|8f(-;<SNQ><_%K}zehn0Xe8CoNhFfv2J5&4X9NPcVwJ8za3>uuJm^X8V$m2SkZ
z)ssf29cq2+N+(G=Ena{{mXY!8<`R%l<2p3_e>^8=%tJYIi8tCwUu!RM6T7`_)~GMP
zFwg8OF1F@cQN6puqgK>T=GrT*ew9ayFx#~#%{)rTj8)rs1o{)rrZ#m4ua>8Iu%b4s
z$v3UZp$g<ZFeQTD<fqp8=>^iu-=Wt}GTJME6~RhiWw7A-hr$((L3Glj9RXack|t>z
z8Z5~j%8NIAkrDO=@Gg?}h(Qvg>GBWch0LB+eqWTCIZk?I*mx*mR#W3Yi9p2yQ5bY&
znGFPL3Zj_$PzioflvG_w*s8UcuU;g=gDE#o5y;V|4tzrRptOQ0jSj?<0}pyC?>uRj
zF`Wt;QYp(q(O}bufCXy%^T5=<3Aaj^lMbgMJ$HFZ9=6Ah=0K~C0E8^64?o~%L1I_<
zA}8`9D~1;E2xQky$f}rp$KGM8t(PC!75SyTVC{jm=b;>TTBCduay*rNy$c(wFGg*C
z2;8tDKXYE7q5R8P9_4$nY7DEg39rvbb4wAQ<11~xTi$hNkMzq=Xk@;#0%96@LL~$J
zsGR)8t?fPs19q<h5FONliqQ<}Q-d1NrY|r#<qexY$jO%tZ#3uX73%9gt<Z{Q(cyb{
zy$4$UH5i0XAzVP1ma9V>S5@%Xqs&7ke@s6_h4X1-co*>*1PvfG_>%)+(ckcd8U8eu
zjUyZf=sOXdgoxLZh-WeQEW&dDVSx`J4^JSTL^y?T8sQ8A3SUjgCy>1)Yq|I6-FrXg
zzM}M)e82FrRmS+d{G;&t89Xha$Ez3_Mi@aDMR*Ef3&K`}Z2)~QY0Nk0mg>CT>hkRv
z-XU)nw+H)>?ngL)uoGdI{7x&$?~7IXjr_eBt=@uQLIxUz6RCq4*WVCyf?S^LnV>Vg
zZZ)Q_YnODqw0pMD7-?$VQ^Jfjp*1snTIjJZY&dQZS%6C$bZDqZ=D)~t<s!}l7dd2!
zy0{I{v;4Z`9ycd%9^Cubb9!;1!yCz4tu3`q3x2E7?X(|DBRv~Z{-S(P+7>7^zly(x
zl2*IHn=&Zx4I1dj2x?c(B0DE%%ZL46fp&3`3AN?B<zvoaptCUKq`Y4q5AZN>HRK$!
rCPGKv4yy8C5R8nuuec}d%^a_@(CM2XiKQESGkEfHu!~mZt>EN8Si+9O
delta 1526
zcmZWpO>7%Q6rP#=U$5=8*LFzLgpjr&CP4`#P$>O@q(U@pL#38RRH0GmY(mzt<BXju
zg|TvoIDllSVuaLlg}6hn98eDxf&)be2^oR77L^0y21xMU*ew!x^}hMud*8m@ee*nj
zJb8QEpH8R!U(Wrobbsg*f6&<V2_Y8P99ZKofF?=k9%<RIGEVm>CDKj|&h}`_(aZqm
zYGwlSq?4G^kW^aSY8#+-wa*^<USdJseCRug&23%RNt|8o^r*|-9`m@z3p{8uUCicb
z9d{FlXL`)#*&ah}v8VN*aH!)RXuEa2#O1kO47H`6)=qq$=TSQc-v?is+9Q;3a-Nj?
z)LZ71`qun<C=bmjGhx6Y{>KCLxi$5HJxhHxXV0on?b*@<2nltm-@RB!;WAX{EMy9h
zNrCAm-&GsV>G|t)XN=QU2$loOgGD<ZOJ8~>F-V1OHi5G~QXw^y!OLBzf?W%wBi(-R
z7D;WNNt$NG7dMsXj_o$SkT#~cRbk?;jJSQ6v?3XHZ{SWp0Y8+vo)1d!b26`WdFkra
z-MYL;WC|V?oFkB<6$AK;3}6RCnI9m!ks0vd&aPK0V3rSb2L)2Skx7Fn((<hO(H*2`
z)SvG7B<x?)lYnqD?k<+UBT3@RKxSlCreR3ZZ3{BmSFd{q2i`7-cw?oxel;H6e6taY
ztH3vFdRx>DZ_PdgeHEk1@~eX#t1i}SO%Xt-V>Gs_9b+S|iE(ht1=aSC(oyxLzctdy
zR^ye%wuq~7t+`wSUAie&uEfjrSS*X}nmDK~rY<mXNPWX1bw4%DXQ4!634j<bbs3sT
z18PzeR((BKAu>!E<T4O<X}}%<EslcA0%lXje8_|b3<<Cj4U7P=1ewCEcJLDm=iwRR
zG{PB#v+AAf{!2PI`5<#ok0hq=;XhZLMP?&gKwto6Q~b9OivEUQ*%4DHdm3R{{gy2p
znL+vtKshHe$e%|zf^Zb!7{YOc6A1VK@gl-Y>O^>hzI*HY@GD9u)kpbLc7*ZE>i7I>
zvqhlglqg}O51}7nAHsfwL4+ZMM*%vi=K7V&a(zXtR2yO#!;h)=qXWgqk&Yph5k?RW
z=$-j4>Z7;SuTi`!A-G0b2AH{XQPPv-jfIv4lSN>nC}(Z9WVGy-0|yu5q{14{Nh_g}
zRU9!djf6=!&@i$FhZ{_vsUyz6k*&-kX&StnCHjCfyAv$URd(KkhA+Hars4$^6-SD%
z;49(~>ZXckk*%uv;uA%VG)B-Z#<3D7)qBOK{GWj~@SGs(PI1PY0CE8eo>E5Xa1ke1
t=&F~Hts*4q)zU=&s6Xc)bB6?;E@iJ{!7$gaby%&gJ}iyWlKQN4{2v}`J1zhK
diff --git a/CoLo-AT/data_analysis/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py
index b5be8b8..f608879 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -26,6 +26,8 @@ class Analyzer():
self.dataset_labels = dataset_labels
def plot_loc_err_and_trace(self, loc_err, trace, time_arr, measurement_count, recorder_name = None):
+
+ [lm_measurement_num, relative_measurment_num] = measurement_count
fig = plt.figure()
plt.suptitle('Correctness analysis')
fig1 = fig.add_subplot(311)
@@ -34,8 +36,9 @@ class Analyzer():
fig1.plot(time_arr, loc_err, label = recorder_name)
fig2.plot(time_arr, trace, label = recorder_name)
- fig3.bar(time_arr, measurement_count, label = recorder_name)
-
+ #fig3.bar(time_arr, lm_measurement_num, label = "landmark observation")
+ fig3.bar(time_arr, relative_measurment_num, label = "relative observation")
+ fig3.bar(time_arr, lm_measurement_num, bottom = relative_measurment_num, label = "landmark observation")
fig1.set_title('Estimation deviation error')
fig1.set_xlabel('Time[s]')
@@ -78,7 +81,8 @@ class Analyzer():
finish_flag = False
loc_err_per_run = []
- measurement_num = []
+ lm_measurement_num = []
+ relative_measurment_num = []
trace_per_run = []
time_arr = []
@@ -86,7 +90,8 @@ class Analyzer():
loc_err_per_time_iterval = 0
trace_per_time_iterval = 0
num_dataline_per_time_iterval = 0
- measurement_count = 0
+ lm_measurement_count = 0
+ relative_measurement_count = 0
while interval_start_time <= time < interval_start_time+unit_time_interval:
try:
@@ -96,8 +101,10 @@ class Analyzer():
break
time = data_in_time_order[time_index][0]
- if update_in_time_order[time_index] == 'landmark observation' or update_in_time_order[time_index] == 'relative observation':
- measurement_count +=1
+ if update_in_time_order[time_index] == 'landmark observation':
+ lm_measurement_count +=1
+ if update_in_time_order[time_index] == 'relative observation':
+ relative_measurement_count +=1
loc_err_per_time_iterval+= data_in_time_order[time_index][7]
trace_per_time_iterval += data_in_time_order[time_index][4]
@@ -116,7 +123,10 @@ class Analyzer():
loc_err_per_run.append(0)
trace_per_run.append(0)
- measurement_num.append(measurement_count)
+ lm_measurement_num.append(lm_measurement_count)
+ relative_measurment_num.append(relative_measurement_count)
+
+
time_arr.append((interval_start_time+unit_time_interval+interval_start_time)/2)
interval_start_time = interval_start_time+unit_time_interval
@@ -126,7 +136,7 @@ class Analyzer():
print('Avg trace of state variances per run: ', sum(trace_per_run)/len(trace_per_run))
if plot_graphs:
- self.plot_loc_err_and_trace(loc_err_per_run, trace_per_run, time_arr, measurement_count = measurement_num, recorder_name = recorder_name)
+ self.plot_loc_err_and_trace(loc_err_per_run, trace_per_run, time_arr, measurement_count = [lm_measurement_num, relative_measurment_num], recorder_name = recorder_name)
return loc_err_per_run, trace_per_run, time_arr
@@ -222,6 +232,7 @@ class Analyzer():
def algos_comparison(self, arr_data_recorder, only_trace=None):
+ print("************Algorithm Comparison***************")
arr_loc_err = []
arr_trace = []
for data_recorder in arr_data_recorder:
diff --git a/CoLo-AT/gs_sci_vs_ls_ci.py b/CoLo-AT/gs_sci_vs_ls_ci.py
new file mode 100644
index 0000000..e3bba30
--- /dev/null
+++ b/CoLo-AT/gs_sci_vs_ls_ci.py
@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Sun Apr 8 17:55:06 2018
+
+@author: william
+"""
+
+import os, sys
+import getpass
+sys.path.append(os.path.join(os.path.dirname(__file__), "."))
+from dataset_manager.realworld_dataset_manager import RW_Dataset_Manager
+from simulation_process.sim_manager import SimulationManager
+from robots.robot_system import RobotSystem
+from simulation_process.state_recorder import StatesRecorder
+from data_analysis.data_analyzer import Analyzer
+from data_analysis.realtime_plot import animate_plot
+
+# load algorithms
+sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
+from centralized_ekf import Centralized_EKF # works
+from ekf_ls_bda import EKF_LS_BDA
+from ekf_ls_ci import EKF_LS_CI
+from ekf_gs_ci2 import EKF_GS_CI2
+from ekf_gs_sci2 import EKF_GS_SCI2
+
+
+# dataset_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/"
+dataset_path = "/home/william/CoLo/CoLo-D/CoLo-Datasets/official_dataset1/" # 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)
+analyzer = Analyzer('analyzer', robot_labels)
+
+loc_algo = EKF_GS_CI2('algo')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
+sim = SimulationManager('sim gs_ci')
+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()
+loc_algo = EKF_LS_CI('algo')
+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)
+
+##############################################################################
+'''
+testing_dataset.dataset_reset()
+loc_algo = EKF_GS_SCI2('algo')
+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)
+'''
+analyzer.algos_comparison([state_recorder, state_recorder_LS_CI])
diff --git a/CoLo-AT/isrr2017_v2_2019.py b/CoLo-AT/isrr2017_v2_2019.py
new file mode 100644
index 0000000..344afec
--- /dev/null
+++ b/CoLo-AT/isrr2017_v2_2019.py
@@ -0,0 +1,93 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Sun Apr 8 17:55:06 2018
+
+@author: william
+"""
+
+import os, sys
+import getpass
+sys.path.append(os.path.join(os.path.dirname(__file__), "."))
+from dataset_manager.realworld_dataset_manager import RW_Dataset_Manager
+from simulation_process.sim_manager import SimulationManager
+from robots.robot_system import RobotSystem
+from simulation_process.state_recorder import StatesRecorder
+from data_analysis.data_analyzer import Analyzer
+from data_analysis.realtime_plot import animate_plot
+
+# load algorithms
+sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
+from centralized_ekf import Centralized_EKF # works
+from ekf_ls_bda import EKF_LS_BDA
+from ekf_ls_ci import EKF_LS_CI
+from ekf_gs_ci2 import EKF_GS_CI2
+from ekf_gs_sci2 import EKF_GS_SCI2
+
+
+# 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)
+analyzer = Analyzer('analyzer', robot_labels)
+
+loc_algo = EKF_GS_CI2('algo')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
+sim = SimulationManager('sim gs_ci')
+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()
+loc_algo = Centralized_EKF('algo')
+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)
+
+
+##############################################################################
+
+testing_dataset.dataset_reset()
+loc_algo = EKF_LS_BDA('algo')
+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)
+
+
+##############################################################################
+
+testing_dataset.dataset_reset()
+loc_algo = EKF_LS_CI('algo')
+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)
+
+##############################################################################
+
+testing_dataset.dataset_reset()
+loc_algo = EKF_GS_SCI2('algo')
+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)
+
+analyzer.algos_comparison([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI])
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-36.pyc
index dfff1580319cd69bccc411af302e0a2a4aeb2dd0..6843e8adf7a54bbc8527d351585099c800e71b27 100644
GIT binary patch
delta 51
zcmZoyU8Krk%*)GFcgr<;BZnTNjJ1A7er~FMd1g*dW@4_sbG}bLn9y|$(cf&&7$67$
DkLeF}
delta 24
fcmZ3a+OEoB%*)Glq%=BaBZnR%qr&DO#sEP8QnCg!
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-36.pyc
index 7bbf05027bcebec06b2ffaf2f49d45c423207ea6..8f679589595ce4259c225d0622f73a6b0385245e 100644
GIT binary patch
delta 168
zcmca9dta8*n3tF9K-TS;T^l)d8D*^WGxBp&^~*DJaxxQh^_}y5^1+0zV~GA{b4EF4
zMvlqb*>om1v8?8);S^ygVX5J5X3S!p?8)lR$ToQ*tK{VMtQVM>f*3b9vH3DH-kN-Y
zvw@L$b0F7gCfQr88HpwFS-04WGt+Yu<1=B5tXqt!w<cTinlr{uF65of=ET6jP~<mR
Nh|hpgXR<TjGys%aH3t9y
delta 164
zcmcaFdsCLvn3tF9NNIG8`$kS(Mn?I~L5y<Dj1rRzS*|OTu-5RWFr_o40AV^1HZzJa
zl(5wBHZx|ii7<#WOjcl(m|VcB&B#7^4yz<n5!2*Nte2S?lbAMlu=z4GMoqrP*}%xU
zIh^Y>lWY`gMq){PRup@2W_oU7d?t*Mb&D}IYO+1AIb;0fa^Bf&jtmS8MV^x-_zW2J
JCVTQt0|3X?EM))y
diff --git a/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc
index 83bf5d28309a25e87b1bc794f5adbd1a8d27d0d4..0011e92cb63c4fc5e20457ee0cf17255617a39e5 100644
GIT binary patch
delta 55
zcmX>hby14Vn3tF9e#}NT0VXBp8ish56oz01O%}hGObiSR>9b~nz)5>3W9MdlCP{7p
DyvY(3
delta 46
zcmca8bwY~Gn3tDJ&3hx80FxwB4MRLj3PUi1CbQp5CI$wE^jR}O;N)ftCP{7p0;UX8
diff --git a/CoLo-AT/localization_algos/ekf_ls_ci.py b/CoLo-AT/localization_algos/ekf_ls_ci.py
index fe39646..cfe89cd 100644
--- a/CoLo-AT/localization_algos/ekf_ls_ci.py
+++ b/CoLo-AT/localization_algos/ekf_ls_ci.py
@@ -40,8 +40,8 @@ class EKF_LS_CI(ekf_algo_framework):
s[i+1,0] = s[i+1,0] + sin(self_theta)*v*delta_t #y
var_u_v = sigma_odo[0,0]
- sigma_s[i:i+2, i:i+2] = sigma_s[i:i+2, i:i+2]+ delta_t*delta_t*rot_mtx(self_theta)*matrix([[var_u_v, 0],[0, 0]])*rot_mtx(self_theta).T
-
+ #sigma_s[i:i+2, i:i+2] = sigma_s[i:i+2, i:i+2]+ delta_t*delta_t*rot_mtx(self_theta)*matrix([[var_u_v, 0],[0, 0]])*rot_mtx(self_theta).T
+ sigma_s[i:i+2, i:i+2] = sigma_s[i:i+2, i:i+2]+ delta_t*delta_t*rot_mtx(self_theta)*sigma_odo*rot_mtx(self_theta).T
return [s, orinetations, sigma_s]
@@ -78,8 +78,6 @@ class EKF_LS_CI(ekf_algo_framework):
z_hat = rot_mtx(self_theta).getT()*(np.matrix([delta_x, delta_y]).getT()) # shifted to robot frame
-
-
sigma_z = rot_mtx(bearing) * sigma_ob * rot_mtx(bearing).getT()
sigma_invention = H * local_sigma * H.getT() + sigma_z
kalman_gain = local_sigma*H.getT()*sigma_invention.getI()
diff --git a/CoLo-AT/localization_algos/gs_ci_bound.py b/CoLo-AT/localization_algos/gs_ci_bound.py
index 21f007c..da3a820 100644
--- a/CoLo-AT/localization_algos/gs_ci_bound.py
+++ b/CoLo-AT/localization_algos/gs_ci_bound.py
@@ -12,7 +12,7 @@ class GS_CI_Bound(ekf_algo_framework):
self.d_max = 2 # max measurement distance
self.d_var = 0.2
self.bearing_var = 0.2
- self.var_v = 0.2
+ self.var_v = 0.1
def state_variance_init(self, num_robots):
return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)
--
GitLab