Important Note

I worked extensively on this lab with Joey Horwitz (jah569) and David Kim (jk2537). Joey's website can viewed here, and David's can be viewed here.

Objectives

Have the robot traverse the room by reaching certain waypoints displayed below.

Accomplish the task by using techniques studied throughout the course

System Capabilities

Robot is able to rotation and traverse somewhat reliably
Robot can determine its orientation via IMU sensor data
Robot can detect obstacles ahead of the robot via ToF sensor
Computer and robot can exchange information via bluetooth
Computer can compute is able to localize the robot's current position by using the update step of Bayes filter algorithm
Robot is unable to compute it absolute position or heading (I am not familiar with the magnetometer in the IMU)
IMU gyroscope data has significant drift
Robot is able to do various computation onboard

Overview

Our system has the computer in full control of sending commands to the robot. Our idea is that all the robot would be doing is gathering information of it environment and sending it to my computer to be analyzed. The computer handles the prediction and update step of Bayes filter, computing the movement control instructions for the robot, obstacle avoidance, and sending PWM values to control each motor separately. This will be explored in more depth in the implementation section. Please see the below flowchart to visualize the behavior of the system.

As you can see, our system uses local path planning to move to the next waypoint, utilizing the turn-go-turn approach. During the robot's movement, it checks to make sure it does not hit any obstacles, and if it will collide, it follows a right hand follow approach until it reaches a point where there is space to move forward again. To control the robot's movement, we used a hard-coding approach with altering PWM values and a constant time delay. We heavily tested and quantified a method of accurately turning a certain amount of degrees and traveling forward a specified distance. We decided to take this approach because it was much easier to implement, but also because the sensors are not too reliable. For instance, the gyroscope has significant drift and running it for a long time, like in this lab, would produce very inaccurate data. Although it is possible to establish an absolute heading using the magnetometer, we did not have a lot of experience with utilizing its data. The localization computation on the computer is very quick, only taking a few seconds, but we elected to only preform the observation loop and localization steps at every waypoint. Based on prior troubles with preforming the observation loop, we elected to only localize sparsely because we do no trust the observation loop data very well.

Implementation

Below shows our implementation of lab 13. It shows the steps we make during each iteration of our loop.

# Run through each motion steps
i = 0
boost = 0
while True:
    print("\n\n----------------WP", i, "----------------")
        
    # Prediction Step
    current_odom = waypoints[i]
    prev_odom = waypoints[0] if i == 0 else waypoints[i - 1]
    loc.prediction_step(current_odom, prev_odom)
    # loc.print_prediction_stats(plot_data=True)
    
    # Get Observation Data by executing a 360 degree rotation motion
    loc.get_observation_data()

    # Run Update Step
    loc.update_step()
    loc.plot_update_step_data(plot_data=True)
    argmax_bel = get_max(loc.bel)
    current_belief = loc.mapper.from_map(*argmax_bel[0])
    print(f"Current belief: {current_belief}")
    
    # Check if we reached a waypoint
    x, y, _ = argmax_bel[0]
    way_x = waypoints[i][0]
    way_y = waypoints[i][1]
    if way_x - 1 <= x <= way_x + 1 and way_y - 1 <= y <= way_y + 1:
        i += 1
        print(f"New goal waypoint {i}")
    
    # Termination condition
    if i == len(waypoints):
        print("We done!")
        break
    
    # Compute the necessary control to get to the next waypoint (assuming no obstacles)
    delta_rot_1, delta_trans, delta_rot_2 = loc.compute_control(waypoints_in_m[i], current_belief)
    print(f"Control values: {delta_rot_1}, {delta_trans}, {delta_rot_2}")
    speed_rot_1, time_rot_1 = rot_delta_to_pwm(delta_rot_1, boost)
    speed_trans, time_trans = trans_delta_to_pwm(delta_trans)
    speed_rot_2, time_rot_2 = rot_delta_to_pwm(delta_rot_2, boost)
    
    # Invoke motor inputs...
    # First rotation:
    if time_rot_1 < 0:
        print(f"Rotating right {speed_rot_1}")
        rc.rotate_right(speed_rot_1)
    else:
        print(f"Rotating left {speed_rot_1}")
        rc.rotate_left(speed_rot_1)
    await asyncio.sleep(abs(time_rot_1))
    rc.active_stop()
    
    await asyncio.sleep(1)
    
    # Try to move forward with the forward controls.
    # If there are obstacles, scrap the rest of the control and try again with new localization data.
    obstacles_avoided = execute_safe_trajectory(speed_trans, time_trans)
    if obstacles_avoided:
        continue
    
    await asyncio.sleep(1)
    
    # Otherwise... continue with the second rotation
    if time_rot_2 < 0:
        print(f"Rotating right {speed_rot_2}")
        rc.rotate_right(speed_rot_2)
    else:
        print(f"Rotating left {speed_rot_2}")
        rc.rotate_left(speed_rot_2)
    await asyncio.sleep(abs(time_rot_2))
    rc.active_stop()
    
    boost += 1

