Eli Zhang

Part 1

In order to set up the Kalman filter, the first step is determining its A and B matrices. These matrices are dependent on the values of the d and m variables, which are proportional to the steady-state speed and 90% rise time, respectively.

In order to determine these parameters, I executed a 1 second step response with maximum power over a long distance and plotted the data.

From my step response data, I calculated the steady state speed from around 0.85 seconds to 1 second, when the speed graph seemed to plateau. The slope at this point was around 1112 mm/s.

Next, based on the steady state equation and equations from Lecture 12 (pictured below), I found d to be around 0.0009. Then I found the 90% rise time by looking at when the speed was 90% ramped up, which seemed to take around 0.8 seconds. From this, I found the value for m to be around 0.0003125.

With these d and m values, I could make my A and B matrices. Afterwards, I discretized my A and B matrices according to my average sample time.

Part 2

Next, I needed to estimate the noise parameters to use in my Kalman filter. I receive an updated measurement for position every 35 - 40 milliseconds (0.035s to 0.04s). Based on my lab 3 data, measurements up to 1 meter away with the ToF sensor still only had a standard deviation of around 1 mm. The resulting sigma value for position noise I got was 5.345224838. For speed, since my speed measurement was directly dependent on position, I also got 5.345224838.

Part 3

Using the values I calculated above, I was able to plug in my noise approximations into the provided lab 7 functions.

Then, I was able to run the Kalman filter function on data I measured before!

I ended up not having to adjust my covariance matrices because the Kalman filter graph looked good. However, if I were to increase sigma_1 and sigma_2 (affecting sigma_u) or decrease sigma_4 (affecting sigma_z), it would have made the Kalman filter prediction more reliant on the sensor readings than the Kalman model, which would make the Kalman filter graph closer to the original distance graph.

As you can see in the graph above, the Kalman filter predicts a little ahead of the ToF reading for the car, which is exactly as intended. Using this data, I should be able to predict more accurately where the robot actually is instead of lagging behind with only the ToF readings.

Part 4

The final part of this lab was implementing the Kalman filter on the Arduino itself. This just involved translating the Python code to use the BasicLinearAlgebra library to work with matrices.

All that's left was setting the initial conditions. Since before I set the initial condition for mu to be based on the first reading from the ToF sensor, this time I set it to a pretty arbitrary starting distance, since it should quickly correct itself.

Part 5

Finally, I tested the Kalman filter on the Arduino and logged the results. Below you scan see a snippet of what was logged:

After negating the distance readings and graphing the results, it was pretty clear that the Kalman filter was behaving as expected. As you can see, the Kalman filter predicts that the robot is actually closer than the readings suggest, which makes sense if the distance readings aren't updating instantaneously.

Part 1a

The first part of this lab was executing a controlled stunt. Initially, I chose Controlled Stunt A. In this stunt, the robot drives towards a wall, and when it is half a meter away from it, it flips and turns around.

In order to set up my robot for the stunt, I added quite a few new commands:

Before this lab, I was mainly using coasting to slow down. I added active braking to test whether stopping suddenly would cause the car to flip. The BRAKE_THEN_FORWARD and BRAKE_THEN_BACKWARD commands allowed me to immediately move after actively braking briefly, which I also wanted to test to see if the car would flip earlier. Since there was a bit of latency introduced when sending a bluetooth command, I added this command so the car could immediately execute the next command after the first.

The most important command that I added was MOVE_TOWARDS_WALL_AND_FLIP, which gets the estimated distance using the Kalman filter, accelerates at maximum speed until it reaches the target distance, and then accelerates in the opposite direction at maximum power. Initially, I used PID for the forward movement to reach the target, but since I wasn't getting enough speed and I wanted to accelerate as fast as possible anyways, I got rid of it and opted to just use maximum PWM values.

Unfortunately, for the first controlled stunt, I needed the robot to build up enough speed before it reached the sticky pad in order to flip. No matter what I tried, I was unable to get the robot to flip even at the maximum speed. I even tried moving all my batteries to the top of my robot to change where the center of mass was; I thought if I had a higher center of mass, the car might be more unstable and prone to flipping.

Part 1b

Because I couldn't get the first stunt to work, I decided to work on Controlled Stunt B. The goal of this stunt is to drive forward starting from ~4 meters away, execute a rapid drift for 180 degrees, and then return back to the starting line as fast as possible. For this stunt, I combined the Kalman filter model with PID control and IMU readings from earlier labs. First, I wrote a new command, MOVE_TOWARDS_WALL_AND_TURN.

