Ananya's Portfolio - Time of Flight

Lab 7: Kalman Filter

2026-03-23T00:00:00+00:00

Lab 7: Kalman Filter</h1>
The goal of this lab is to implement a Kalman filter so the robot, Meep, is able to drive at a high speed towards a wall and stop before hitting it. We do this by creating a physics model to predict Meep’s movement, then tuning a Kalman filter and using its predictions to estimate position while waiting for feedback from the time of flight.</p>

What is a Kalman Filter?</h2>
A Kalman filter is a way to combine a model for a system and sensor measurements to get an accurate estimate for the system. In this case, we have a time of flight that give accurate, but slow estimates for our position. By creating a physics model for our system, we can combine a calculated position with our sensor measurements to get quicker and more accurate location data for our robot.</p>

Estimating Drag and Momentum</h2>

To start, we need to have an accurate physical model for our bot. Let u</code> be the control signal (aka the PWM value or duty cycle) passed in to our motors), and let x</code> be Meep’s position, represented as distance from the wall infront of Meep. This leaves us with the equation below where d</code> is a constant representing drag while m</code> represents momentum.</p>

We can find d</code> by finding the steady speed for some u</code> (motor contorl input). I chose to operate my car at an analog value of 150 (duty cycle of 58.82%). I ran 3 trials and calculated the velocity from each as shown below.</p>

From there, I took the average of the three runs and attempted to fit a expoential decay function to the measurements.</p>

From here, we see the steady state velocity is -3176 mm/s. We can calculate d</code> to be 150/the steady state velocity so that the control input can be the passed in PWM value in the future. We calculate m</code> to be t_90/ln(1-0.9) where t_90 is the time it takes to get to 90% of the steady velocity. We have a rise time of 1.73 s giving us an m</code> of 0.75.</p>

d</span> = -</span>0.047227600035556115</span></span>
m</span> =</span> 0.7508005698913823</span></span>
A</span> =</span> np</span>.</span>array</span>([</span></span>
    [</span>0</span>,</span> 1</span>],</span></span>
    [</span>0</span>,</span> d</span>/</span>m</span>]</span></span>
])</span></span>
</span>
B</span> =</span> np</span>.</span>array</span>([</span></span>
    [</span>0</span>],</span></span>
    [</span>1</span>/</span>m</span>]</span></span>
])</span></span>
</span>
C</span> =</span> np</span>.</span>array</span>([[</span>1</span>,</span>0</span>]])</span></span>
</span>
def</span> kf</span>(</span>mu</span>,</span>sigma</span>,</span>u</span>,</span>y</span>,</span> dt</span>,</span> update</span> =</span> True</span>):</span></span>
    Ad</span> =</span> np</span>.</span>eye</span>(</span>len</span>(</span>A</span>))</span> +</span> dt</span> *</span> A  </span></span>
    Bd</span> =</span> dt</span> *</span> B</span></span>
    </span></span>
    mu_p</span> =</span> Ad</span>.</span>dot</span>(</span>mu</span>)</span> +</span> Bd</span>.</span>dot</span>(</span>u</span>)</span> </span></span>
    sigma_p</span> =</span> Ad</span>.</span>dot</span>(</span>sigma</span>.</span>dot</span>(</span>Ad</span>.</span>transpose</span>()))</span> +</span> sig_u</span></span>
</span>
    if</span> update</span>:</span></span>
        sigma_m</span> =</span> C</span>.</span>dot</span>(</span>sigma_p</span>.</span>dot</span>(</span>C</span>.</span>transpose</span>()))</span> +</span> sig_z</span></span>
        kkf_gain</span> =</span> sigma_p</span>.</span>dot</span>(</span>C</span>.</span>transpose</span>().</span>dot</span>(</span>np</span>.</span>linalg</span>.</span>inv</span>(</span>sigma_m</span>)))</span></span>
    </span></span>
        y_m</span> =</span> y</span>-</span>C</span>.</span>dot</span>(</span>mu_p</span>)</span></span>
        mu</span> =</span> mu_p</span> +</span> kkf_gain</span>.</span>dot</span>(</span>y_m</span>)</span>    </span></span>
        sigma</span>=</span>(</span>np</span>.</span>eye</span>(</span>2</span>)</span>-</span>kkf_gain</span>.</span>dot</span>(</span>C</span>)).</span>dot</span>(</span>sigma_p</span>)</span></span>
        return</span> mu</span>,</span>sigma</span></span>
    else</span>:</span></span>
        return</span> mu_p</span>,</span> sigma_p</span></span>