Before we begin our loop, we run an update step of the Bayes filter. Once we enter the loop, we run the full Bayes filter, starting with the prediction step and then the update step, to hopefully get an accurate localization of where we are inside the room. We then check if we are at a waypoint, or we are within a certain range of a waypoint. We do this because we realized it is pretty difficult to actually both reach the exact point, and actually get the localization to work properly. If we are at waypoint we want to reach, we then update the current target waypoint to the next waypoint we want to reach in the sequence. Then, we check if we reached the final waypoint and stop if we have. Then we compute the control information to send to the robot to try to move to the next waypoint. As mentioned before, he we utilize the turn-go-turn approach. We opted to not use PID and instead we use a function that correlates distance to a PWM value that is sent to the robot. This is also done for rotating certain angles. This works surprising well, but error does accumulate. While moving, we have the robot transmit its front ToF sensor readings to check that it does not run into a well. If the robot detects that it will run into a wall, we implemented an right wall follow algorithm to avoid hitting the wall. The robot follows the right wall until the left sensor sees enough space so that it can move forward again.

Results

Below shows our best run, both the videos of the execution and planning steps.

Here is a screenshot of plotter after the final localization.

As you can see from the videos, our implementation did not work successfully, however it did manage to hit the first few waypoints, while avoiding obstacles. We attributed this to various issues. For one, our localization was still very inconsistent, even with using the prediction step; sometimes it would localize almost perfectly and other times, it couldn't be more worse. When it localizes wrong, the robot moves farther away from the targeted point, and this error builds up significantly if localized wrong several times in a row, which can be seen in the video several times. However, the robot can came back from these errors if the localizations is right. Additionally, our our localization predicted our heading or angle wrong which made us turn and travel in the wrong direction. We reach the first point after a few localizations, which can also be seen in the video. Another issue we encountered was moving correctly. It often did not move as well as we would have liked, especially with turning, which could be attributed to decreasing battery. We fixed some of the error slowly adding an additional "boost" to the PWM values to help with overcoming the dying battery. To improve this even more, we could have tried to figure out how to incorporate PID control with our implementation. We all had difficulties in the past with implementing a accurate PID turning controller, so that is why we decided to focus on other things. Overall, I believe if we had better localization and better control over our robot, I think we could have gotten the robot to travel to all the waypoints. We spent a lot of time troubleshooting to optimize the localization, but ultimately, time got the best of us and we could not implement the PID movement, which would have helped significantly. We discussed using hard coding the movements to reach all the waypoints, but we did not see that as a representation of the topics we learned in this class. Even so, we believe that our implementation and thought process was sound, but the hardware was the biggest limitation.

Printed Bayes Filter Output

----------------WP 0 ----------------
2022-05-18 16:54:35,421 | INFO     |: Prediction Step
2022-05-18 16:54:35,504 | INFO     |:  | Prediction Time: 0.079 secs
[[2.793]
 [1.109]
 [1.046]
 [1.065]
 [1.089]
 [0.586]
 [0.439]
 [0.4  ]
 [0.428]
 [0.554]
 [0.451]
 [0.382]
 [0.389]
 [0.494]
 [0.785]
 [1.02 ]
 [3.561]
 [1.892]]