The first part of my code moves forward at a specified max power until it exceeds a custom set point that can be set through a bluetooth command parameter. When the Kalman filter predicted distance to the wall is below a certain threshold (with a small margin of error), I switch to the next stage.

The next portion is very similar to the work I did in lab 9: I reset the set point to 180 degrees, then apply the active brake (to stop moving forward and start the drift). In reality, I included the distance updates from the ToF only for debugging the logs later on. The important part of this step comes later, where I use PID updates that are based on yaw calculations using the IMU.

As shown above, in this step I constantly use PID to calculate the motor speed based on the change in yaw from the initial starting point. I also update the IMU constantly to get new gyroscope readings. Finally, when I reach the target angle (with a small margin for error), I stop and go to the next stage.

The final stage is simply driving forward as fast as possible, since the car has now turned 180 degrees.

Overall, this controlled stunt went much, much better for me. I was able to reuse parts of the first attempt as well as parts of lab 7 and 9 which I had already completed. The Kalman filter did actually seem to help make the car act properly before it was too late. When I looked at the data, it did seem like the PID was indeed updating properly.

When I begin the spin, I set the Kalman filter distance logs to 0, since the predicted distance is pretty arbitrary after the drift begins. You can see in the data how the PID control modifies the power of the turn while it is drifting to adjust for the angle. Then, when it reaches the correct angle (now KF dist is -1 as an indicator), I set power to the maximum again and go straight. So everything seems to be working well!

Part 2

At this point in the lab, everything went wrong. For some reason, I stopped being able to program my Artemis board and my ToF sensors and bluetooth stopped working. I kept getting data underflow errors for some reason too. I did everything I could to try and debug the situation, like commenting out my code and changing the allocated memory for all my logging. However, since the Artemis bootloader didn't work on my computer these fixes didn't work.

Luckily, for some reason I was still able to program the board on Aryaa's computer. We worked together for the open loop stunts, but since my board's bluetooth wasn't working, we had to program them manually.

We did two simple stunts. The first stunt was spinning in a figure 8, which we achieved by varying the ratio of the left wheels to the right wheels and swapping the ratio when turning in the opposite direction. The second was spinning in place, which is useful for lab 9! I'm still interested in doing cooler stunts (with ramps and stuff), but I might have to come back to it when I figure out how to actually program my microcontroller again.

Part 1

This lab involves calculating the yaw values using the IMU. At first, this seemed pretty easy, since we did most of the setup in lab 3. However, yaw values were tricky to work with since the yaw axis is perpendicular to the direction of gravity. Because of this, I couldn't use the accelerometer values from the IMU.

As a result, I opted to use the gyroscope, which detects only changes in yaw. As mentioned in earlier labs, gyroscope values are prone to drift over time, since they only track differences and sum them over time. For pitch and roll, these were easy to counteract using a complementary filter with the accelerometer. However, since the accelerometer values weren't usable, I tried using the magnetometer. Overall, the magnetometer readings were often garbage values, maybe because of the proximity to the motors (which generate magnetic fields) or just because the sensor wasn't working consistently.

I decided to use only the gyroscope yaw values instead of the complementary filter, since the drift was quite minor (only a degree every few seconds) and I was rotating the robot pretty qucikly relative to the drift rate.

Part 2

To get the robot to spin in place, I had to make a new bluetooth command, since in earlier labs I had only driven it straight towards a wall. My new command, called SPIN_AND_MEASURE, also used PID, which meant I had to adjust the set point for my PID function (I set it to 30 degrees).

For my SPIN_AND_MEASURE command, I also added maxPower, totalAngle, and duration parameters that you can pass in. The maxPower allowed me to adjust the speed of the rotation, which I had to do depending on how charged the battery was. The totalAngle let me change the rotation from 360 to 720 to see whether the rotated amount and readings are consistent. The duration determined how long to pause after every angle increment; in my case, every 30 degrees, I stopped for a short time.

The rest of my code was pretty similar to the PID code I wrote earlier. However, this time I had an additional outer while loop that compared the cumulative angle with the total target angle. The cumulative angle increased in small increments; I used PID control to move towards these incremental set points.

For the rest of the code execution, I continuously append power, yaw, and raw data to my data arrays and update my IMU. I also check to see if I arrived at the target angle; if so, I brake and wait the specified duration.

Since I got my robot to move slowly, I was able to make continuous measurements, which often got me over 300+ measurements.

Part 3

I measured the entire space two times; you can find my data here.

For each measurement, I placed the robot with its ToF sensor pointed down and had it rotate in a full 360 degree circle.

When looking at the distance versus time graph alone, it's hard to understand the surrounding space since the car was not moving continuously.