</span></code></pre>Tuning noise on simulated data</h2>
From here, we need to tune our uncertainity of our sensors and model. We have 4 parameters we can tune, the position and speed uncertainity from the physics model, the sensor noise, and the initial sigma.</p>
I started with a position and velocity noise of 100 and a sensor noise of 400. This noise was too large the sensor uncertainity. There was a major lag in the Kalman filter as sensor readings were bascially not trusted at all</p>

From there, I lowered the sensor noise as was able to see the filter follow the time of flight. I wanted the filter to be able to deviate from the time of flight so I chose a standard deviation of 10. This lead to the filter following the sensor almost exactly on all my datasets.</p>



I then had my kalman extrapolate predicted positions for the time steps where the time of flight was not ready with a new reading. This resulted in the spiky graph below.</p>

This made me reevalute my position and velocity uncertainity. I expect the physics model to be relavtively accurate for the position but not for the velocity. I updated the sigmas to reflect this (decreaing position to 10 which increasing the velcoity uncertainity), to get the graph below:</p>

sigma_1</span> =</span> np</span>.</span>sqrt</span>((</span>1</span>**</span>2</span>)</span>*</span>1</span>/</span>dt</span>)</span> # position variance</span></span>
sigma_2</span> =</span>  np</span>.</span>sqrt</span>((</span>50</span>**</span>2</span>)</span>*</span>1</span>/</span>dt</span>)</span> # speed variance</span></span>
sigma_3</span> =</span> 10</span> # sensor noise</span></span>
</span>
sig_u</span>=</span>np</span>.</span>array</span>([[</span>sigma_1</span>**</span>2</span>,</span>0</span>],[</span>0</span>,</span>sigma_2</span>**</span>2</span>]])</span></span>
sig_z</span>=</span>np</span>.</span>array</span>([[</span>sigma_3</span>**</span>2</span>]])</span></span></code></pre>Implement the Kalman Filter with PID</h2>
From there, I implemented the Kalman filter on the Artemis as shown below</p>
Matrix</span><</span>2</span>,</span>1</span>></span> mu </span>=</span> {</span>0</span>,</span>0</span>};</span> </span></span>
Matrix</span><</span>2</span>,</span>2</span>></span> sigma </span>=</span> {</span>100</span>,</span>0</span>,</span>100</span>,</span>0</span>};</span> </span></span>
</span>
float</span> d </span>= -</span>0.047227600035556115</span>;</span></span>
float</span> m </span>=</span> 0.7508005698913823</span>;</span></span>
</span>
</span>
Matrix</span><</span>2</span>,</span>2</span>></span> kA </span>=</span> {</span>0</span>,</span> 1</span>,</span></span>
                 0</span>,</span> d</span>/</span>m</span>};</span> </span></span>
</span>
Matrix</span><</span>2</span>,</span>1</span>></span> kB </span>=</span> {</span>0</span>,</span> 1</span>/</span>m</span>};</span> </span></span>
</span>
Matrix</span><</span>1</span>,</span>2</span>></span> kC </span>=</span> {</span>1</span>,</span> 0</span>};</span> </span></span>
</span>
float</span> var_position </span>=</span> 9.09</span>;</span></span>
float</span> var_speed </span>=</span> 227272.73</span>;</span></span>
float</span> var_sensor </span>=</span> 100</span>;</span></span>
</span>
Matrix</span><</span>2</span>,</span>2</span>></span> sigma_u </span>=</span> {</span>var_position</span>,</span> 0</span>,</span></span>
                        0</span>,</span> var_speed</span>};</span></span>