2022-05-18 16:54:46,917 | INFO     |: Update Step
2022-05-18 16:54:46,921 | INFO     |:      | Update Time: 0.003 secs
2022-05-18 16:54:46,922 | INFO     |: Bel index     : (1, 1, 11) with prob = 1.0
2022-05-18 16:54:46,923 | INFO     |: Bel_bar prob at index = 7.315639963206552e-09
2022-05-18 16:54:46,924 | INFO     |: Belief        : (-1.219, -0.914, 50.000)
Current belief: (-1.2191999999999998, -0.9144, 50.0)
Control values: -4.848424217331683, 0.8553193555625874, 134.84842421733168
Rotating right 100
Moving forward: 60 PWM
Rotating left 100
-------------------------------------


----------------WP 0 ----------------
2022-05-18 16:54:51,672 | INFO     |: Prediction Step
2022-05-18 16:54:51,719 | INFO     |:  | Prediction Time: 0.046 secs
[[1.142]
 [1.151]
 [0.847]
 [0.413]
 [0.323]
 [0.28 ]
 [0.291]
 [0.365]
 [0.318]
 [0.259]
 [0.235]
 [0.232]
 [2.523]
 [1.696]
 [2.858]
 [2.283]
 [1.737]
 [1.272]]
2022-05-18 16:55:01,933 | INFO     |: Update Step
2022-05-18 16:55:01,936 | INFO     |:      | Update Time: 0.003 secs
2022-05-18 16:55:01,937 | INFO     |: Bel index     : (3, 6, 10) with prob = 0.9999999
2022-05-18 16:55:01,938 | INFO     |: Bel_bar prob at index = 1.017330396250979e-05
2022-05-18 16:55:01,938 | INFO     |: Belief        : (-0.610, 0.610, 30.000)
Current belief: (-0.6095999999999998, 0.6096000000000003, 30.0)
Control values: -120.39961534756544, 0.9176223188218562, -89.60038465243456
Rotating right 102
Moving forward: 60 PWM


----------------WP 0 ----------------
2022-05-18 16:55:04,006 | INFO     |: Prediction Step
2022-05-18 16:55:04,050 | INFO     |:  | Prediction Time: 0.043 secs
[[3.703]
 [3.013]
 [1.297]
 [1.356]
 [1.492]
 [1.284]
 [1.177]
 [1.169]
 [0.818]
 [0.441]
 [0.35 ]
 [0.317]
 [0.332]
 [0.383]
 [0.304]
 [0.292]
 [0.227]
 [0.225]]
2022-05-18 16:55:13,891 | INFO     |: Update Step
2022-05-18 16:55:13,894 | INFO     |:      | Update Time: 0.003 secs
2022-05-18 16:55:13,895 | INFO     |: Bel index     : (9, 3, 17) with prob = 0.9999999
2022-05-18 16:55:13,896 | INFO     |: Bel_bar prob at index = 1.8522359652750974e-34
2022-05-18 16:55:13,897 | INFO     |: Belief        : (1.219, -0.305, 170.000)
Current belief: (1.2192000000000005, -0.30479999999999985, 170.0)
Control values: 10.099905355615533, 1.8352027898845407, -0.09990535561553315
Rotating left 102
Moving forward: 60 PWM
Rotating right 102
-------------------------------------


----------------WP 0 ----------------
2022-05-18 16:55:18,807 | INFO     |: Prediction Step
2022-05-18 16:55:18,852 | INFO     |:  | Prediction Time: 0.044 secs
[[1.179]
 [0.814]
 [0.688]
 [0.633]
 [0.927]
 [1.835]
 [2.311]
 [1.674]
 [1.252]
 [1.695]
 [2.057]
 [2.707]
 [0.845]
 [0.793]
 [0.787]
 [0.825]
 [2.854]
 [1.851]]
