Lab 7

Net ID: pp386

Objective: Kalman Filter

This lab deals with Kalman Filters and how to use them to make the computations faster for the robot to respond faster to obstacles along with sensor readings. Kalman filters give us the probabilistic sensor readings after each ieration.

Step Response

First I implement a step response to my car, that is give it a fixed PWM (127) and when it gets close to the wall, stop it completely using active braking (225 to all 4 pins). The top speed of the robot achieved was 1304 mm/s and the 90% rise time was 1.8 seconds. I was only able to take 9 readings since I tried running the robot from very close to the wall, but I think 9 readings should be sufficient data to develop a Kalman filter.

TOF sensor readings

Velocity of the robot calculated from TOF and time

The topspeed of the robot was calculated to be 1304 mm/s or 1.3 m/s. The 90% rise time was 1.8 seconds. I calculated the d parameter as 1/topspeed and the m parameter using the risetime calculation.

topspeed=-1304 ##(mm/s)
risetime= 1.8 ##s
d = 1/topspeed

m = -d*risetime/np.log(0.1) 

A = np.array([  [0,1]   , 
                [0, -d/m]])
B = np.array([  [0] , 
                [1/m]])

##A= array([[ 0.        ,  1.        ],
##       [ 0.        , -1.27921394]])

## B= array([[   0.        ],
##      [-1668.09497848]])

## C=array([[-1,0]])

Kalman Filter sanity check

I implemented the Kalman filter using the equations given in the lab documentation page. After calculating my A, B and C matrices as shown in the above code cell, the next step was to discretize these matrices since we are dealing with limited number of readings.

delta_t=0.3125
A_d= [[1.         0.3125    ]
 [0.         0.60024564]]
B_d=[[  0.        ]
 [521.27968078]] 
 

I used the TOF data from Lab 6 PID and then tried to implement a Kalman filter on the data. I played with the different sigma values to get a good plot of the Kalman filter readings. sigma_z=(0.050.05) sigma_1=0.01

sigma_z=(0.050.05), sigma_1=0.1

sigma_z=(0.025*0.025), sigma_1=0.1

By changing the sigma_z values, I was able to change the amount to which my sensor readings were deemed accurate and basically how confident I was in those sensor readings. I found out the above combination of sigmas to yield the best KF output.

Implementation of Kalman filter on the robot

I implemented the Kalman filter using the manual given to us and then I updated the PID values according to the updated KF readings. The video below shows the implementation of Kalman filter on the robot.

Conclusion

In this lab, I implemented a Kalman filter which can give quicker readings and quicker update time for the sensors. This can mean that our robot can be quicker. I was only using a P controller in Lab 6 which used to oscillate a lot but by adding the Kalman filter you can see that the car doesnt oscillate as much and quickly settles to the Setpoint. Kalman filter can thus be a very useful tool in Fast Robots.