</span>
Matrix</span><</span>1</span>></span> sigma_z </span>=</span> {</span>var_sensor</span>};</span></span>
</span>
float</span> kf</span>(</span>float</span> control_in</span>,</span> float</span> sensor_reading</span>,</span> float</span> dt</span>,</span> int</span> tof_valid</span>){</span></span>
  Matrix</span><</span>1</span>></span> control </span>=</span> {</span>control_in</span>};</span></span>
  Matrix</span><</span>1</span>></span> sensor </span>=</span> {</span>sensor_reading</span>};</span></span>
  // Matrix<1> dt = {dt/1000.0};</span></span>
  Matrix</span><</span>2</span>,</span> 2</span>></span> I </span>=</span> {</span> 1</span>,</span> 0</span>,</span> 0</span>,</span> 1</span>};</span></span>
  Matrix</span><</span>2</span>,</span> 2</span>></span> Ad </span>=</span> I</span>;</span></span>
  Ad</span>(</span>0</span>,</span>0</span>)</span> +=</span> kA</span>(</span>0</span>,</span>0</span>)</span> *</span> (</span>dt</span>);</span></span>
  Ad</span>(</span>0</span>,</span>1</span>)</span> +=</span> kA</span>(</span>0</span>,</span>1</span>)</span> *</span> (</span>dt</span>);</span></span>
  Ad</span>(</span>1</span>,</span>0</span>)</span> +=</span> kA</span>(</span>1</span>,</span>0</span>)</span> *</span> (</span>dt</span>);</span></span>
  Ad</span>(</span>1</span>,</span>1</span>)</span> +=</span> kA</span>(</span>1</span>,</span>1</span>)</span> *</span> (</span>dt</span>);</span></span>
  Matrix</span><</span>2</span>,</span> 1</span>></span> Bd </span>=</span> kB</span>;</span></span>
  Bd</span>(</span>0</span>,</span>0</span>)</span> *=</span> (</span>dt</span>);</span></span>
  Bd</span>(</span>1</span>,</span>0</span>)</span> *=</span> (</span>dt</span>);</span></span>
</span>
  Matrix</span><</span>2</span>,</span> 1</span>></span> mu_p </span>=</span> Ad</span>*</span>mu </span>+</span> Bd</span>*</span>control</span>;</span></span>
  Matrix</span><</span>2</span>,</span> 2</span>></span> sigma_p </span>=</span> Ad</span>*</span>sigma</span>*~</span>Ad </span>+</span> sigma_u</span>;</span></span>
</span>
  if</span> (</span>tof_valid</span>) {</span></span>
    Matrix</span><</span>1</span>></span> sigma_m </span>=</span> kC </span>*</span> sigma_p </span>* ~</span>kC </span>+</span> sigma_z</span>;</span></span>
    Matrix</span><</span>2</span>,</span>1</span>></span> kff_gain </span>=</span> sigma_p</span>*</span>(</span>~</span>kC</span>*</span>Inverse</span>(</span>sigma_m</span>));</span></span>
</span>
    Matrix</span><</span>1</span>></span> y_m </span>=</span> sensor</span>-</span>kC</span>*</span>mu_p</span>;</span></span>
    mu </span>=</span> mu_p</span>+</span>kff_gain</span>*</span>y_m</span>;</span> </span></span>
    sigma </span>=</span> (</span>I </span>-</span> kff_gain</span>*</span>kC</span>)</span>*</span>sigma_p</span>;</span></span>
</span>
    return</span> mu</span>(</span>0</span>,</span>0</span>);</span></span>
  }</span></span>
  else</span> {</span></span>
    mu </span>=</span> mu_p</span>;</span></span>
    sigma </span>=</span> sigma_p</span>;</span></span>
    return</span> mu_p</span>(</span>0</span>,</span>0</span>);</span></span>
  }</span></span>
</span>
}</span></span></code></pre>
Then I integrated it with my pid code:</p>
void</span> pid_wall_dist</span>(</span>float</span> dist</span>){</span></span>
  float</span> curr_dist</span>;</span></span>
  time_stamps</span>[</span>i</span>]</span> =</span> millis</span>();</span></span>
  if</span> (</span>!</span>distanceSensor2</span>.</span>checkForDataReady</span>()) {</span></span>
    tof2_readings</span>[</span>i</span>]</span> = -</span>5</span>;</span></span>
</span>
    // no sensor data on the first time step</span></span>
    if</span> (</span>i </span>==</span> 0</span>) {</span></span>
      tof1_readings</span>[</span>i</span>]</span> = -</span>5</span>;</span></span>
      p_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      i_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      d_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      motor_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      i</span>++</span>;</span></span>
      if</span> (</span>i </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
        i </span>=</span> 0</span>;</span></span>
      }</span></span>
      return</span> ;</span></span>
    }</span></span>
</span>
    // no valid readings yet</span></span>
    else if</span> (</span>tof2_readings</span>[</span>i</span>-</span>1</span>]</span> == -</span>5</span> &&</span> tof1_readings</span>[</span>i</span>-</span>1</span>]</span> == -</span>5</span>){</span></span>
      tof1_readings</span>[</span>i</span>]</span> =</span> tof1_readings</span>[</span>i</span>-</span>1</span>];</span></span>
      p_data</span>[</span>i</span>]</span> =</span> p_data</span>[</span>i</span>-</span>1</span>];</span></span>
      i_data</span>[</span>i</span>]</span> =</span> i_data</span>[</span>i</span>-</span>1</span>];</span></span>
      d_data</span>[</span>i</span>]</span> =</span> d_data</span>[</span>i</span>-</span>1</span>];</span></span>
      motor_data</span>[</span>i</span>]</span> =</span> motor_data</span>[</span>i</span>-</span>1</span>];</span></span>
      i</span>++</span>;</span></span>
      if</span> (</span>i </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
        i </span>=</span> 0</span>;</span></span>
      }</span></span>
      return</span> ;</span></span>
    }</span></span>
</span>
</span>
    // Use kalman to predict position</span></span>
    else</span> {</span></span>
      int</span> dt </span>=</span> time_stamps</span>[</span>i</span>]</span> -</span> time_stamps</span>[</span>i</span>-</span>1</span>];</span></span>
</span>
      if</span> (</span>dt </span>==</span> 0</span>) {</span></span>
        curr_dist </span>=</span> tof1_readings</span>[</span>i</span>-</span>1</span>];</span>   </span></span>
      }</span></span>
      else</span> {</span></span>
        curr_dist </span>=</span> kf</span>(</span>motor_data</span>[</span>i</span>-</span>1</span>],</span> 0</span>, (</span>float</span>)</span> dt</span>/</span>1000.0</span>,</span> false</span>);</span></span>
      }</span></span>
      tof1_readings</span>[</span>i</span>]</span> =</span> curr_dist</span>;</span></span>
    }</span></span>
  }</span></span>
</span>
</span>
  // Use Kalman with TOF to predict position and update the filter</span></span>
  else</span> {</span></span>
    int</span> dt </span>=</span> time_stamps</span>[</span>i</span>]</span> -</span> time_stamps</span>[</span>i</span>-</span>1</span>];</span></span>
    tof2_readings</span>[</span>i</span>]</span> =</span> distanceSensor2</span>.</span>getDistance</span>();</span></span>
    tof1_readings</span>[</span>i</span>]</span> =</span> kf</span>(</span>motor_data</span>[</span>i</span>-</span>1</span>],</span> tof2_readings</span>[</span>i</span>], (</span>float</span>)</span> dt</span>/</span>1000.0</span>,</span> true</span>);</span></span>
    curr_dist </span>=</span>  tof1_readings</span>[</span>i</span>];</span></span>
    distanceSensor2</span>.</span>clearInterrupt</span>();</span></span>
    old_time </span>=</span> time_stamps</span>[</span>i</span>];</span></span>
  }</span></span>
  float</span> e </span>=</span> tof1_readings</span>[</span>i</span>]</span> -</span> dist</span>;</span></span>
  error_total </span>+=</span> e</span>;</span></span>
</span>
  // Integrator Windup code ///</span></span>
  if</span> (</span>error_total </span>></span> 2000</span>){</span></span>
    error_total </span>=</span> 2000</span>;</span></span>
  }</span></span>
  else if</span> (</span>error_total </span>< -</span>3000</span>){</span></span>
    error_total </span>= -</span>3000</span>;</span></span>
  }</span></span>
  /////////end//////////////</span></span>
</span>
  error_change </span>=</span> e</span>-</span>last_error</span>;</span></span>
  int</span> motor_out </span>=</span> (</span>int</span>)(</span>Kp</span>*</span>e </span>+</span> Ki</span>*</span>error_total </span>+</span> Kd</span>*</span>error_change</span>);</span></span>
</span>
  p_data</span>[</span>i</span>]</span> =</span> Kp</span>*</span>e</span>;</span></span>
  i_data</span>[</span>i</span>]</span> =</span> Ki</span>*</span>error_total</span>;</span></span>
  d_data</span>[</span>i</span>]</span> =</span> Kd</span>*</span>error_change</span>;</span></span>
</span>
</span>
  ///// Motor Updates //////////////</span></span>
  if</span> (</span>motor_out </span>> -</span>8</span> &&</span> motor_out </span><</span> 8</span>){</span></span>
    forward</span>(</span>0</span>,</span> 0</span>);</span></span>
    motor_out </span>=</span> 0</span>;</span> </span></span>
  }</span></span>
  else if</span> (</span>motor_out </span>></span> 5</span>){</span></span>
    if</span> (</span>motor_out </span><</span> 37</span>){</span></span>
      motor_out </span>=</span> 37</span>;</span> </span></span>
    }</span></span>
    else if</span> (</span>motor_out </span>></span> 255</span>){</span></span>
      motor_out </span>=</span> 255</span>;</span></span>
    }</span></span>
    forward</span>(</span>motor_out</span>-</span>3</span>,</span> motor_out</span>+</span>3</span>);</span>   </span></span>
  }</span></span>
  else</span> {</span></span>
    if</span> (</span>motor_out </span>> -</span>37</span>){</span></span>
      motor_out </span>= -</span>37</span>;</span> </span></span>
    }</span></span>
    else if</span> (</span>motor_out </span>< -</span>255</span>){</span></span>
      motor_out </span>= -</span>255</span>;</span></span>
    }</span></span>
    backward</span>(</span>-</span>motor_out</span>,</span> -</span>motor_out</span>);</span></span>
  }</span></span>
  motor_data</span>[</span>i</span>]</span> =</span> motor_out</span>;</span></span>
  ///////////end/////////////////</span></span>
  </span></span>
  // Step forward</span></span>
  last_error </span>=</span> e</span>;</span></span>
  i</span>++</span>;</span></span>
  if</span> (</span>i </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
    i </span>=</span> 0</span>;</span></span>
  }</span></span>
}</span></span></code></pre>
I started with only allowing the robot to run for 1 second before stopping it to ensure that any initial implementation bugs could be worked out without damage being done to Meep. Once, the Kalman filter was showing expected results, I was ready to run the full PID.</p>