2022-05-18 16:55:29,302 | INFO     |: Update Step
2022-05-18 16:55:29,306 | INFO     |:      | Update Time: 0.004 secs
2022-05-18 16:55:29,308 | INFO     |: Bel index     : (5, 4, 14) with prob = 0.9992326
2022-05-18 16:55:29,309 | INFO     |: Bel_bar prob at index = 6.660099661621505e-06
2022-05-18 16:55:29,319 | INFO     |: Belief        : (0.000, 0.000, 110.000)
Current belief: (1.3877787807814457e-16, 1.3877787807814457e-16, 110.0)
Control values: 96.56505117707798, 0.6887089370699354, -26.565051177077976
Rotating left 104
Moving forward: 60 PWM
Rotating right 104
-------------------------------------


----------------WP 0 ----------------
2022-05-18 16:55:34,017 | INFO     |: Prediction Step
2022-05-18 16:55:34,105 | INFO     |:  | Prediction Time: 0.087 secs
[[1.451]
 [1.279]
 [1.317]
 [1.091]
 [2.384]
 [2.839]
 [2.749]
 [1.324]
 [2.036]
 [2.524]
 [2.052]
 [1.683]
 [1.643]
 [0.665]
 [0.288]
 [0.215]
 [0.435]
 [0.433]]
2022-05-18 16:55:44,168 | INFO     |: Update Step
2022-05-18 16:55:44,172 | INFO     |:      | Update Time: 0.003 secs
2022-05-18 16:55:44,173 | INFO     |: Bel index     : (4, 4, 2) with prob = 0.9694437
2022-05-18 16:55:44,174 | INFO     |: Bel_bar prob at index = 2.4915395892791677e-08
2022-05-18 16:55:44,175 | INFO     |: Belief        : (-0.305, 0.000, -130.000)
Current belief: (-0.30479999999999985, 1.3877787807814457e-16, -130.0)
Control values: -5.29609958366882, 0.4378463657494489, -44.70390041633118
Rotating right 106
Moving forward: 60 PWM
Rotating right 106
-------------------------------------


----------------WP 0 ----------------
2022-05-18 16:55:47,982 | INFO     |: Prediction Step
2022-05-18 16:55:48,070 | INFO     |:  | Prediction Time: 0.087 secs
[[1.21 ]
 [1.338]
 [1.142]
 [1.189]
 [1.25 ]
 [1.087]
 [1.45 ]
 [3.246]
 [3.382]
 [1.645]
 [2.987]
 [2.311]
 [0.335]
 [0.283]
 [0.282]
 [0.301]
 [0.368]
 [0.371]]
2022-05-18 16:55:57,734 | INFO     |: Update Step
2022-05-18 16:55:57,738 | INFO     |:      | Update Time: 0.003 secs
2022-05-18 16:55:57,739 | INFO     |: Bel index     : (3, 4, 0) with prob = 1.0
2022-05-18 16:55:57,741 | INFO     |: Bel_bar prob at index = 0.0013380387715377373
2022-05-18 16:55:57,742 | INFO     |: Belief        : (-0.610, 0.000, -170.000)
Current belief: (-0.6095999999999998, 1.3877787807814457e-16, -170.0)
Control values: 78.80960965539143, 0.30806648633046746, -88.80960965539143
Rotating left 108
Moving forward: 60 PWM
Rotating right 108
-------------------------------------


----------------WP 0 ----------------
2022-05-18 16:56:02,022 | INFO     |: Prediction Step
2022-05-18 16:56:02,066 | INFO     |:  | Prediction Time: 0.043 secs
[[0.972]
 [0.778]
 [1.117]
 [3.016]
 [2.891]
 [1.501]
 [1.956]
 [2.936]
 [2.513]
 [1.879]
 [0.715]
 [0.521]
 [0.617]
 [0.88 ]
 [1.023]
 [0.962]
 [0.975]
 [1.084]]