The raw distance versus yaw graph is a bit better, but the raw yaw values are slightly offset due to drift inaccuracies accumulated over time.

Here are all the individual distance versus raw yaw values.

We know all yaw values lie from 0 to 360, so I map the yaw readings to be between 0 and 360. Normalized:

In polar coordinates:

Visually, the PID controller seemed to work very well; every small increment, I would reset the target angle according to the current angle, which mitigated any yaw drift accumulation. I was able to always get around 360 degrees of turning (overshooting maybe a maximum of 30 degrees or so, even if the final yaw reading was not 360 degrees from the initial starting point.) I also tried testing how consistent my rotation and measurements were by using a larger total angle. To do this, I did a 720 degree rotation. As you can see in the graph below, the measurements are pretty much exactly periodic, which shows that my PID was working correctly.

Overall, given the speed of my rotations, the drift of the sensor was a maximum of 5 degrees off (this is a pretty lenient margin given my actual observations, since I reset the target every 30 degrees). On average, the drift was around 1 or 2 degrees. In a 4x4m room with my car at the center, the maximum distance I could measure is around 2*sqrt(2)=2.83m, which would be at the corners of the room. That means the max I could be off would be around 2830 - 2000/cos(45 - 5) = 2830 - 2610 = ~220 mm in either the x or y direction. With only 2 degrees of drift, it goes down to 2830 - 2000/cos(45 - 2) = 95 mm. Not too bad!

Part 4

Translating to Cartesian coordinates from each starting point is pretty straightforward; since the car always started with the ToF facing downward, I could use the following equations:

x = -distance * sin(theta) + x_origin * 304.8 (where 304.8 is the conversion factor from feet to mm)

y = -distance * cos(theta) + y_origin * 304.8

For the translation portion of the matrix, I simply need to shift the value to the point where my robot is placed for each scan. For the rotation, I needed to flip my y and x (explained below). There was no other angular rotation necessary. This matrix can be multiplied by a vector of the distance * sin/cos(theta) values, like in the equation above.

I had to negate my y and theta values to get an interpretable cartesian mapping because I think my ToF sensor is installed on the back of the car. The final mapping is as follows:

I decided to calculate the values again with my first set of data, which I skipped before because I thought it was less accurate.

Most of the data looks good (and looks like how I remember it in lab). However, I was still a bit unhappy with the bottom right corner, which seems to have failed in the second pass (first graph) and looks a bit ambiguous in the first pass (second graph). I ended up graphing it with the data I collected from when I tried to do a 720 degree rotation to see how consistent my PID and rotation was. This one looked slightly better!

And finally, here is everything combined:

Here are the points I manually filtered. The inner box border I based off of the scans I did at the origin and the bottom and top right corners. The points I used are below.

Part 1

This lab had a pretty straightforward setup (just downloading and installing the files from the repository). I initially had some problems installing PyQt5 since I have an Apple M1 Silicon mac, but I installed PyQt6 and changed one line of the source code to use PyQt6 instead of PyQt5 and it worked. The simulator can be started with a simple GUI. Once it's started, you can control a virtual robot using the arrow keys and adjust the view accordingly.

Part 2

The first task was getting the simulated robot to drive in a square. This was pretty straightforward and just involved setting the linear and angular velocity do different values.

After making small modifications to my code to log the data after every delay, I was able to graph the odometry and ground truth.

As you can see in the two runs above, even though I was setting the angular velocity to make the robot turn in perfect right angles, the ground truth was not actually a perfectly accurate square. This could either reflect errors in the angular velocity or the async timer sleep function, which determines the duration of the velocity command. Since the odometry position estimate is based on simulated on-board data (like an IMU), you also see how prone it is to building up errors over time.

I ran the simulation multiple times with the same commands. You can see how the ground truth is relatively consistent but still differs slightly, whereas the odometry data is extremely unreliable.

Part 3

The second task was getting the simulated robot to navigate the simulated space without crashing into obstacles. This was also pretty easy to do with closed loop obstacle avoidance.

The first technique I tried to avoid obstacles was simply getting a distance reading (equivalent to a ToF sensor reading) with cmdr.get_sensor(), then turning if it was below a certain threshold, otherwise going straight.

This worked okay, but it was very slow and still grazed walls when the ToF sensor just barely passed by a corner. Next, I tried with smaller distances and greater anglular velocity.

Next, I wrote a simple program to back up and turn 90 degrees, which reduced the frequency of grazing walls and worked pretty well with the 90 degree layout of the room (0:54 in the video). However, the 90 degree turns were still inconsistent, like before.