Using my original PID settings, we can see that Meep is able to start at its max duty cycle and stop just before hitting the wall.</p>
</iframe>


Collaboration and Sources</h2>
I referenced the lecture slides when setting up the Kalman filter. I also referenced Trevor Dales and Aidan Derocher’s lab writeups.</p>



Lab 5: Linear PID control and Linear Interpolation
2026-03-09T00:00:00+00:00
Lab 5: Linear PID control and Linear interpolation</h1>
The goal of this lab was to get the robot, Meep, to drive forward and stop extactly 304 mm from a wall via PID controls. This lab explores tuning a PID controller, extrapolating sensor data, and handling integrator windup.</p>
Why PID?</h2>
PID or Proportional, Inetegral, and Derivative controller is a controller that adjusts the PWM signal based on the bots current error from a set point. In this lab, the setpoint is the distance away from the wall and the error is determined to be the difference between the car’s estimated position (via Time of Flight) and the setpoint.</p>
I chose to implement a full PID controller for this lab. I started with just a proportional term, which resulted in either heavy overshoot or stopping a great distance from the target. While I added the I term (required since I am a grad student), this overshoot issue got worse. To correct for this, the derivative term was added. I implemented a PID_START</code> and PID_STOP</code> command to signal to the robot to start and stop the PID cotnroller. The PID_START</code> command takes in values for Kp</code>, Ki</code>, and Kd</code> and triggers the pid_wall_dist</code> function to run as shown below:</p>
case PID_START: </span>{</span></span>
  Serial</span>.</span>println</span>(</span>"Starting PID"</span>);</span></span>
  success </span>=</span> robot_cmd</span>.</span>get_next_value</span>(</span>Kp</span>);</span></span>
  success </span>=</span> robot_cmd</span>.</span>get_next_value</span>(</span>Ki</span>);</span></span>
  success </span>=</span> robot_cmd</span>.</span>get_next_value</span>(</span>Kd</span>);</span></span>
  if</span> (</span>!</span>success</span>)</span></span>
      return</span>;</span></span>
</span>
  pid_running </span>=</span> true</span>;</span> //tell loop to call pid_wall_dist</span></span>
  </span></span>
  // reset variables</span></span>
  i </span>=</span> 0</span>;</span></span>
  error_total </span>=</span> 0</span>;</span> </span></span>
  last_error </span>=</span> 0</span>;</span></span>
  error_change </span>=</span> 0</span>;</span></span>
  break</span>;</span></span>
}</span></span></code></pre>// Takes in [dist] which is the setpoint in mm</span></span>
void</span> pid_wall_dist</span>(</span>float</span> dist</span>){</span></span>
  float</span> curr_dist</span>;</span></span>
  time_stamps</span>[</span>i</span>]</span> =</span> millis</span>();</span></span>
  </span></span>
  // if there is no sensor reading</span></span>
  if</span> (</span>!</span>distanceSensor2</span>.</span>checkForDataReady</span>()) {</span></span>
    tof2_readings</span>[</span>i</span>]</span> = -</span>5</span>;</span></span>
</span>
    // no data to extrapolate with</span></span>
    if</span> (</span>i </span>==</span> 0</span>) {</span></span>
      tof1_readings</span>[</span>i</span>]</span> = -</span>5</span>;</span></span>
      p_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      i_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      d_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      motor_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      i</span>++</span>;</span></span>
      if</span> (</span>i </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
        i </span>=</span> 0</span>;</span></span>
      }</span></span>
      return</span> ;</span></span>
    }</span></span>
</span>
    // only one reading or no valid readings</span></span>
    else if</span> (</span>i </span>==</span> 1</span> ||</span> tof1_readings</span>[</span>i</span>-</span>1</span>]</span> == -</span>5</span> ||</span> tof1_readings</span>[</span>i</span>-</span>2</span>]</span> == -</span>5</span>){</span></span>
      tof1_readings</span>[</span>i</span>]</span> =</span> tof1_readings</span>[</span>i</span>-</span>1</span>];</span></span>
      p_data</span>[</span>i</span>]</span> =</span> p_data</span>[</span>i</span>-</span>1</span>];</span></span>
      i_data</span>[</span>i</span>]</span> =</span> i_data</span>[</span>i</span>-</span>1</span>];</span></span>
      d_data</span>[</span>i</span>]</span> =</span> d_data</span>[</span>i</span>-</span>1</span>];</span></span>
      motor_data</span>[</span>i</span>]</span> =</span> motor_data</span>[</span>i</span>-</span>1</span>];</span></span>
      i</span>++</span>;</span></span>
      if</span> (</span>i </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
        i </span>=</span> 0</span>;</span></span>
      }</span></span>
      return</span> ;</span></span>
    }</span></span>
</span>
    // Can extrapolate next tof value</span></span>
    else</span> {</span></span>
      int</span> dt </span>=</span> time_stamps</span>[</span>i</span>-</span>1</span>]</span> -</span> time_stamps</span>[</span>i</span>-</span>2</span>];</span></span>
</span>
      if</span> (</span>dt </span>==</span> 0</span>) {</span></span>
        curr_dist </span>=</span> tof1_readings</span>[</span>i</span>-</span>1</span>];</span>   // fallback</span></span>
      }</span></span>
      else</span> {</span></span>
        curr_dist </span>=</span> (</span>old_slope</span>)</span> *</span>                    //  m</span></span>
              (</span>time_stamps</span>[</span>i</span>]</span> -</span> time_stamps</span>[</span>i</span>-</span>1</span>])</span> +</span>  // *x</span></span>
              tof1_readings</span>[</span>i</span>-</span>1</span>];</span>                    // +b</span></span>
      }</span></span>
      if</span> (</span>curr_dist </span><</span> 0</span>) {</span></span>
        tof1_readings</span>[</span>i</span>]</span> =</span> dist</span>;</span></span>
      }</span></span>
      else if</span> (</span>curr_dist </span>></span> 5000</span>){</span></span>
        tof1_readings</span>[</span>i</span>]</span> =</span> 4000</span>;</span></span>
      }</span></span>
      else</span> {</span></span>
        tof1_readings</span>[</span>i</span>]</span> =</span> curr_dist</span>;</span></span>
      }</span></span>
    }</span></span>
  }</span></span>
</span>
  // if there is a sensor reading</span></span>
  else</span> {</span></span>
    tof1_readings</span>[</span>i</span>]</span> =</span> distanceSensor2</span>.</span>getDistance</span>();</span></span>
    tof2_readings</span>[</span>i</span>]</span> =</span> tof1_readings</span>[</span>i</span>];</span></span>
    curr_dist </span>=</span>  tof1_readings</span>[</span>i</span>];</span></span>
    distanceSensor2</span>.</span>clearInterrupt</span>();</span></span>
    old_slope </span>=</span> (</span>curr_dist </span>-</span> old_tof</span>)</span>/</span>(</span>time_stamps</span>[</span>i</span>]</span>-</span>old_time</span>);</span></span>
    old_tof </span>=</span> curr_dist</span>;</span></span>
    old_time </span>=</span> time_stamps</span>[</span>i</span>];</span></span>
  }</span></span>
</span>
  // update errors and controller</span></span>
  float</span> e </span>=</span> curr_dist </span>-</span> dist</span>;</span></span>
  error_total </span>+=</span> e</span>;</span></span>
  error_change </span>=</span> e</span>-</span>last_error</span>;</span></span>
  int</span> motor_out </span>=</span> (</span>int</span>)(</span>Kp</span>*</span>e </span>+</span> Ki</span>*</span>error_total </span>+</span> Kd</span>*</span>error_change</span>);</span></span>
</span>
  p_data</span>[</span>i</span>]</span> =</span> Kp</span>*</span>e</span>;</span></span>
  i_data</span>[</span>i</span>]</span> =</span> Ki</span>*</span>error_total</span>;</span></span>
  d_data</span>[</span>i</span>]</span> =</span> Kd</span>*</span>error_change</span>;</span></span>
  motor_data</span>[</span>i</span>]</span> =</span> motor_out</span>;</span></span>
</span>
  // update motors (corrected for straight drive)</span></span>
  if</span> (</span>motor_out </span>></span> 0</span>){</span></span>
    forward</span>(</span>motor_out</span>-</span>3</span>,</span> motor_out</span>+</span>3</span>);</span>   </span></span>
  }</span></span>
  else</span> {</span></span>
    backward</span>(</span>-</span>motor_out</span>,</span> -</span>motor_out</span>);</span></span>
  }</span></span>
  last_error </span>=</span> e</span>;</span></span>
  </span></span>
  i</span>++</span>;</span></span>
  if</span> (</span>i </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
    i </span>=</span> 0</span>;</span></span>
  }</span></span>
}</span></span></code></pre>Prelab: Data transfer</h2>
One of the major decisions of this lab way how to tranfer data. Transferring each piece of data in the control loop
would slow the loop down, resulting in slower updates and slower reaction time. Trying to store all the data is limited by the maximum storage of the Artemis. I decide to store all my values and transmit after PID has stopped. This results in a long waiting time after running my controller, but allows the code to loop fasting during the original run time. I allows the buffer to override the inital values if the controller tries to save more values than the set max buffer length. I parse this data in python lists like I have in previous labs and save them to a CSV for the future.</p>
case PID_STOP: </span>{</span></span>
  // Stop PID controller and the motors</span></span>
  pid_running </span>=</span> false</span>;</span></span>
  analogWrite</span>(</span>AB1_left</span>,</span>0</span>);</span> </span></span>
  analogWrite</span>(</span>AB2_left</span>,</span>0</span>);</span></span>
  analogWrite</span>(</span>AB1_right</span>,</span>0</span>);</span> </span></span>
  analogWrite</span>(</span>AB2_right</span>,</span>0</span>);</span></span>
</span>
  // tranfer the data written</span></span>
  for</span> (</span>int</span> j </span>=</span> 0</span>;</span> j </span><</span> i</span>;</span> j</span>++</span>){</span></span>
    tx_estring_value</span>.</span>clear</span>();</span></span>
    tx_estring_value</span>.</span>append</span>(</span>time_stamps</span>[</span>j</span>]);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>":"</span>);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>tof1_readings</span>[</span>j</span>]);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>":"</span>);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>tof2_readings</span>[</span>j</span>]);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>":"</span>);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>p_data</span>[</span>j</span>]);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>":"</span>);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>i_data</span>[</span>j</span>]);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>":"</span>);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>d_data</span>[</span>j</span>]);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>":"</span>);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>motor_data</span>[</span>j</span>]);</span></span>
    tx_characteristic_string</span>.</span>writeValue</span>(</span>tx_estring_value</span>.</span>c_str</span>());</span></span>
  }</span></span>
  </span></span>
  // Signal the data tranfer is done and it is safe to stop the listener</span></span>
  tx_estring_value</span>.</span>clear</span>();</span></span>
  tx_estring_value</span>.</span>append</span>(</span>"Done"</span>);</span></span>
  tx_characteristic_string</span>.</span>writeValue</span>(</span>tx_estring_value</span>.</span>c_str</span>());</span>          </span></span>
  break</span>;</span></span>
}</span></span></code></pre>Range/Sampling time</h2>
I chose to swap my time of flight from short range mode to long range so that the max distance would be 4m instead of 1.5m. I originally decided to use short range mode since it takes far less time to be ready with a distance reading. However, we work on extrapolating values so that we can update the motors with an educated guess of our distance readings. This prevents the polling delay on the time of flight from being a limiting factor in the speed between motor updates. Since the delay becomes a non-issue, the benefits of long range mode outweigh the cons.</p>
With the original loop waiting for the time of flight to be ready, the average time between motor updates was 100.29 ms</strong>. I modified the loop to skip when the time of flight was not ready. This sped the loop up to and average of 2.4 ms</strong> between updates. Iterations that required processing a time of flight value took much longer, while iterations with no new time of flight reading typically ran very fast. Finally, when data extrapolation was added (as dsicussed below) the loop ran updating the motor values every 7.68 ms</strong> on average.</p>
Data Extrapolation</h3>
We use linear extrapolation to estimate the next time of flight value when one isn’t available. The way I decided to do this was to use the previous two valid time of flight readings to make a slope. I orginally used the last two readings to calcualte the slope (including the last two estimated readings). This was problematic since the estimated readings were often smaller than the real distaces since the bot tends to slow down as it approaches the wall. When an accurate ToF reading came in, the slope would become postive and the code would assume Meep was moving away from the wall rather than towards it. To find the next value, we simply use slope-intercept with the change in time and previous estimated distance.</p>