2022-05-18 16:56:13,319 | INFO     |: Update Step
2022-05-18 16:56:13,324 | INFO     |:      | Update Time: 0.004 secs
2022-05-18 16:56:13,325 | INFO     |: Bel index     : (3, 3, 4) with prob = 0.9999999
2022-05-18 16:56:13,326 | INFO     |: Bel_bar prob at index = 1.9825930747671232e-07
2022-05-18 16:56:13,328 | INFO     |: Belief        : (-0.610, -0.305, -90.000)
Current belief: (-0.6095999999999998, -0.30479999999999985, -90.0)
New goal waypoint 1
Control values: 89.80018989627192, 0.9176055797563567, -179.80018989627192
Rotating left 110
Moving forward: 60 PWM
Rotating right 110
-------------------------------------


----------------WP 1 ----------------
2022-05-18 16:56:18,237 | INFO     |: Prediction Step
2022-05-18 16:56:18,284 | INFO     |:  | Prediction Time: 0.046 secs
[[0.775]
 [1.283]
 [1.866]
 [2.328]
 [2.557]
 [0.902]
 [0.992]
 [2.346]
 [2.178]
 [1.903]
 [1.831]
 [1.879]
 [0.887]
 [0.742]
 [0.655]
 [1.278]
 [1.486]
 [1.581]]
2022-05-18 16:56:29,605 | INFO     |: Update Step
2022-05-18 16:56:29,616 | INFO     |:      | Update Time: 0.003 secs
2022-05-18 16:56:29,618 | INFO     |: Bel index     : (5, 3, 4) with prob = 1.0
2022-05-18 16:56:29,622 | INFO     |: Bel_bar prob at index = 2.2748836240086056e-22
2022-05-18 16:56:29,623 | INFO     |: Belief        : (0.000, -0.305, -90.000)
Current belief: (1.3877787807814457e-16, -0.30479999999999985, -90.0)
Control values: 89.40474059150154, 0.30801662292804893, -179.40474059150154
Rotating left 112
Moving forward: 60 PWM
Rotating right 112
-------------------------------------


----------------WP 1 ----------------
2022-05-18 16:56:34,047 | INFO     |: Prediction Step
2022-05-18 16:56:34,092 | INFO     |:  | Prediction Time: 0.043 secs
[[0.877]
 [1.083]
 [1.44 ]
 [2.084]
 [2.223]
 [0.663]
 [0.627]
 [0.708]
 [2.071]
 [1.828]
 [1.671]
 [1.643]
 [1.787]
 [1.701]
 [1.018]
 [0.902]
 [1.021]
 [1.685]]
2022-05-18 16:56:44,256 | INFO     |: Update Step
2022-05-18 16:56:44,259 | INFO     |:      | Update Time: 0.003 secs
2022-05-18 16:56:44,260 | INFO     |: Bel index     : (7, 2, 5) with prob = 0.9999999
2022-05-18 16:56:44,261 | INFO     |: Bel_bar prob at index = 9.734434165353852e-11
2022-05-18 16:56:44,262 | INFO     |: Belief        : (0.610, -0.610, -70.000)
Current belief: (0.6096, -0.6095999999999999, -70.0)
New goal waypoint 2
Control values: -18.833834758750342, 0.31446513320239494, -151.16616524124964
Rotating right 114
Moving forward: 60 PWM
Rotating right 114
-------------------------------------


----------------WP 2 ----------------
2022-05-18 16:56:48,702 | INFO     |: Prediction Step
2022-05-18 16:56:48,749 | INFO     |:  | Prediction Time: 0.045 secs
[[1.442]
 [1.28 ]
 [1.273]
 [1.35 ]
 [1.413]
 [1.235]
 [0.872]
 [0.715]
 [0.78 ]
 [1.756]
 [2.134]
 [2.313]
 [2.379]
 [2.072]
 [1.307]
 [1.33 ]
 [1.71 ]
 [1.76 ]]