With some tweaking, it was possible to get the robot go up to 2 m/s and faster without any crashes (1:29 in the video)! I ran it for over 16 turns and there were no crashes. I had a 0.4m distance tolerance at this speed to avoid crashing into walls, but for the slower speeds I needed only 0.2m.

Part 1

The first function I had to implement was compute_control, which calculates the control information necessary to move the robot from one pose to another. Implementing this was easy: first, you figure out how the robot needs to turn to get into the direction to drive straight towards the destination point, then drive it and turn it so it's facing the correct destination direction.

Next, I based my odom motion model function, which is supposed to calculate how probable it is to transition from prev_pose to cur_pose given actual_u, off of the following equation:

I used the provided localization Gaussian functions and provided noise parameters:

The next part was writing the prediction step. In this step, I update the probabilities of moving to a certain area given the previous estimate of position and the motion model.

I spent a while trying to come up with ways to optimize the prediction step with numpy since there is a 6-nested for loop used to update the predictions. I decided to precompute the grid_to_map values, since the function would be called multiple times with identical values and it should be idempotent. However, I couldn't figure out a way to do better optimizations since the odom_motion_model function can't be vectorized easily. Instead, I ended up also doing what the lab assignment suggested and checking to see if the probability of it being in a certain area was above a certain threshold, since extremely low probabilities would be negligible but still computationally expensive to calculate.

Finally, for my update step, I calculated the likelihood of observing all 18 measurements from the current raycast for each location, then updated the bel values to reflect the likelihood calculations. This assumes that each observation from the raycast is independent, otherwise I wouldn't be able to multiply the probabilities. The update step is based on the sensor model, which calculates probabilities based on a gaussian distribution that uses the observed and expected readings from the sensor.

Part 2

The next part of the lab was actually testing the Bayes filter in the simulated environment. In the simulation, the car moves throughout the environment to certain points, then spins around and makes distance measurements, which is essentially doing a raycast. This was very reminiscent of the work we did in the earlier mapping lab.

Overall, the Bayes filter seemed to work extremely well! As you can see below (I included multiple trials), when running the simulation, the blue line (which is the belief) follows the ground truth quite closely. This performance is so much more useful than the regular prediction that can be made with only the odometer data.

In the last plot, you can even see how the predicted location gets off for a little bit but then immediately corrects itself. It's likely that it got off because it was near multiple regions that could yield similar sensor readings. This shows the power of the Bayes filter: errors don't propagate very far because they're corrected by future observations!

I also wanted to see how the Bayes filter prediction changed based on the number of observations made per each raycast. When I changed the ray count to 4, the predicted values were way off, but surprisingly with more rays the prediction didn't actually get that much closer to the ground truth. In fact, at some points, it seems to have gotten pretty off. This seems to suggest that the ray count is pretty optimal; given the noise and uncertainty of the measurements, it's possible that adding more rays would have very slight but negligible returns.

Part 1

The first part of the lab was testing the simulator localization, which was essentially what we had done in lab 11. This was just so I could familiarize myself with the simulator environment and plotter and Bayes filter, which we end up using later in the lab. The provided simulator Jupyter Notebook moves the simulated robot around the obstacle course, plotting the true location, the predicted location, and the odometry data. The resulting output looked extremely similar to the simulated localization from lab 11.

Part 2

The next part of the lab was to update the robot to turn in a circle and make distance measurements every 20 degrees. This was essentially the same as the raycasting that the simulated robot did. It was also relatively straightforward to implement because I had already done it in lab 9 to map out the obstacle course space.

There were two important differences with the implementation this time: the robot in lab 9 was supposed to turn around 14 times, but here we were recommended to do at least 18. Additionally, in this lab, in order to correctly match with the data used in the update step, the robot needs to rotate counterclockwise to make measurements.

In order to implement these two functions, I added a new command to the robot's Arduino code, SPIN_AND_MEASURE_ACCURATE. As you can see in the screenshot above, the command has several parameters you can pass in to tune the rotation information, including: maximum power, total angle, angle increment, duration after each angle, and angle tolerance. I also had to change my set point and target angle to be negative, since the spinning was counterclockwise. Initially, I tried rotating clockwise and then reversing the list in the Python code, but I couldn't get it to work. Afterwards, I just had to change my logging function to only output the distance readings from the ToF sensor, since I didn't need any IMU values. Everything else on the Arduino was identical to lab 9.

In the Jupyter Notebook, I needed to implement only one function, called perform_observation_loop. The purpose of this function is to tell the robot to make its 360 degree rotation and then receive the data back over bluetooth. I accomplished this with two simple bluetooth commands (one to tune the PID for better results and the other which I described earlier).