// Code to calculate slope when a new distance reading is available</span></span>
tof1_readings</span>[</span>i</span>]</span> =</span> distanceSensor2.</span>getDistance</span>();</span></span>
tof2_readings</span>[</span>i</span>]</span> =</span> tof1_readings</span>[</span>i</span>];</span></span>
curr_dist </span>=</span>  tof1_readings</span>[</span>i</span>];</span></span>
distanceSensor2.</span>clearInterrupt</span>();</span></span>
old_slope </span>=</span> (</span>curr_dist </span>-</span> old_tof</span>)</span>/</span>(</span>time_stamps</span>[</span>i</span>]</span>-</span>old_time</span>);</span></span>
old_tof </span>=</span> curr_dist</span>;</span></span>
old_time </span>=</span> time_stamps</span>[</span>i</span>];</span></span></code></pre>// using point slope to estimate when data isn't available</span></span>
curr_dist </span>=</span> (</span>old_slope</span>)</span> *</span>                    //  m</span></span>
      (</span>time_stamps</span>[</span>i</span>]</span> -</span> time_stamps</span>[</span>i</span>-</span>1</span>])</span> +</span>  // *x</span></span>
      tof1_readings</span>[</span>i</span>-</span>1</span>];</span>                    // +b</span></span>
</span>
// Sanity checks to make sure we aren't making up a crazy distance </span></span>
if</span> (</span>curr_dist </span><</span> 0</span>) {</span></span>
  tof1_readings</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
}</span></span>
else if</span> (</span>curr_dist </span>></span> 5000</span>){</span></span>
  tof1_readings</span>[</span>i</span>]</span> =</span> 4000</span>;</span></span>
}</span></span>
else</span> {</span></span>
  tof1_readings</span>[</span>i</span>]</span> =</span> curr_dist</span>;</span></span>
}</span></span></code></pre>Tuning process and results</h2>
I started with just a proportional controller. Since my max distance from the time of flight was 4000 mm and the setpoint was 304 mm, the max error was This resulted in some successfully runs, but the controller tended to overshoot and hit the wall and have steady state error in both the negative and positive direction. Here’s a video of the most successful run on the gain only controller.</p>
</iframe>
From there, I decided to add the integral term, since I wanted to see if PID was needed or if PI would work well. I was reminded that Ki should typically be a very small value as the cummlative error can get to a large number fairly quickly.</p>
</iframe>
Since I was experiencing a large amount of overshoot, I decided to add the derivative term and use a full PID controller. I restarted my tuning process, finding a Kp that resulted in the car being stable with steady state error (0.06). I added a small Ki to try to correct for this (0.0001). At this point, the car began to overshoot its set point, so I added Kd (1). I was able to successfully stop at the set point in these conditions.</p>
</iframe>