2022-05-18 16:56:58,753 | INFO     |: Update Step
2022-05-18 16:56:58,757 | INFO     |:      | Update Time: 0.003 secs
2022-05-18 16:56:58,758 | INFO     |: Bel index     : (3, 2, 17) with prob = 0.5009916
2022-05-18 16:56:58,759 | INFO     |: Bel_bar prob at index = 1.602676449124761e-11
2022-05-18 16:56:58,760 | INFO     |: Belief        : (-0.610, -0.610, 170.000)
Current belief: (-0.6095999999999998, -0.6095999999999999, 170.0)
Control values: 175.61230699415114, 1.2652836519927062, 134.38769300584886
Rotating left 116
Moving forward: 60 PWM
Rotating left 116
-------------------------------------


----------------WP 2 ----------------
2022-05-18 16:57:04,303 | INFO     |: Prediction Step
2022-05-18 16:57:04,392 | INFO     |:  | Prediction Time: 0.089 secs
[[0.033]
 [0.061]
 [0.09 ]
 [0.17 ]
 [0.367]
 [1.157]
 [1.297]
 [1.293]
 [0.975]
 [0.8  ]
 [0.742]
 [0.748]
 [0.838]
 [1.116]
 [1.187]
 [2.075]
 [1.866]
 [1.687]]
2022-05-18 16:57:14,299 | INFO     |: Update Step
2022-05-18 16:57:14,305 | INFO     |:      | Update Time: 0.004 secs
2022-05-18 16:57:14,306 | INFO     |: Bel index     : (9, 6, 1) with prob = 0.9999999
2022-05-18 16:57:14,307 | INFO     |: Bel_bar prob at index = 7.537725080953532e-15
2022-05-18 16:57:14,314 | INFO     |: Belief        : (1.219, 0.610, -150.000)
Current belief: (1.2192000000000005, 0.6096000000000003, -150.0)
Control values: 38.529149352899154, 1.6479621354873424, -128.52914935289914
Rotating left 118
Moving forward: 60 PWM


----------------WP 2 ----------------
2022-05-18 16:57:16,168 | INFO     |: Prediction Step
2022-05-18 16:57:16,212 | INFO     |:  | Prediction Time: 0.044 secs
[[0.756]
 [0.765]
 [0.844]
 [1.18 ]
 [1.823]
 [1.919]
 [1.745]
 [2.766]
 [3.149]
 [0.067]
 [0.037]
 [0.013]
 [0.017]
 [0.038]
 [0.063]
 [0.106]
 [0.182]
 [0.191]]
2022-05-18 16:57:25,863 | INFO     |: Update Step
2022-05-18 16:57:25,867 | INFO     |:      | Update Time: 0.004 secs
2022-05-18 16:57:25,868 | INFO     |: Bel index     : (1, 4, 1) with prob = 0.5081536
2022-05-18 16:57:25,869 | INFO     |: Bel_bar prob at index = 1.5386720452144e-07
2022-05-18 16:57:25,870 | INFO     |: Belief        : (-1.219, 0.000, -150.000)
Current belief: (-1.2191999999999998, 1.3877787807814457e-16, -150.0)
Control values: 123.27532317423845, 2.0546861171478237, 146.72467682576155
Rotating left 118
Moving forward: 60 PWM
Rotating left 118
-------------------------------------


----------------WP 2 ----------------
2022-05-18 16:57:31,693 | INFO     |: Prediction Step
2022-05-18 16:57:31,787 | INFO     |:  | Prediction Time: 0.093 secs
[[0.598]
 [0.608]
 [0.311]
 [0.195]
 [0.137]
 [0.115]
 [0.103]
 [0.111]
 [0.134]
 [0.201]
 [0.33 ]
 [0.59 ]
 [2.492]
 [2.043]
 [2.535]
 [2.903]
 [1.414]
 [1.427]]
2022-05-18 16:57:41,505 | INFO     |: Update Step
2022-05-18 16:57:41,509 | INFO     |:      | Update Time: 0.002 secs
2022-05-18 16:57:41,510 | INFO     |: Bel index     : (4, 8, 9) with prob = 0.9999999
2022-05-18 16:57:41,510 | INFO     |: Bel_bar prob at index = 4.603371588796173e-21
2022-05-18 16:57:41,512 | INFO     |: Belief        : (-0.305, 1.219, 10.000)
Current belief: (-0.30479999999999985, 1.2192000000000003, 10.0)
Control values: -76.7498056279166, 2.332633464563175, -173.2501943720834
Rotating right 120
Moving forward: 60 PWM
Rotating right 120
-------------------------------------


