Ryan's ECE 4960 Site
Lab 11
In this lab, we implemented a Bayes Filter to perform grid localization with a simulator in Python in a grid similar to this:
Specifically, most of the work in this lab was dedicated to implementing the algorithm shown below along with its helper functions:
Compute Control
The first function I implemented was the compute_control function. This function takes a current and previous pose and computes the required actions to get the robot from the previous pose to the current pose. Essentially, it computes the "u" in the Bayes Filter:
More specifically, it computes the first rotation (delta_rot_1), the translation (delta_trans), and the second rotation (delta_rot_2) described in this diagram from lecture:
I used simple trigonometry using the inverse tangent and the difference of the current and previous pose in the x and y axes to calculate the first rotation. Then I used the Pythagorean Theorem and the difference of the current and previous pose in the x and y axes to calculate the translation. Finally, to get the third rotation, I subtracted the first rotation and previous pose rotation from the current pose and normalized it. Here is the code I wrote for this function:
def compute_control(cur_pose, prev_pose):
new_rot_1 = (180/np.pi)*np.arctan2((cur_pose[1] - prev_pose[1]), (cur_pose[0]-prev_pose[0]))
delta_rot_1 = loc.mapper.normalize_angle(new_rot_1 - prev_pose[2])
delta_trans = np.sqrt((cur_pose[0]-prev_pose[0])**2 + (cur_pose[1] - prev_pose[1])**2)
delta_rot_2 = loc.mapper.normalize_angle(cur_pose[2] - delta_rot_1 - prev_pose[2])
return delta_rot_1, delta_trans, delta_rot_2
Motion Model
Next, I implemented the odom_motion_model function, which correlates to this part of the Bayes Filter:
This function takes the current and previous pose as well as the control input u as parameters. Then it essentially computes the probability that the robot is at the current pose given the previous pose and control input u. To calculate this, I first calculated the control input between the current and previous poses that were passed into the function. Then I used the Gaussian function to essentially "compare the similarity" between the passed in control input u and the other control input calculated from the passed in current and previous pose - the closer the values are the higher the relative likelihood and the farther the values are the lower the relative likelihood. I did this for each of the two rotations and the translation and multiplied the likelihoods together, which I returned. Here is the code I wrote for this function:
def odom_motion_model(cur_pose, prev_pose, u):
x_i_delta_rot_1, x_i_delta_trans, x_i_delta_rot_2 = compute_control(cur_pose,prev_pose)
rot_1_prob = loc.gaussian(x_i_delta_rot_1,u[0],loc.odom_rot_sigma)
trans_prob = loc.gaussian(x_i_delta_trans,u[1],loc.odom_trans_sigma)
rot_2_prob = loc.gaussian(x_i_delta_rot_2,u[2],loc.odom_rot_sigma)
prob = rot_1_prob * trans_prob * rot_2_prob
return prob
Prediction Step
Next, I implemented the prediction_step function, which correlates to these parts of the Bayes Filter:
This function takes in the current and previous odometry data as parameters. Then it essentially fills the bel_bar matrix with the marginal probability distribution of where it thinks it is without knowing its sensor measurements yet. To calculate this, I used 6 nested for loops to represent the outer loop in the Bayes Filter and the inner summation on the second line of the Bayes Filter (there are 6 nested loops instead of 2 as expected from the Bayes Filter formula because we are looking at 3 dimensions). Since this is an incredibly expensive operation, I optimized it by including an if statement in the middle that checks to see if it is worth computing (I first ran it without this optimization and it took over 30 seconds each time). At the inside of the last nested for loops, I essentially just transcribed the contents of the summation on line 2 of the Bayes Filter in Python, which was very easy with the helper functions. Finally, when the loops were all done, I normalized everything. Here is the code I wrote for this function:
def prediction_step(cur_odom, prev_odom):
u = compute_control(cur_odom, prev_odom)
for x_t_minus_1_i in range(mapper.MAX_CELLS_X):
for y_t_minus_1_i in range(mapper.MAX_CELLS_Y):
for a_t_minus_1_i in range(mapper.MAX_CELLS_A):
if loc.bel[x_t_minus_1_i,y_t_minus_1_i,a_t_minus_1_i] > 0.0001:
for x_t_i in range(mapper.MAX_CELLS_X):
for y_t_i in range(mapper.MAX_CELLS_Y):
for a_t_i in range(mapper.MAX_CELLS_A):
pose_t_i = mapper.from_map(x_t_i,y_t_i,a_t_i)
pose_t_minus_1_i = mapper.from_map(x_t_minus_1_i,y_t_minus_1_i,a_t_minus_1_i)
loc.bel_bar[x_t_i, y_t_i, a_t_i] += odom_motion_model(pose_t_i, pose_t_minus_1_i,u) * loc.bel[x_t_minus_1_i,y_t_minus_1_i,a_t_minus_1_i]
eta = 1/(np.sum(loc.bel_bar)) # normalization constant
loc.bel_bar = loc.bel_bar*eta # normalize
Sensor Model
Next, I implemented the sensor_model function, which correlates to this part of the Bayes Filter:
This function takes in an array of the 18 observed sensor measurements as well as an array of the 18 precached sensor measurements at that position. Then it calculates the probability that the sensor measurements are true at that position using the precached values at that point. The way it does this is similar to the Motion Model, where it uses the Gaussian function to essentially "compare the similarity" between the measured sensor values and the precached values at that position - the closer the values are the higher the relative likelihood and the farther the values are the lower the relative likelihood. Each of the likelihoods is stored in an array that is returned (they will be multiplied together in the Update Step). Here is the code I wrote for this function:
def sensor_model(obs, precached):
prob_array = []
for i in range(18):
prob_array.append(loc.gaussian(obs[i],precached[i],loc.sensor_sigma))
return prob_array
Update Step
Next, I implemented the update_step function, which correlates to this part of the Bayes Filter:
This function essentially just updates the bel matrix with the probability distribution of where it thinks the robot is given the sensor measurements and bel_bar. To calculate this, I used three nested for loops with one loop for each dimension. At the inside of the last nested for loop, I essentially just transcribed line 3 of the Bayes Filter in Python. Finally, I normalized everything. Here is the code I wrote for this function:
def update_step():
for x_t_i in range(mapper.MAX_CELLS_X):
for y_t_i in range(mapper.MAX_CELLS_Y):
for a_t_i in range(mapper.MAX_CELLS_A):
#sensor_model step is not right at all - returns an ARRAY, should probably be a float:
loc.bel[x_t_i, y_t_i, a_t_i] = np.prod(sensor_model(loc.obs_range_data,mapper.get_views(x_t_i,y_t_i,a_t_i))) * loc.bel_bar[x_t_i, y_t_i, a_t_i]
eta = 1/(np.sum(loc.bel)) # normalization constant
loc.bel = loc.bel * eta # normalize
Trying the Bayes Filter!
Finally, I ran the code to see how well my Bayes Filter implementation worked as the robot traversed its pre-planned trajectory. Below are videos of these demonstrations, with and without the marginal distribution plotting.
Plotting Ground Truth (green), Belief (blue), and Odometry (red):
Plotting Ground Truth (green), Belief (blue), Odometry (red), and Marginal Distribution (white blocks; whiter means greater probability):
Below is the console output of the specific stats of one of the runs. As can be seen, the most probable states match the the ground truth pose very closely for the X and Y dimensions. The Bayes Filter seems to work well here for these X and Y values because in this simulated environment the sensor data is fairly accurate and precise, but it will likely work much less well when given less reliable and more noisy sensors. However, the yaw angle dimensions do not match very well, and I think this is probably due to me normalizing the angles.
----------------- 0 -----------------
2022-04-25 21:04:22,515 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:04:22,521 | INFO |: GT index : (6, 3, 6)
2022-04-25 21:04:22,522 | INFO |: Prior Bel index : (4, 3, 3) with prob = 0.1074386
2022-04-25 21:04:22,523 | INFO |: POS ERROR : (0.591, 0.215, 69.607)
2022-04-25 21:04:22,525 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:04:25,886 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:04:25,893 | INFO |: GT index : (6, 3, 6)
2022-04-25 21:04:25,894 | INFO |: Bel index : (6, 4, 6) with prob = 1.0
2022-04-25 21:04:25,895 | INFO |: Bel_bar prob at index = 0.00013881729127078254
2022-04-25 21:04:25,896 | INFO |: GT : (0.286, -0.090, 319.607)
2022-04-25 21:04:25,897 | INFO |: Belief : (0.305, 0.000, -50.000)
2022-04-25 21:04:25,898 | INFO |: POS ERROR : (-0.019, -0.090, 369.607)
2022-04-25 21:04:25,899 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 1 -----------------
2022-04-25 21:04:28,023 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:04:28,041 | INFO |: GT index : (7, 2, 5)
2022-04-25 21:04:28,042 | INFO |: Prior Bel index : (5, 1, 3) with prob = 0.0821984
2022-04-25 21:04:28,046 | INFO |: POS ERROR : (0.511, 0.369, 406.306)
2022-04-25 21:04:28,049 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:04:31,452 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:04:31,458 | INFO |: GT index : (7, 2, 5)
2022-04-25 21:04:31,459 | INFO |: Bel index : (7, 2, 6) with prob = 1.0
2022-04-25 21:04:31,460 | INFO |: Bel_bar prob at index = 0.0005083305329467872
2022-04-25 21:04:31,461 | INFO |: GT : (0.511, -0.546, 656.306)
2022-04-25 21:04:31,461 | INFO |: Belief : (0.610, -0.610, -50.000)
2022-04-25 21:04:31,462 | INFO |: POS ERROR : (-0.098, 0.064, 706.306)
2022-04-25 21:04:31,467 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 2 -----------------
2022-04-25 21:04:32,552 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:04:32,562 | INFO |: GT index : (7, 2, 4)
2022-04-25 21:04:32,563 | INFO |: Prior Bel index : (6, 4, 4) with prob = 0.1503744
2022-04-25 21:04:32,564 | INFO |: POS ERROR : (0.207, -0.546, 723.386)
2022-04-25 21:04:32,565 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:04:35,908 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:04:35,920 | INFO |: GT index : (7, 2, 4)
2022-04-25 21:04:35,920 | INFO |: Bel index : (7, 2, 4) with prob = 0.9999999
2022-04-25 21:04:35,921 | INFO |: Bel_bar prob at index = 6.287308582071773e-05
2022-04-25 21:04:35,921 | INFO |: GT : (0.511, -0.546, 993.386)
2022-04-25 21:04:35,922 | INFO |: Belief : (0.610, -0.610, -90.000)
2022-04-25 21:04:35,923 | INFO |: POS ERROR : (-0.098, 0.064, 1083.386)
2022-04-25 21:04:35,923 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 3 -----------------
2022-04-25 21:04:37,030 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:04:37,044 | INFO |: GT index : (7, 0, 4)
2022-04-25 21:04:37,045 | INFO |: Prior Bel index : (8, 0, 5) with prob = 0.1644957
2022-04-25 21:04:37,046 | INFO |: POS ERROR : (-0.379, 0.267, 1063.386)
2022-04-25 21:04:37,047 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:04:40,439 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:04:40,455 | INFO |: GT index : (7, 0, 4)
2022-04-25 21:04:40,456 | INFO |: Bel index : (6, 1, 4) with prob = 0.9999983
2022-04-25 21:04:40,457 | INFO |: Bel_bar prob at index = 0.0021077392866740005
2022-04-25 21:04:40,458 | INFO |: GT : (0.535, -0.952, 1353.386)
2022-04-25 21:04:40,459 | INFO |: Belief : (0.305, -0.914, -90.000)
2022-04-25 21:04:40,460 | INFO |: POS ERROR : (0.231, -0.037, 1443.386)
2022-04-25 21:04:40,461 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 4 -----------------
2022-04-25 21:04:43,585 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:04:43,587 | INFO |: GT index : (8, 0, 8)
2022-04-25 21:04:43,588 | INFO |: Prior Bel index : (7, 2, 10) with prob = 0.0971906
2022-04-25 21:04:43,589 | INFO |: POS ERROR : (0.192, -0.491, 1409.806)
2022-04-25 21:04:43,591 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:04:46,976 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:04:46,993 | INFO |: GT index : (8, 0, 8)
2022-04-25 21:04:46,993 | INFO |: Bel index : (8, 1, 9) with prob = 1.0
2022-04-25 21:04:46,994 | INFO |: Bel_bar prob at index = 0.00034693014259250314
2022-04-25 21:04:46,995 | INFO |: GT : (0.801, -1.101, 1799.806)
2022-04-25 21:04:46,996 | INFO |: Belief : (0.914, -0.914, 10.000)
2022-04-25 21:04:46,996 | INFO |: POS ERROR : (-0.113, -0.187, 1789.806)
2022-04-25 21:04:46,997 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 5 -----------------
2022-04-25 21:04:53,127 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:04:53,142 | INFO |: GT index : (11, 0, 11)
2022-04-25 21:04:53,143 | INFO |: Prior Bel index : (10, 0, 11) with prob = 0.1668739
2022-04-25 21:04:53,144 | INFO |: POS ERROR : (0.075, 0.280, 1800.225)
2022-04-25 21:04:53,145 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:04:56,511 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:04:56,516 | INFO |: GT index : (11, 0, 11)
2022-04-25 21:04:56,517 | INFO |: Bel index : (10, 1, 11) with prob = 1.0
2022-04-25 21:04:56,518 | INFO |: Bel_bar prob at index = 0.004791596696405744
2022-04-25 21:04:56,519 | INFO |: GT : (1.599, -0.940, 2210.225)
2022-04-25 21:04:56,520 | INFO |: Belief : (1.524, -0.914, 50.000)
2022-04-25 21:04:56,522 | INFO |: POS ERROR : (0.075, -0.025, 2160.225)
2022-04-25 21:04:56,523 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 6 -----------------
2022-04-25 21:04:58,634 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:04:58,638 | INFO |: GT index : (11, 2, 12)
2022-04-25 21:04:58,639 | INFO |: Prior Bel index : (11, 2, 13) with prob = 0.2058537
2022-04-25 21:04:58,640 | INFO |: POS ERROR : (-0.155, 0.070, 2149.357)
2022-04-25 21:04:58,640 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:02,008 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:05:02,010 | INFO |: GT index : (11, 2, 12)
2022-04-25 21:05:02,011 | INFO |: Bel index : (10, 2, 12) with prob = 0.9999999
2022-04-25 21:05:02,012 | INFO |: Bel_bar prob at index = 2.909084993446881e-06
2022-04-25 21:05:02,013 | INFO |: GT : (1.674, -0.540, 2599.356)
2022-04-25 21:05:02,013 | INFO |: Belief : (1.524, -0.610, 70.000)
2022-04-25 21:05:02,014 | INFO |: POS ERROR : (0.150, 0.070, 2529.356)
2022-04-25 21:05:02,015 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 7 -----------------
2022-04-25 21:05:04,107 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:04,125 | INFO |: GT index : (11, 3, 13)
2022-04-25 21:05:04,125 | INFO |: Prior Bel index : (9, 4, 13) with prob = 0.0889599
2022-04-25 21:05:04,126 | INFO |: POS ERROR : (0.522, -0.180, 2514.992)
2022-04-25 21:05:04,127 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:07,449 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:05:07,464 | INFO |: GT index : (11, 3, 13)
2022-04-25 21:05:07,465 | INFO |: Bel index : (11, 3, 13) with prob = 1.0
2022-04-25 21:05:07,466 | INFO |: Bel_bar prob at index = 0.004101168849040615
2022-04-25 21:05:07,467 | INFO |: GT : (1.741, -0.180, 2964.992)
2022-04-25 21:05:07,468 | INFO |: Belief : (1.829, -0.305, 90.000)
2022-04-25 21:05:07,469 | INFO |: POS ERROR : (-0.088, 0.125, 2874.992)
2022-04-25 21:05:07,470 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 8 -----------------
2022-04-25 21:05:10,604 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:10,617 | INFO |: GT index : (11, 5, 14)
2022-04-25 21:05:10,618 | INFO |: Prior Bel index : (10, 4, 15) with prob = 0.1116641
2022-04-25 21:05:10,619 | INFO |: POS ERROR : (0.209, 0.320, 2858.392)
2022-04-25 21:05:10,624 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:13,935 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:05:13,944 | INFO |: GT index : (11, 5, 14)
2022-04-25 21:05:13,945 | INFO |: Bel index : (11, 5, 14) with prob = 0.9998590
2022-04-25 21:05:13,946 | INFO |: Bel_bar prob at index = 0.004612065584525311
2022-04-25 21:05:13,946 | INFO |: GT : (1.733, 0.320, 3348.392)
2022-04-25 21:05:13,947 | INFO |: Belief : (1.829, 0.305, 110.000)
2022-04-25 21:05:13,948 | INFO |: POS ERROR : (-0.096, 0.015, 3238.392)
2022-04-25 21:05:13,949 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 9 -----------------
2022-04-25 21:05:17,118 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:17,125 | INFO |: GT index : (11, 6, 16)
2022-04-25 21:05:17,126 | INFO |: Prior Bel index : (10, 4, 15) with prob = 0.0988329
2022-04-25 21:05:17,127 | INFO |: POS ERROR : (0.202, 0.650, 3259.450)
2022-04-25 21:05:17,129 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:20,426 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:05:20,433 | INFO |: GT index : (11, 6, 16)
2022-04-25 21:05:20,434 | INFO |: Bel index : (11, 7, 16) with prob = 1.0
2022-04-25 21:05:20,435 | INFO |: Bel_bar prob at index = 1.5448765854544247e-05
2022-04-25 21:05:20,436 | INFO |: GT : (1.726, 0.650, 3749.450)
2022-04-25 21:05:20,438 | INFO |: Belief : (1.829, 0.914, 150.000)
2022-04-25 21:05:20,440 | INFO |: POS ERROR : (-0.102, -0.265, 3599.450)
2022-04-25 21:05:20,441 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 10 -----------------
2022-04-25 21:05:22,544 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:22,546 | INFO |: GT index : (10, 6, 17)
2022-04-25 21:05:22,547 | INFO |: Prior Bel index : (11, 8, 16) with prob = 0.1184147
2022-04-25 21:05:22,548 | INFO |: POS ERROR : (-0.540, -0.311, 3610.912)
2022-04-25 21:05:22,550 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:25,865 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:05:25,874 | INFO |: GT index : (10, 6, 17)
2022-04-25 21:05:25,875 | INFO |: Bel index : (9, 7, 17) with prob = 0.9999999
2022-04-25 21:05:25,876 | INFO |: Bel_bar prob at index = 1.127051139552993e-05
2022-04-25 21:05:25,876 | INFO |: GT : (1.289, 0.908, 4120.912)
2022-04-25 21:05:25,877 | INFO |: Belief : (1.219, 0.914, 170.000)
2022-04-25 21:05:25,878 | INFO |: POS ERROR : (0.069, -0.006, 3950.912)
2022-04-25 21:05:25,879 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 11 -----------------
2022-04-25 21:05:28,999 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:29,006 | INFO |: GT index : (7, 6, 4)
2022-04-25 21:05:29,007 | INFO |: Prior Bel index : (6, 8, 2) with prob = 0.0687248
2022-04-25 21:05:29,008 | INFO |: POS ERROR : (0.114, -0.472, 4350.396)
2022-04-25 21:05:29,009 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:32,335 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:05:32,349 | INFO |: GT index : (7, 6, 4)
2022-04-25 21:05:32,350 | INFO |: Bel index : (7, 7, 3) with prob = 0.7120126
2022-04-25 21:05:32,351 | INFO |: Bel_bar prob at index = 0.038325344273912446
2022-04-25 21:05:32,352 | INFO |: GT : (0.418, 0.747, 4580.397)
2022-04-25 21:05:32,353 | INFO |: Belief : (0.610, 0.914, -110.000)
2022-04-25 21:05:32,354 | INFO |: POS ERROR : (-0.191, -0.168, 4690.397)
2022-04-25 21:05:32,355 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 12 -----------------
2022-04-25 21:05:34,587 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:34,602 | INFO |: GT index : (7, 4, 6)
2022-04-25 21:05:34,603 | INFO |: Prior Bel index : (6, 4, 6) with prob = 0.0450771
2022-04-25 21:05:34,603 | INFO |: POS ERROR : (0.003, 0.095, 4676.243)
2022-04-25 21:05:34,605 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:37,975 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:05:37,991 | INFO |: GT index : (7, 4, 6)
2022-04-25 21:05:37,992 | INFO |: Bel index : (6, 4, 6) with prob = 1.0
2022-04-25 21:05:37,994 | INFO |: Bel_bar prob at index = 0.04507718482584398
2022-04-25 21:05:37,994 | INFO |: GT : (0.308, 0.095, 4986.243)
2022-04-25 21:05:37,995 | INFO |: Belief : (0.305, 0.000, -50.000)
2022-04-25 21:05:37,996 | INFO |: POS ERROR : (0.003, 0.095, 5036.243)
2022-04-25 21:05:37,998 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 13 -----------------
2022-04-25 21:05:40,102 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:40,104 | INFO |: GT index : (6, 3, 2)
2022-04-25 21:05:40,105 | INFO |: Prior Bel index : (4, 2, 3) with prob = 0.0626157
2022-04-25 21:05:40,105 | INFO |: POS ERROR : (0.388, 0.366, 5026.354)
2022-04-25 21:05:40,107 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:43,474 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:05:43,490 | INFO |: GT index : (6, 3, 2)
2022-04-25 21:05:43,491 | INFO |: Bel index : (5, 3, 2) with prob = 1.0
2022-04-25 21:05:43,492 | INFO |: Bel_bar prob at index = 0.034304265987684004
2022-04-25 21:05:43,492 | INFO |: GT : (0.083, -0.243, 5276.354)
2022-04-25 21:05:43,494 | INFO |: Belief : (0.000, -0.305, -130.000)
2022-04-25 21:05:43,497 | INFO |: POS ERROR : (0.083, 0.061, 5406.354)
2022-04-25 21:05:43,499 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 14 -----------------
2022-04-25 21:05:46,602 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:46,619 | INFO |: GT index : (5, 2, 1)
2022-04-25 21:05:46,620 | INFO |: Prior Bel index : (2, 1, 17) with prob = 0.0686621
2022-04-25 21:05:46,621 | INFO |: POS ERROR : (0.637, 0.482, 5083.431)
2022-04-25 21:05:46,624 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:50,010 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:05:50,022 | INFO |: GT index : (5, 2, 1)
2022-04-25 21:05:50,022 | INFO |: Bel index : (4, 3, 1) with prob = 1.0
2022-04-25 21:05:50,023 | INFO |: Bel_bar prob at index = 0.001523708046172246
2022-04-25 21:05:50,024 | INFO |: GT : (-0.277, -0.433, 5613.431)
2022-04-25 21:05:50,024 | INFO |: Belief : (-0.305, -0.305, -150.000)
2022-04-25 21:05:50,025 | INFO |: POS ERROR : (0.028, -0.128, 5763.431)
2022-04-25 21:05:50,027 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 15 -----------------
2022-04-25 21:05:53,175 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:53,187 | INFO |: GT index : (3, 2, 0)
2022-04-25 21:05:53,188 | INFO |: Prior Bel index : (3, 4, 15) with prob = 0.1535830
2022-04-25 21:05:53,189 | INFO |: POS ERROR : (-0.066, -0.463, 5460.126)
2022-04-25 21:05:53,190 | INFO |: ---------- PREDICTION STATS -----------
2022-04-25 21:05:56,532 | INFO |: ---------- UPDATE STATS -----------
2022-04-25 21:05:56,547 | INFO |: GT index : (3, 2, 0)
2022-04-25 21:05:56,550 | INFO |: Bel index : (3, 2, 0) with prob = 1.0
2022-04-25 21:05:56,551 | INFO |: Bel_bar prob at index = 0.0026602650853020317
2022-04-25 21:05:56,553 | INFO |: GT : (-0.676, -0.463, 5950.126)
2022-04-25 21:05:56,554 | INFO |: Belief : (-0.610, -0.610, -170.000)
2022-04-25 21:05:56,555 | INFO |: POS ERROR : (-0.066, 0.147, 6120.126)
2022-04-25 21:05:56,556 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------