Initially, I tried setting a global "done" variable when the bluetooth receiver line contained a certain end flag message, but this caused my code to hang (probably some incorrect asynchronous logic). Since it was just as easy to wait a certain duration to be sure the robot had finished making measurements and then combine all the messages, I did that instead. I did have to filter out all the non-numeric debug messages that were also sent over the bluetooth line, but that was pretty trivial.

Part 3

For the next part of the lab, I tested the localization by putting it in the real life environment, running the update step, and viewing the predicted location. I used a uniform prior, since the robot was not moving from a previous location.

Here are the predicted locations when placed at the following spots, respectively: (-3, -2), (0, 3), (5, -3), (5, 3)

As you can see, the predicted location was actually quite accurate, even just with 18 distances and a uniform prior. For (-3, -2) and (5, -3), the location was actually exactly where I placed it. However, for (0, 3) and (5, 3), it was offset, but only slightly.

Below you can see the Bayes Filter probability for (-3, -2). The units are in meters, but when converted to feet, it is pretty much exactly -3 ft and -2 ft.

Below is the belief for (0, 3). The y coordinate is off by 1 foot (4 feet up instead of 3 feet). However, everything else is correct.

The rest of the values for the remaining two points are similar.

Here is a video showing the localization of all four preset points in the lab space:

Overall, localization with only 18 points worked surprisingly well for my robot. I was impressed by how accurately it predicted, even with a uniform prior. It was clear that some spots worked way more consistently than others (bottom left and right), probably because there were very few other locations that could have yielded similar readings. The localization seemed to perform the worst when there were long stretches of open space, where it's possible the ToF readings may have also become more inconsistent. For the most part, it worked super well! This lab also set a lot of good foundation for lab 13.

Part 1

Since this lab was open-ended, there were a lot of different ideas to consider. In the recent lab 12, we had done real localization, which was a very appealing candidate for the waypoint navigation. Beforehand, we had done precise PID control for moving quickly to reach a certain point away from the wall and rotating in place. In our localization labs, we also had written the foundation code to calculate the steps necessary to navigate between two poses. With all these ideas in mind, here were the approaches I mapped out:

Approach 1: Localization

The idea of this approach is to spin 360 degrees, localize in place, and then calculate the turns/distance to travel in order to move from one waypoint/pose to another. All of this would be repeated in a loop, so even if an angle or distance were to be off, I could just repeat the localization and slowly converge on the correct waypoint. This would require an Arduino function to turn a certain angle and a function to move forward a certain distance.

Approach 2: Faster, pre-planned path with precise PID control

With this approach, I can pre-compute the turns and distances away from the wall to hit each waypoint quickly. Then, after starting in the proper spot, I would execute all the bluetooth commands in series necessary to hit the waypoints. With precise PID and tolerance tuning, localization would be unnecessary, since the starting angle would be known and the path would be the same every time.

Approach 3: Double ToF navigation

With this approach, instead of having to fully localize, I could have a pretty decent idea of the angle of my robot with respect to the walls. Using the second ToF sensor on the side of my car, I could rotate my car back and forth until I minimized the distance to the ToF on the side of the car. As you can see in the diagram below, minimizing the distance to the second ToF means the car would be consistently parallel to the wall.

Having consistent right angles means I would be able to do a very rapid path navigation with fast straight line movement and consistent 90 degree turns.

Approach 4: If all else fails...

If everything were to go wrong, I wanted to have a contingency plan. I could always resort to open loop timed controls to quickly navigate the course. Luckily I didn't have to!

Part 2

Since I had so many possible ways to approach the problem, I wanted to do a comparison of which ones would be the most feasible and realistic.

Approach 1 seemed the most interesting to me, since having the robot approximate its own location and automatically navigate between waypoints seemed ideal. However, it would be slow, since it would have to frequently turn 360 degrees to localize. In my experience, my robot didn't get accurate localizations when turning quickly, so it would be hard to speed up the process.

Approach 2 would sacrifice the benefits of localization (being able to orient and drive the robot towards a waypoint no matter where it was) but it would also be much faster and potentially more accurate. The localization update step only returned predicted angles in increments of 20 degrees, which means the robot could actually get off. This was a large consideration in favor of the second approach compared to the first.

Approach 3 would be faster, but also could be prone to more errors. For some waypoints in more open spaces, using just the two ToF sensors to determine orientation and location would yield a much worse prediction for the robot than a full 360 degree localization.

Approach 4 would most likely be the easiest and quickest to implement, but it would also use the fewest things we learned in class! So it was the last desirable choice.