----------------WP 2 ----------------
2022-05-18 16:57:47,368 | INFO     |: Prediction Step
2022-05-18 16:57:47,412 | INFO     |:  | Prediction Time: 0.043 secs
[[1.482]
 [1.516]
 [1.188]
 [0.625]
 [0.514]
 [0.067]
 [0.039]
 [0.042]
 [0.064]
 [0.105]
 [0.173]
 [2.428]
 [2.372]
 [2.663]
 [2.271]
 [1.28 ]
 [1.303]
 [2.597]]
2022-05-18 16:57:57,667 | INFO     |: Update Step
2022-05-18 16:57:57,671 | INFO     |:      | Update Time: 0.002 secs
2022-05-18 16:57:57,672 | INFO     |: Bel index     : (8, 8, 7) with prob = 0.9999999
2022-05-18 16:57:57,673 | INFO     |: Bel_bar prob at index = 2.029374241875861e-17
2022-05-18 16:57:57,674 | INFO     |: Belief        : (0.914, 1.219, -30.000)
Current belief: (0.9144000000000002, 1.2192000000000003, -30.0)
Control values: -67.9263955367063, 2.1638735637740023, -142.0736044632937
Rotating right 122
Moving forward: 60 PWM
Rotating right 122
-------------------------------------


----------------WP 2 ----------------
2022-05-18 16:58:03,118 | INFO     |: Prediction Step
2022-05-18 16:58:03,163 | INFO     |:  | Prediction Time: 0.044 secs
[[2.529]
 [1.235]
 [0.958]
 [0.885]
 [0.644]
 [0.216]
 [0.126]
 [0.093]
 [0.079]
 [0.077]
 [0.087]
 [0.111]
 [0.158]
 [0.238]
 [1.921]
 [2.179]
 [1.824]
 [1.76 ]]
2022-05-18 16:58:12,857 | INFO     |: Update Step
2022-05-18 16:58:12,861 | INFO     |:      | Update Time: 0.003 secs
2022-05-18 16:58:12,862 | INFO     |: Bel index     : (6, 0, 13) with prob = 0.9999991
2022-05-18 16:58:12,863 | INFO     |: Bel_bar prob at index = 3.157698662837785e-15
2022-05-18 16:58:12,864 | INFO     |: Belief        : (0.305, -1.219, 90.000)
Current belief: (0.3048000000000003, -1.2191999999999998, 90.0)
Control values: -46.51141131937869, 0.42893878351112036, 76.51141131937868
Rotating right 124
Moving forward: 60 PWM


----------------WP 2 ----------------
2022-05-18 16:58:14,818 | INFO     |: Prediction Step
2022-05-18 16:58:14,862 | INFO     |:  | Prediction Time: 0.044 secs
[[2.291]
 [2.263]
 [3.355]
 [1.416]
 [1.305]
 [3.085]
 [2.222]
 [1.297]
 [0.989]
 [0.898]
 [0.823]
 [0.301]
 [0.178]
 [0.122]
 [0.102]
 [0.098]
 [0.103]
 [0.101]]
2022-05-18 16:58:24,544 | INFO     |: Update Step
2022-05-18 16:58:24,574 | INFO     |:      | Update Time: 0.017 secs
2022-05-18 16:58:24,578 | INFO     |: Bel index     : (10, 8, 17) with prob = 0.9999999
2022-05-18 16:58:24,580 | INFO     |: Bel_bar prob at index = 0.0005848466707414098
2022-05-18 16:58:24,582 | INFO     |: Belief        : (1.524, 1.219, 170.000)
Current belief: (1.5240000000000002, 1.2192000000000003, 170.0)
Control values: 77.03930015451704, 2.3276104141372116, -127.03930015451704
Rotating left 124
Moving forward: 60 PWM
Rotating right 124
-------------------------------------