The PID controller should also be able to find the wall again when displaced. I pulled the car back to show that it was able to recorrect.</p>
</iframe>
Grad Task: Integrator Windup</h2>
As previously mentioned, the cummalitive error can get very large very quickly, especially when first starting the controller. Integrator windup occurs when the integrator term gets overpopulated very quickly when the setpoint is set.</p>
</iframe>
We can avoid this by limiting the max value of the total error. This prevents the error accumulation from blowing up, even if we keep the bot away from the set point for an extended period of time.</p>
if</span> (</span>error_total </span>></span> 2000</span>){</span></span>
  error_total </span>=</span> 2000</span>;</span></span>
}</span></span>
else if</span> (</span>error_total </span>< -</span>3000</span>){</span></span>
  error_total </span>= -</span>3000</span>;</span></span>
}</span></span></code></pre></iframe>
Collaboration and Sources</h2>
I referenced the lecture slides when setting up PID. I discussed ideas for code structure with Dyllan Hofflich.</p>

Ananya's Portfolio - Time of Flight

Lab 7: Kalman Filter

Collaboration and Sources</h2> I referenced the lecture slides when setting up the Kalman filter. I also referenced Trevor Dales and Aidan Derocher’s lab writeups.</p>

Lab 5: Linear PID control and Linear Interpolation

Lab 5: Linear PID control and Linear interpolation</h1> The goal of this lab was to get the robot, Meep, to drive forward and stop extactly 304 mm from a wall via PID controls. This lab explores tuning a PID controller, extrapolating sensor data, and handling integrator windup.</p>

Collaboration and Sources</h2> I referenced the lecture slides when setting up PID. I discussed ideas for code structure with Dyllan Hofflich.</p>

Collaboration and Sources</h2>
I referenced the lecture slides when setting up the Kalman filter. I also referenced Trevor Dales and Aidan Derocher’s lab writeups.</p>

Lab 5: Linear PID control and Linear interpolation</h1>
The goal of this lab was to get the robot, Meep, to drive forward and stop extactly 304 mm from a wall via PID controls. This lab explores tuning a PID controller, extrapolating sensor data, and handling integrator windup.</p>

Collaboration and Sources</h2>
I referenced the lecture slides when setting up PID. I discussed ideas for code structure with Dyllan Hofflich.</p>