Lab 11
Net ID: pp386
Objective: Grid Localization using Bayes Filter
In this lab, the objective is to use grid localization using Bayes filter to find out the location of the robot as it moves around a room. Like we saw im previous lab and as shown in the image below, the odometry estimations depicted in Red are way off from the actual ground truth values depicted in Green. Therefore, we need to use probabilistic estimation to calculate the location of the robot better.
This lab uses the algorithm discussed in class for Bayes filter:
The code consists of 5 functions primarily for implementing the Bayes Filter.
Compute Control
The function compute_control basically takes the current and previous pose of the robot and outputs the rotations and translation required to move the robot from the previous pose to the current pose. Using numpy and the mapper.normalize_angle I was able to implement the code for this function.
def compute_control(cur_pose, prev_pose):
""" Given the current and previous odometry poses, this function extracts
the control information based on the odometry motion model.
Args:
cur_pose ([Pose]): Current Pose
prev_pose ([Pose]): Previous Pose
Returns:
[delta_rot_1]: Rotation 1 (degrees)
[delta_trans]: Translation (meters)
[delta_rot_2]: Rotation 2 (degrees)
"""
changeinx = cur_pose[0] - prev_pose[0]
changeiny = cur_pose[1] - prev_pose[1]
delta_trans = np.sqrt(changeiny**2 + changeinx**2)
delta_rot_1 = loc.mapper.normalize_angle((np.arctan2(changeiny,changeinx)*180/np.pi) - cur_pose[2])
delta_rot_2 = loc.mapper.normalize_angle(cur_pose[2] - prev_pose[2] - delta_rot_1)
return delta_rot_1, delta_trans, delta_rot_2
Odom Motion Model
This function takes in the current pose, previous pose and the control data that was actually sent to the robot. We call the compute_control function in this function and calculate the errors in rotations and translation. We then multiply the three errors by passing them through a Gaussian function and multiplying them to get the probability value.
def odom_motion_model(cur_pose, prev_pose, u):
""" Odometry Motion Model
Args:
cur_pose ([Pose]): Current Pose
prev_pose ([Pose]): Previous Pose
(rot1, trans, rot2) (float, float, float): A tuple with control data in the format
format (rot1, trans, rot2) with units (degrees, meters, degrees)
Returns:
prob [float]: Probability p(x'|x, u)
"""
actual_u = compute_control(cur_pose,prev_pose)
e1 = u[0] - actual_u[0] #error in rot1
e2 = u[1] - actual_u[1] #error in trans
e3 = u[2] - actual_u[2] #error in rot2
prob = loc.gaussian(e1,0,loc.odom_rot_sigma)*loc.gaussian(e2,0,loc.odom_trans_sigma)*loc.gaussian(e3,0,loc.odom_rot_sigma)
return prob
Prediction Step
THis function calculates the prediction of the Bayes filter. It has current pose and previous pose as the arguments and for each of the previous X,Y and Angles and the current X,Y and Angles, we calculate the belief of the system. We are using 0.0001 as the threshold for identifying whether the probability value needs to be calculated or not since this kind of function having 6 for loops is very slow and has a very high time complexity. The bel_bar is calculated and then it is normalized using the code line given in the manual.
def prediction_step(cur_odom, prev_odom):
""" Prediction step of the Bayes Filter.
Update the probabilities in loc.bel_bar based on loc.bel from the previous time step and the odometry motion model.
Args:
cur_odom ([Pose]): Current Pose
prev_odom ([Pose]): Previous Pose
"""
u = compute_control(cur_odom, prev_odom)
for prev_x in range(mapper.MAX_CELLS_X): #iterate through previous x
for prev_y in range(mapper.MAX_CELLS_Y): #iterate through previous y
for prev_theta in range(mapper.MAX_CELLS_A): #iterate through previous angles
if (loc.bel[prev_x][prev_y][prev_theta]>0.0001):
for cur_x in range(mapper.MAX_CELLS_X): #iterate through current x
for cur_y in range(mapper.MAX_CELLS_Y): #iterate through current y
for cur_theta in range(mapper.MAX_CELLS_A): #iterate through current angles
curr_pose = mapper.from_map(cur_x,cur_y,cur_theta)
prev_pose = mapper.from_map(prev_x,prev_y,prev_theta)
loc.bel_bar[cur_x][cur_y][cur_theta] += odom_motion_model(curr_pose,prev_pose, u) * loc.bel[prev_x][prev_y][prev_theta]
loc.bel_bar = loc.bel_bar/np.sum(loc.bel_bar)
Sensor Model
THis function basically takes in the current true robot pose and the current_pose that was observed. The probability is calculated using the loc.gaussian function and this returns the probability values (likelihoods) of each of the sensor measurements.
def sensor_model(obs,current_pose):
""" This is the equivalent of p(z|x).
Args:
obs ([ndarray]): A 1D array consisting of the true observations for a specific robot pose in the map
current_pose(tuple): Tuple consisting of the current pose (x,y,theta)
Returns:
[ndarray]: Returns a 1D array of size 18 (=loc.OBS_PER_CELL) with the likelihoods of each individual sensor measurement
"""
(x,y,theta) = current_pose
prob_array = np.zeros(mapper.OBS_PER_CELL)
for i in range(mapper.OBS_PER_CELL):
prob_array[i] = loc.gaussian(obs[i],loc.mapper.get_views(x,y,theta)[i],loc.sensor_sigma)
return prob_array
Update Step
After prediction and getting the sensor values, we update the beliefs of each of the loc.bel by multiplying the sensor model outputs with the bayesian prediction to get the final beliefs. We then normalize loc.bel like we did with bel_bar in the prediction step.
def update_step(obs):
""" Update step of the Bayes Filter.
Update the probabilities in loc.bel based on loc.bel_bar and the sensor model.
"""
for x in range(mapper.MAX_CELLS_X):
for y in range(mapper.MAX_CELLS_Y):
for a in range(mapper.MAX_CELLS_A):
loc.bel[x][y][a] = np.prod(sensor_model(loc.obs_range_data,(x,y,a)))*loc.bel_bar[x][y][a]
loc.bel = loc.bel / np.sum(loc.bel)
Simulation video
In the video below, the Green color depicts the ground truth, the red depicts the odometry values and the blue depicts the Bayesian estimations. As we can see, the bayesian predictions are pretty close to the ground truth and much much better than the odometry reading predictions.
Inference
Prediction and belief for each of the iterations. From the Ground truth and belief shown below, we can see that the beliefs of the Bayesian filter pretty closely match the ground truth. From this I infer that using probabilistic robotics is extremely essential for robotics and localization.
----------------- 0 -----------------
2022-04-26 02:42:56,291 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:42:56,313 | INFO |: GT index : (6, 3, 6)
2022-04-26 02:42:56,316 | INFO |: Prior Bel index : (5, 1, 5) with prob = 0.0835766
2022-04-26 02:42:56,319 | INFO |: POS ERROR : (0.291, 0.824, 29.893)
2022-04-26 02:42:56,323 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:43:04,524 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:43:04,530 | INFO |: GT index : (6, 3, 6)
2022-04-26 02:43:04,532 | INFO |: Bel index : (6, 4, 6) with prob = 0.9999999
2022-04-26 02:43:04,535 | INFO |: Bel_bar prob at index = 6.029659901426069e-05
2022-04-26 02:43:04,539 | INFO |: GT : (0.291, -0.090, 679.893)
2022-04-26 02:43:04,541 | INFO |: Belief : (0.305, 0.000, -50.000)
2022-04-26 02:43:04,543 | INFO |: POS ERROR : (-0.013, -0.090, 729.893)
2022-04-26 02:43:04,548 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 1 -----------------
2022-04-26 02:43:06,826 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:43:06,834 | INFO |: GT index : (7, 2, 5)
2022-04-26 02:43:06,837 | INFO |: Prior Bel index : (6, 2, 7) with prob = 0.0629984
2022-04-26 02:43:06,840 | INFO |: POS ERROR : (0.207, 0.080, 686.591)
2022-04-26 02:43:06,845 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:43:14,255 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:43:14,261 | INFO |: GT index : (7, 2, 5)
2022-04-26 02:43:14,263 | INFO |: Bel index : (6, 2, 5) with prob = 1.0
2022-04-26 02:43:14,267 | INFO |: Bel_bar prob at index = 0.0003454142551285154
2022-04-26 02:43:14,271 | INFO |: GT : (0.511, -0.530, 1376.591)
2022-04-26 02:43:14,274 | INFO |: Belief : (0.305, -0.610, -70.000)
2022-04-26 02:43:14,278 | INFO |: POS ERROR : (0.207, 0.080, 1446.591)
2022-04-26 02:43:14,282 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 2 -----------------
2022-04-26 02:43:15,581 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:43:15,591 | INFO |: GT index : (7, 2, 4)
2022-04-26 02:43:15,593 | INFO |: Prior Bel index : (5, 2, 3) with prob = 0.1330150
2022-04-26 02:43:15,596 | INFO |: POS ERROR : (0.511, 0.080, 1463.292)
2022-04-26 02:43:15,602 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:43:23,192 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:43:23,202 | INFO |: GT index : (7, 2, 4)
2022-04-26 02:43:23,204 | INFO |: Bel index : (7, 2, 4) with prob = 1.0
2022-04-26 02:43:23,207 | INFO |: Bel_bar prob at index = 8.747796601759297e-07
2022-04-26 02:43:23,209 | INFO |: GT : (0.511, -0.530, 2073.292)
2022-04-26 02:43:23,213 | INFO |: Belief : (0.610, -0.610, -90.000)
2022-04-26 02:43:23,215 | INFO |: POS ERROR : (-0.098, 0.080, 2163.292)
2022-04-26 02:43:23,219 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 3 -----------------
2022-04-26 02:43:24,474 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:43:24,488 | INFO |: GT index : (7, 0, 4)
2022-04-26 02:43:24,490 | INFO |: Prior Bel index : (8, 0, 4) with prob = 0.1186880
2022-04-26 02:43:24,493 | INFO |: POS ERROR : (-0.380, 0.283, 2163.292)
2022-04-26 02:43:24,497 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:43:32,095 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:43:32,107 | INFO |: GT index : (7, 0, 4)
2022-04-26 02:43:32,110 | INFO |: Bel index : (7, 1, 4) with prob = 1.0
2022-04-26 02:43:32,114 | INFO |: Bel_bar prob at index = 0.00426564861796035
2022-04-26 02:43:32,118 | INFO |: GT : (0.535, -0.936, 2793.292)
2022-04-26 02:43:32,123 | INFO |: Belief : (0.610, -0.914, -90.000)
2022-04-26 02:43:32,126 | INFO |: POS ERROR : (-0.075, -0.021, 2883.292)
2022-04-26 02:43:32,131 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 4 -----------------
2022-04-26 02:43:35,427 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:43:35,437 | INFO |: GT index : (8, 0, 8)
2022-04-26 02:43:35,440 | INFO |: Prior Bel index : (8, 0, 7) with prob = 0.0986229
2022-04-26 02:43:35,443 | INFO |: POS ERROR : (-0.118, 0.136, 2909.241)
2022-04-26 02:43:35,449 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:43:42,782 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:43:42,801 | INFO |: GT index : (8, 0, 8)
2022-04-26 02:43:42,804 | INFO |: Bel index : (8, 1, 9) with prob = 1.0
2022-04-26 02:43:42,811 | INFO |: Bel_bar prob at index = 0.0016077695467455848
2022-04-26 02:43:42,815 | INFO |: GT : (0.796, -1.083, 3599.241)
2022-04-26 02:43:42,817 | INFO |: Belief : (0.914, -0.914, 10.000)
2022-04-26 02:43:42,823 | INFO |: POS ERROR : (-0.118, -0.169, 3589.241)
2022-04-26 02:43:42,826 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 5 -----------------
2022-04-26 02:43:49,121 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:43:49,136 | INFO |: GT index : (11, 0, 11)
2022-04-26 02:43:49,139 | INFO |: Prior Bel index : (11, 1, 11) with prob = 0.0909999
2022-04-26 02:43:49,144 | INFO |: POS ERROR : (-0.253, -0.020, 3598.934)
2022-04-26 02:43:49,151 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:43:57,132 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:43:57,139 | INFO |: GT index : (11, 0, 11)
2022-04-26 02:43:57,142 | INFO |: Bel index : (10, 1, 11) with prob = 1.0
2022-04-26 02:43:57,146 | INFO |: Bel_bar prob at index = 0.07093688334116348
2022-04-26 02:43:57,151 | INFO |: GT : (1.575, -0.934, 4368.934)
2022-04-26 02:43:57,154 | INFO |: Belief : (1.524, -0.914, 50.000)
2022-04-26 02:43:57,158 | INFO |: POS ERROR : (0.051, -0.020, 4318.934)
2022-04-26 02:43:57,166 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 6 -----------------
2022-04-26 02:43:59,533 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:43:59,541 | INFO |: GT index : (11, 2, 12)
2022-04-26 02:43:59,543 | INFO |: Prior Bel index : (9, 3, 12) with prob = 0.1001863
2022-04-26 02:43:59,545 | INFO |: POS ERROR : (0.439, -0.238, 4328.053)
2022-04-26 02:43:59,547 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:44:07,145 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:44:07,152 | INFO |: GT index : (11, 2, 12)
2022-04-26 02:44:07,155 | INFO |: Bel index : (10, 2, 12) with prob = 0.9999999
2022-04-26 02:44:07,159 | INFO |: Bel_bar prob at index = 0.013530256305397908
2022-04-26 02:44:07,163 | INFO |: GT : (1.658, -0.543, 5118.053)
2022-04-26 02:44:07,168 | INFO |: Belief : (1.524, -0.610, 70.000)
2022-04-26 02:44:07,172 | INFO |: POS ERROR : (0.134, 0.066, 5048.053)
2022-04-26 02:44:07,177 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 7 -----------------
2022-04-26 02:44:09,430 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:44:09,446 | INFO |: GT index : (11, 3, 13)
2022-04-26 02:44:09,448 | INFO |: Prior Bel index : (11, 3, 13) with prob = 0.1629538
2022-04-26 02:44:09,450 | INFO |: POS ERROR : (-0.095, 0.120, 5033.771)
2022-04-26 02:44:09,453 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:44:17,270 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:44:17,280 | INFO |: GT index : (11, 3, 13)
2022-04-26 02:44:17,282 | INFO |: Bel index : (11, 3, 13) with prob = 1.0
2022-04-26 02:44:17,286 | INFO |: Bel_bar prob at index = 0.16295380723050845
2022-04-26 02:44:17,290 | INFO |: GT : (1.734, -0.185, 5843.771)
2022-04-26 02:44:17,292 | INFO |: Belief : (1.829, -0.305, 90.000)
2022-04-26 02:44:17,296 | INFO |: POS ERROR : (-0.095, 0.120, 5753.771)
2022-04-26 02:44:17,300 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 8 -----------------
2022-04-26 02:44:20,720 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:44:20,725 | INFO |: GT index : (11, 5, 14)
2022-04-26 02:44:20,728 | INFO |: Prior Bel index : (11, 3, 14) with prob = 0.1165633
2022-04-26 02:44:20,732 | INFO |: POS ERROR : (-0.090, 0.628, 5756.382)
2022-04-26 02:44:20,738 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:44:28,393 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:44:28,400 | INFO |: GT index : (11, 5, 14)
2022-04-26 02:44:28,403 | INFO |: Bel index : (11, 5, 14) with prob = 0.9996599
2022-04-26 02:44:28,406 | INFO |: Bel_bar prob at index = 0.05574271931545838
2022-04-26 02:44:28,409 | INFO |: GT : (1.739, 0.323, 6586.382)
2022-04-26 02:44:28,412 | INFO |: Belief : (1.829, 0.305, 110.000)
2022-04-26 02:44:28,415 | INFO |: POS ERROR : (-0.090, 0.018, 6476.382)
2022-04-26 02:44:28,419 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 9 -----------------
2022-04-26 02:44:31,939 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:44:31,959 | INFO |: GT index : (11, 6, 16)
2022-04-26 02:44:31,961 | INFO |: Prior Bel index : (11, 5, 14) with prob = 0.2665198
2022-04-26 02:44:31,966 | INFO |: POS ERROR : (-0.086, 0.348, 6516.511)
2022-04-26 02:44:31,974 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:44:39,693 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:44:39,699 | INFO |: GT index : (11, 6, 16)
2022-04-26 02:44:39,706 | INFO |: Bel index : (11, 7, 16) with prob = 1.0
2022-04-26 02:44:39,710 | INFO |: Bel_bar prob at index = 0.01737914821475014
2022-04-26 02:44:39,715 | INFO |: GT : (1.743, 0.653, 7346.510)
2022-04-26 02:44:39,718 | INFO |: Belief : (1.829, 0.914, 150.000)
2022-04-26 02:44:39,722 | INFO |: POS ERROR : (-0.086, -0.261, 7196.510)
2022-04-26 02:44:39,728 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 10 -----------------
2022-04-26 02:44:41,994 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:44:42,005 | INFO |: GT index : (10, 7, 16)
2022-04-26 02:44:42,009 | INFO |: Prior Bel index : (9, 6, 1) with prob = 0.0976445
2022-04-26 02:44:42,012 | INFO |: POS ERROR : (0.100, 0.324, 7507.755)
2022-04-26 02:44:42,016 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:44:49,829 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:44:49,836 | INFO |: GT index : (10, 7, 16)
2022-04-26 02:44:49,840 | INFO |: Bel index : (10, 7, 16) with prob = 1.0
2022-04-26 02:44:49,844 | INFO |: Bel_bar prob at index = 1.231130239462519e-05
2022-04-26 02:44:49,848 | INFO |: GT : (1.319, 0.934, 8077.755)
2022-04-26 02:44:49,852 | INFO |: Belief : (1.524, 0.914, 150.000)
2022-04-26 02:44:49,856 | INFO |: POS ERROR : (-0.205, 0.019, 7927.755)
2022-04-26 02:44:49,860 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 11 -----------------
2022-04-26 02:44:53,178 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:44:53,183 | INFO |: GT index : (7, 6, 3)
2022-04-26 02:44:53,184 | INFO |: Prior Bel index : (9, 5, 7) with prob = 0.0729318
2022-04-26 02:44:53,187 | INFO |: POS ERROR : (-0.779, 0.523, 8205.643)
2022-04-26 02:44:53,189 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:45:00,891 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:45:00,896 | INFO |: GT index : (7, 6, 3)
2022-04-26 02:45:00,899 | INFO |: Bel index : (7, 7, 3) with prob = 0.9999999
2022-04-26 02:45:00,901 | INFO |: Bel_bar prob at index = 1.1577674324137937e-13
2022-04-26 02:45:00,904 | INFO |: GT : (0.441, 0.828, 8895.644)
2022-04-26 02:45:00,907 | INFO |: Belief : (0.610, 0.914, -110.000)
2022-04-26 02:45:00,911 | INFO |: POS ERROR : (-0.169, -0.087, 9005.644)
2022-04-26 02:45:00,915 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 12 -----------------
2022-04-26 02:45:03,292 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:45:03,306 | INFO |: GT index : (6, 4, 6)
2022-04-26 02:45:03,309 | INFO |: Prior Bel index : (6, 4, 6) with prob = 0.0692578
2022-04-26 02:45:03,315 | INFO |: POS ERROR : (-0.025, 0.198, 8990.726)
2022-04-26 02:45:03,320 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:45:11,262 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:45:11,268 | INFO |: GT index : (6, 4, 6)
2022-04-26 02:45:11,269 | INFO |: Bel index : (6, 5, 5) with prob = 1.0
2022-04-26 02:45:11,271 | INFO |: Bel_bar prob at index = 0.004686688665216406
2022-04-26 02:45:11,273 | INFO |: GT : (0.279, 0.198, 9660.726)
2022-04-26 02:45:11,275 | INFO |: Belief : (0.305, 0.305, -70.000)
2022-04-26 02:45:11,277 | INFO |: POS ERROR : (-0.025, -0.107, 9730.726)
2022-04-26 02:45:11,280 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 13 -----------------
2022-04-26 02:45:13,508 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:45:13,517 | INFO |: GT index : (6, 3, 2)
2022-04-26 02:45:13,519 | INFO |: Prior Bel index : (6, 3, 3) with prob = 0.0652651
2022-04-26 02:45:13,522 | INFO |: POS ERROR : (-0.272, 0.188, 9701.957)
2022-04-26 02:45:13,527 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:45:21,475 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:45:21,486 | INFO |: GT index : (6, 3, 2)
2022-04-26 02:45:21,489 | INFO |: Bel index : (5, 3, 2) with prob = 1.0
2022-04-26 02:45:21,491 | INFO |: Bel_bar prob at index = 0.058094902634315396
2022-04-26 02:45:21,494 | INFO |: GT : (0.033, -0.117, 10311.957)
2022-04-26 02:45:21,498 | INFO |: Belief : (0.000, -0.305, -130.000)
2022-04-26 02:45:21,502 | INFO |: POS ERROR : (0.033, 0.188, 10441.957)
2022-04-26 02:45:21,505 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 14 -----------------
2022-04-26 02:45:24,802 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:45:24,820 | INFO |: GT index : (4, 3, 1)
2022-04-26 02:45:24,822 | INFO |: Prior Bel index : (2, 2, 2) with prob = 0.0648171
2022-04-26 02:45:24,825 | INFO |: POS ERROR : (0.580, 0.334, 10419.034)
2022-04-26 02:45:24,830 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:45:32,587 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:45:32,595 | INFO |: GT index : (4, 3, 1)
2022-04-26 02:45:32,597 | INFO |: Bel index : (4, 3, 1) with prob = 1.0
2022-04-26 02:45:32,601 | INFO |: Bel_bar prob at index = 0.013334336765791049
2022-04-26 02:45:32,604 | INFO |: GT : (-0.334, -0.275, 11009.034)
2022-04-26 02:45:32,607 | INFO |: Belief : (-0.305, -0.305, -150.000)
2022-04-26 02:45:32,611 | INFO |: POS ERROR : (-0.030, 0.030, 11159.034)
2022-04-26 02:45:32,614 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 15 -----------------
2022-04-26 02:45:35,887 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:45:35,903 | INFO |: GT index : (3, 3, 0)
2022-04-26 02:45:35,907 | INFO |: Prior Bel index : (2, 2, 15) with prob = 0.1116387
2022-04-26 02:45:35,909 | INFO |: POS ERROR : (0.180, 0.332, 10856.111)
2022-04-26 02:45:35,913 | INFO |: ---------- PREDICTION STATS -----------
2022-04-26 02:45:43,597 | INFO |: ---------- UPDATE STATS -----------
2022-04-26 02:45:43,611 | INFO |: GT index : (3, 3, 0)
2022-04-26 02:45:43,612 | INFO |: Bel index : (3, 2, 0) with prob = 0.9999999
2022-04-26 02:45:43,614 | INFO |: Bel_bar prob at index = 1.1378120764054344e-08
2022-04-26 02:45:43,616 | INFO |: GT : (-0.734, -0.278, 11706.111)
2022-04-26 02:45:43,619 | INFO |: Belief : (-0.610, -0.610, -170.000)
2022-04-26 02:45:43,621 | INFO |: POS ERROR : (-0.125, 0.332, 11876.111)
2022-04-26 02:45:43,624 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------