Lab 10: Localization Simulation
Implementation of Simulator Functions
Compute Control
The function compute_control() needs to calculate the required control to get from prev_pose to cur_pose. This assumes the odometry model where we rotate by delta_rot_1 so that we can move to the next point in a straight line by delta_trans, then correct orientation via delta_rot_2.
The equations of motion are therefore as follows, as derived in the lecture slides:
This makes the implementation fairly straightforward, keeping in mind to use numpy broadcasting for operations:
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)
"""
# Define here because used twice
dx = cur_pose[0] - prev_pose[0]
dy = cur_pose[1] - prev_pose[1]
# Note that pose angle is in radians, so normalize_angle() doubly converts to angle and converts to (-180, 180)
delta_rot_1 = np.arctan2(dy, dx) - prev_pose[2]
delta_rot_1 = mapper.normalize_angle(delta_rot_1)
delta_trans = np.sqrt(dy**2 + dx**2)
delta_rot_2 = cur_pose[2] - prev_pose[2] - delta_rot_1
delta_rot_2 = mapper.normalize_angle(delta_rot_2)
return delta_rot_1, delta_trans, delta_rot_2
Odometry Motion Model
To calculate the odometry motion model, we are finding the Gaussian probability that the u_theory needed to get from prev_pose to actual_pose was executed. To compute this, we multiply the three independent probabilities, using loc.gaussian, of u_theory given the actual control input u and the noises for rotation and translation in odometry. The code is given as follows:
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)
"""
# Theoretical control
da1_th, dtrans_th, da2_th = compute_control(cur_pose, prev_pose)
# Make sure the u angles are normalized. Likely unnecessary bc. writeup says u is also an output of compute_control.
da1 = mapper.normalize_angle(u[0])
da2 = mapper.normalize_angle(u[2])
# Calculate gaussian(theory | actual, noise)
prob_da1 = loc.gaussian(da1_th, da1, loc.odom_rot_sigma)
prob_trans = loc.gaussian(dtrans_th, u[1], loc.odom_trans_sigma)
prob_da2 = loc.gaussian(da2_th, da2, loc.odom_rot_sigma)
prob = prob_da1 * prob_trans * prob_da2
return prob
Prediction Step
In the prediction step of the Bayes filter, we need to fill in each xt cell’s bel_bar term with the probability that each other cell x will end up in xt multiplied by the belief in cell x. We are pouring all of our beans from the bel row to the bel_bar row, except we have 12 * 9 * 18 = 1944 cups and therefore 1944*1944 ~= 3.9 million iterations to consider. To save computation, we ignore the cells where the belief is less than 0.001. The code is given as follows:
from itertools import product
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
"""
# For every combination of x, y, a
for x, y, a in product(
range(mapper.MAX_CELLS_X),
range(mapper.MAX_CELLS_Y),
range(mapper.MAX_CELLS_A),
):
# Check if belief is greater than 0.001
if loc.bel[x, y, a] <= 0.001:
continue
# Otherwise, proceed with inner loop
for xt, yt, at in product(
range(mapper.MAX_CELLS_X),
range(mapper.MAX_CELLS_Y),
range(mapper.MAX_CELLS_A),
):
# Discrete -> Continuous
prev_pose = mapper.from_map(x, y, a)
cur_pose = mapper.from_map(xt, yt, at)
# Compute the probability in odometry
u = compute_control(cur_odom, prev_odom)
prob = odom_motion_model(cur_pose, prev_pose, u)
# Fill up the second row of beans
# Note: omit normalization here because it can be done in the update step
loc.bel_bar[xt, yt, at] += (prob * loc.bel[x, y, a])
Sensor Model
The sensor model simply computes the likelihood of making the actual sensor measurement given the position. Since we can precompute the true sensor measurements, this can be calculated as a Gaussian likelihood where the true measurement and noise are the mean and variance.
def sensor_model(obs):
""" 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
Returns:
[ndarray]: Returns a 1D array of size 18 (=loc.OBS_PER_CELL) with the likelihoods of each individual sensor measurement
"""
prob_array = np.zeros(mapper.OBS_PER_CELL)
for i in range(mapper.OBS_PER_CELL):
prob_array[i] = loc.gaussian(loc.obs_range_data[i], obs[i], loc.sensor_sigma)
return prob_array
Update Step
Finally, the update step goes through the bel_bar of every cell, and updates the bel with the sensor model. After this, we do need to normalize because the values usually become much smaller (especially since we multply a lot of probabilities together in our sensor model at each point).
from itertools import product
def update_step():
""" Update step of the Bayes Filter.
Update the probabilities in loc.bel based on loc.bel_bar and the sensor model.
"""
# For every combination of x, y, a
for x, y, a in product(
range(mapper.MAX_CELLS_X),
range(mapper.MAX_CELLS_Y),
range(mapper.MAX_CELLS_A),
):
# Create sensor model by multiplying all independent probabilities together
p_sensor = np.prod(sensor_model(mapper.get_views(x, y, a)))
loc.bel[x, y, a] = p_sensor * loc.bel_bar[x, y, a]
# Normalize at the end
loc.bel /= np.sum(loc.bel)
Results
To show that the simulation works, I ran the pre-planned trajectory all of the way through with the Bayes filter active. The blue dots represent the most likely position according to the Bayes filter, the green dots represent the ground truth of the robot’s position, and the red dots are the odometry measurements.
We can see that the odometry-based notion of position quickly veers off from the ground truth because the simulation incorporates noise in both our sensor and motion models. But with the Bayes filter active, we can see that the estimated position mostly follows the ground truth position of the robot. I also saw that the odometry line varied between attempts, while the Bayes filtere stayed close to the ground truth regardless of how many times I observed the simulation. Therefore I am fairly confident that my implementation of Bayes Filter is accurate.
The video of the simulation (sorry for the alt-tabbing, I have a Windows computer):
Acknowledgements
- Steven helped me with the implementation on some of the functions.
- I referred to Stephan for a reference for some of the functions as well.
Appendix: Output of Bayes Filter From Video
----------------- 0 -----------------
2025-04-21 22:15:49,841 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:15:49,848 | INFO |: GT index : (6, 3, 7)
2025-04-21 22:15:49,851 | INFO |: Prior Bel index : (np.int64(4), np.int64(5), np.int64(6)) with prob = 0.7363113
2025-04-21 22:15:49,855 | INFO |: POS ERROR : (0.587, -0.391, 11.325)
2025-04-21 22:15:49,859 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:15:53,803 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:15:53,817 | INFO |: GT index : (6, 3, 7)
2025-04-21 22:15:53,823 | INFO |: Bel index : (np.int64(6), np.int64(4), np.int64(6)) with prob = 1.0
2025-04-21 22:15:53,826 | INFO |: Bel_bar prob at index = 0.7070517358476904
2025-04-21 22:15:53,827 | INFO |: GT : (0.282, -0.086, 321.325)
2025-04-21 22:15:53,830 | INFO |: Belief : (0.305, 0.000, -50.000)
2025-04-21 22:15:53,834 | INFO |: POS ERROR : (-0.023, -0.086, 371.325)
2025-04-21 22:15:53,838 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 1 -----------------
2025-04-21 22:15:56,104 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:15:56,113 | INFO |: GT index : (7, 2, 5)
2025-04-21 22:15:56,116 | INFO |: Prior Bel index : (np.int64(4), np.int64(5), np.int64(6)) with prob = 1.4659808
2025-04-21 22:15:56,123 | INFO |: POS ERROR : (0.824, -0.821, 348.789)
2025-04-21 22:15:56,125 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:15:59,740 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:15:59,750 | INFO |: GT index : (7, 2, 5)
2025-04-21 22:15:59,755 | INFO |: Bel index : (np.int64(7), np.int64(2), np.int64(6)) with prob = 1.0
2025-04-21 22:15:59,759 | INFO |: Bel_bar prob at index = 1.307148250247701
2025-04-21 22:15:59,760 | INFO |: GT : (0.519, -0.517, 658.789)
2025-04-21 22:15:59,764 | INFO |: Belief : (0.610, -0.610, -50.000)
2025-04-21 22:15:59,766 | INFO |: POS ERROR : (-0.091, 0.093, 708.789)
2025-04-21 22:15:59,772 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 2 -----------------
2025-04-21 22:16:01,014 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:01,028 | INFO |: GT index : (7, 2, 4)
2025-04-21 22:16:01,031 | INFO |: Prior Bel index : (np.int64(4), np.int64(5), np.int64(6)) with prob = 1.4664396
2025-04-21 22:16:01,032 | INFO |: POS ERROR : (0.824, -0.821, 685.869)
2025-04-21 22:16:01,034 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:04,733 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:16:04,754 | INFO |: GT index : (7, 2, 4)
2025-04-21 22:16:04,756 | INFO |: Bel index : (np.int64(6), np.int64(2), np.int64(4)) with prob = 1.0
2025-04-21 22:16:04,758 | INFO |: Bel_bar prob at index = 0.8816000785719103
2025-04-21 22:16:04,760 | INFO |: GT : (0.519, -0.517, 995.869)
2025-04-21 22:16:04,762 | INFO |: Belief : (0.305, -0.610, -90.000)
2025-04-21 22:16:04,765 | INFO |: POS ERROR : (0.214, 0.093, 1085.869)
2025-04-21 22:16:04,767 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 3 -----------------
2025-04-21 22:16:05,987 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:06,003 | INFO |: GT index : (7, 0, 4)
2025-04-21 22:16:06,005 | INFO |: Prior Bel index : (np.int64(4), np.int64(5), np.int64(6)) with prob = 1.4672336
2025-04-21 22:16:06,007 | INFO |: POS ERROR : (0.865, -1.219, 1045.869)
2025-04-21 22:16:06,011 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:09,665 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:16:09,672 | INFO |: GT index : (7, 0, 4)
2025-04-21 22:16:09,674 | INFO |: Bel index : (np.int64(7), np.int64(1), np.int64(4)) with prob = 1.0
2025-04-21 22:16:09,675 | INFO |: Bel_bar prob at index = 0.9900710820999528
2025-04-21 22:16:09,677 | INFO |: GT : (0.560, -0.915, 1355.869)
2025-04-21 22:16:09,681 | INFO |: Belief : (0.610, -0.914, -90.000)
2025-04-21 22:16:09,683 | INFO |: POS ERROR : (-0.050, -0.000, 1445.869)
2025-04-21 22:16:09,686 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 4 -----------------
2025-04-21 22:16:12,930 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:12,944 | INFO |: GT index : (8, 0, 9)
2025-04-21 22:16:12,947 | INFO |: Prior Bel index : (np.int64(6), np.int64(2), np.int64(6)) with prob = 1.5221932
2025-04-21 22:16:12,948 | INFO |: POS ERROR : (0.518, -0.438, 1491.334)
2025-04-21 22:16:12,952 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:16,642 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:16:16,659 | INFO |: GT index : (8, 0, 9)
2025-04-21 22:16:16,661 | INFO |: Bel index : (np.int64(8), np.int64(1), np.int64(9)) with prob = 1.0
2025-04-21 22:16:16,664 | INFO |: Bel_bar prob at index = 0.054388593185183465
2025-04-21 22:16:16,666 | INFO |: GT : (0.823, -1.048, 1801.334)
2025-04-21 22:16:16,670 | INFO |: Belief : (0.914, -0.914, 10.000)
2025-04-21 22:16:16,674 | INFO |: POS ERROR : (-0.091, -0.133, 1791.334)
2025-04-21 22:16:16,679 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 5 -----------------
2025-04-21 22:16:22,913 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:22,929 | INFO |: GT index : (11, 1, 11)
2025-04-21 22:16:22,931 | INFO |: Prior Bel index : (np.int64(6), np.int64(2), np.int64(6)) with prob = 1.5227026
2025-04-21 22:16:22,932 | INFO |: POS ERROR : (1.299, -0.262, 1900.521)
2025-04-21 22:16:22,935 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:26,557 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:16:26,568 | INFO |: GT index : (11, 1, 11)
2025-04-21 22:16:26,571 | INFO |: Bel index : (np.int64(10), np.int64(1), np.int64(11)) with prob = 1.0
2025-04-21 22:16:26,574 | INFO |: Bel_bar prob at index = 0.0005236617996613996
2025-04-21 22:16:26,576 | INFO |: GT : (1.604, -0.872, 2210.521)
2025-04-21 22:16:26,578 | INFO |: Belief : (1.524, -0.914, 50.000)
2025-04-21 22:16:26,581 | INFO |: POS ERROR : (0.080, 0.043, 2160.521)
2025-04-21 22:16:26,584 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 6 -----------------
2025-04-21 22:16:28,824 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:28,833 | INFO |: GT index : (11, 2, 12)
2025-04-21 22:16:28,835 | INFO |: Prior Bel index : (np.int64(6), np.int64(2), np.int64(6)) with prob = 1.5227026
2025-04-21 22:16:28,836 | INFO |: POS ERROR : (1.374, 0.131, 2289.175)
2025-04-21 22:16:28,839 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:32,703 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:16:32,713 | INFO |: GT index : (11, 2, 12)
2025-04-21 22:16:32,715 | INFO |: Bel index : (np.int64(11), np.int64(3), np.int64(13)) with prob = 1.0
2025-04-21 22:16:32,717 | INFO |: Bel_bar prob at index = 0.0005145797165740494
2025-04-21 22:16:32,718 | INFO |: GT : (1.679, -0.479, 2599.175)
2025-04-21 22:16:32,720 | INFO |: Belief : (1.829, -0.305, 90.000)
2025-04-21 22:16:32,721 | INFO |: POS ERROR : (-0.150, -0.174, 2509.175)
2025-04-21 22:16:32,723 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 7 -----------------
2025-04-21 22:16:34,933 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:34,940 | INFO |: GT index : (11, 3, 13)
2025-04-21 22:16:34,941 | INFO |: Prior Bel index : (np.int64(6), np.int64(2), np.int64(6)) with prob = 1.5227026
2025-04-21 22:16:34,943 | INFO |: POS ERROR : (1.442, 0.484, 2654.810)
2025-04-21 22:16:34,947 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:38,665 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:16:38,675 | INFO |: GT index : (11, 3, 13)
2025-04-21 22:16:38,677 | INFO |: Bel index : (np.int64(11), np.int64(3), np.int64(13)) with prob = 1.0
2025-04-21 22:16:38,680 | INFO |: Bel_bar prob at index = 0.000514865400943735
2025-04-21 22:16:38,682 | INFO |: GT : (1.746, -0.125, 2964.810)
2025-04-21 22:16:38,686 | INFO |: Belief : (1.829, -0.305, 90.000)
2025-04-21 22:16:38,689 | INFO |: POS ERROR : (-0.082, 0.179, 2874.810)
2025-04-21 22:16:38,692 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 8 -----------------
2025-04-21 22:16:41,928 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:41,933 | INFO |: GT index : (11, 5, 14)
2025-04-21 22:16:41,935 | INFO |: Prior Bel index : (np.int64(6), np.int64(2), np.int64(6)) with prob = 1.5227026
2025-04-21 22:16:41,937 | INFO |: POS ERROR : (1.437, 0.976, 3037.446)
2025-04-21 22:16:41,940 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:45,552 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:16:45,569 | INFO |: GT index : (11, 5, 14)
2025-04-21 22:16:45,571 | INFO |: Bel index : (np.int64(11), np.int64(4), np.int64(13)) with prob = 0.9976575
2025-04-21 22:16:45,573 | INFO |: Bel_bar prob at index = 0.0005151571124695942
2025-04-21 22:16:45,576 | INFO |: GT : (1.742, 0.366, 3347.446)
2025-04-21 22:16:45,580 | INFO |: Belief : (1.829, 0.000, 90.000)
2025-04-21 22:16:45,583 | INFO |: POS ERROR : (-0.087, 0.366, 3257.446)
2025-04-21 22:16:45,588 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 9 -----------------
2025-04-21 22:16:49,013 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:49,029 | INFO |: GT index : (11, 6, 16)
2025-04-21 22:16:49,032 | INFO |: Prior Bel index : (np.int64(6), np.int64(2), np.int64(6)) with prob = 1.5227026
2025-04-21 22:16:49,038 | INFO |: POS ERROR : (1.435, 1.300, 3436.594)
2025-04-21 22:16:49,041 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:52,863 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:16:52,876 | INFO |: GT index : (11, 6, 16)
2025-04-21 22:16:52,877 | INFO |: Bel index : (np.int64(11), np.int64(7), np.int64(16)) with prob = 1.0
2025-04-21 22:16:52,879 | INFO |: Bel_bar prob at index = 0.0005144032921941865
2025-04-21 22:16:52,881 | INFO |: GT : (1.740, 0.691, 3746.594)
2025-04-21 22:16:52,882 | INFO |: Belief : (1.829, 0.914, 150.000)
2025-04-21 22:16:52,885 | INFO |: POS ERROR : (-0.089, -0.224, 3596.594)
2025-04-21 22:16:52,887 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 10 -----------------
2025-04-21 22:16:55,167 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:55,171 | INFO |: GT index : (10, 7, 16)
2025-04-21 22:16:55,175 | INFO |: Prior Bel index : (np.int64(6), np.int64(2), np.int64(6)) with prob = 1.5227026
2025-04-21 22:16:55,179 | INFO |: POS ERROR : (1.018, 1.576, 3807.865)
2025-04-21 22:16:55,183 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:16:59,000 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:16:59,005 | INFO |: GT index : (10, 7, 16)
2025-04-21 22:16:59,007 | INFO |: Bel index : (np.int64(10), np.int64(7), np.int64(16)) with prob = 1.0
2025-04-21 22:16:59,009 | INFO |: Bel_bar prob at index = 0.0005144032921949541
2025-04-21 22:16:59,010 | INFO |: GT : (1.323, 0.966, 4117.865)
2025-04-21 22:16:59,012 | INFO |: Belief : (1.524, 0.914, 150.000)
2025-04-21 22:16:59,015 | INFO |: POS ERROR : (-0.201, 0.052, 3967.865)
2025-04-21 22:16:59,018 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 11 -----------------
2025-04-21 22:17:02,297 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:17:02,307 | INFO |: GT index : (7, 6, 3)
2025-04-21 22:17:02,311 | INFO |: Prior Bel index : (np.int64(6), np.int64(2), np.int64(6)) with prob = 1.5227026
2025-04-21 22:17:02,316 | INFO |: POS ERROR : (0.139, 1.475, 4265.249)
2025-04-21 22:17:02,325 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:17:06,036 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:17:06,055 | INFO |: GT index : (7, 6, 3)
2025-04-21 22:17:06,057 | INFO |: Bel index : (np.int64(7), np.int64(7), np.int64(3)) with prob = 1.0
2025-04-21 22:17:06,059 | INFO |: Bel_bar prob at index = 0.06214504395066294
2025-04-21 22:17:06,060 | INFO |: GT : (0.444, 0.866, 4575.249)
2025-04-21 22:17:06,061 | INFO |: Belief : (0.610, 0.914, -110.000)
2025-04-21 22:17:06,064 | INFO |: POS ERROR : (-0.166, -0.049, 4685.249)
2025-04-21 22:17:06,066 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 12 -----------------
2025-04-21 22:17:08,302 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:17:08,316 | INFO |: GT index : (6, 4, 6)
2025-04-21 22:17:08,319 | INFO |: Prior Bel index : (np.int64(6), np.int64(2), np.int64(6)) with prob = 1.5227026
2025-04-21 22:17:08,321 | INFO |: POS ERROR : (-0.027, 0.847, 4670.332)
2025-04-21 22:17:08,324 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:17:11,954 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:17:11,971 | INFO |: GT index : (6, 4, 6)
2025-04-21 22:17:11,973 | INFO |: Bel index : (np.int64(6), np.int64(5), np.int64(5)) with prob = 0.9999999
2025-04-21 22:17:11,974 | INFO |: Bel_bar prob at index = 0.7781540205739128
2025-04-21 22:17:11,976 | INFO |: GT : (0.278, 0.237, 4980.332)
2025-04-21 22:17:11,977 | INFO |: Belief : (0.305, 0.305, -70.000)
2025-04-21 22:17:11,979 | INFO |: POS ERROR : (-0.027, -0.068, 5050.332)
2025-04-21 22:17:11,981 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 13 -----------------
2025-04-21 22:17:14,238 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:17:14,252 | INFO |: GT index : (6, 3, 2)
2025-04-21 22:17:14,255 | INFO |: Prior Bel index : (np.int64(6), np.int64(2), np.int64(6)) with prob = 1.5227026
2025-04-21 22:17:14,258 | INFO |: POS ERROR : (-0.275, 0.533, 4961.588)
2025-04-21 22:17:14,260 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:17:17,960 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:17:17,973 | INFO |: GT index : (6, 3, 2)
2025-04-21 22:17:17,975 | INFO |: Bel index : (np.int64(5), np.int64(3), np.int64(2)) with prob = 1.0
2025-04-21 22:17:17,977 | INFO |: Bel_bar prob at index = 0.205696847652052
2025-04-21 22:17:17,979 | INFO |: GT : (0.030, -0.076, 5271.589)
2025-04-21 22:17:17,980 | INFO |: Belief : (0.000, -0.305, -130.000)
2025-04-21 22:17:17,981 | INFO |: POS ERROR : (0.030, 0.229, 5401.589)
2025-04-21 22:17:17,984 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 14 -----------------
2025-04-21 22:17:21,219 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:17:21,231 | INFO |: GT index : (4, 3, 1)
2025-04-21 22:17:21,233 | INFO |: Prior Bel index : (np.int64(6), np.int64(2), np.int64(6)) with prob = 1.5227026
2025-04-21 22:17:21,235 | INFO |: POS ERROR : (-0.644, 0.377, 5298.570)
2025-04-21 22:17:21,238 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:17:24,938 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:17:24,956 | INFO |: GT index : (4, 3, 1)
2025-04-21 22:17:24,959 | INFO |: Bel index : (np.int64(4), np.int64(3), np.int64(1)) with prob = 1.0
2025-04-21 22:17:24,960 | INFO |: Bel_bar prob at index = 0.0050862492083709465
2025-04-21 22:17:24,961 | INFO |: GT : (-0.339, -0.232, 5608.570)
2025-04-21 22:17:24,963 | INFO |: Belief : (-0.305, -0.305, -150.000)
2025-04-21 22:17:24,966 | INFO |: POS ERROR : (-0.034, 0.073, 5758.570)
2025-04-21 22:17:24,968 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------
----------------- 15 -----------------
2025-04-21 22:17:28,174 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:17:28,178 | INFO |: GT index : (3, 3, 0)
2025-04-21 22:17:28,181 | INFO |: Prior Bel index : (np.int64(6), np.int64(2), np.int64(6)) with prob = 1.5227026
2025-04-21 22:17:28,184 | INFO |: POS ERROR : (-1.037, 0.378, 5635.552)
2025-04-21 22:17:28,188 | INFO |: ---------- PREDICTION STATS -----------
2025-04-21 22:17:31,891 | INFO |: ---------- UPDATE STATS -----------
2025-04-21 22:17:31,909 | INFO |: GT index : (3, 3, 0)
2025-04-21 22:17:31,911 | INFO |: Bel index : (np.int64(2), np.int64(3), np.int64(0)) with prob = 0.5444737
2025-04-21 22:17:31,913 | INFO |: Bel_bar prob at index = 0.0005190324947594956
2025-04-21 22:17:31,914 | INFO |: GT : (-0.732, -0.232, 5945.552)
2025-04-21 22:17:31,917 | INFO |: Belief : (-0.914, -0.305, -170.000)
2025-04-21 22:17:31,919 | INFO |: POS ERROR : (0.182, 0.073, 6115.552)
2025-04-21 22:17:31,922 | INFO |: ---------- UPDATE STATS -----------
-------------------------------------