Estimating Drag and Momentum

In order to estimate the drag (d) and momentum (m), I ran the robot at 100 PWM (the max PWM from lab 6) for 4m. I collected timestamped distance data from the ToF during the run and used that to calculate the velocity.

In class we learnt:

From the graph I found the steady state velocity to be 1500mm/s. Taking u to be 1, we get

I then found the 90% rise time to be 1.5s which gives us

Using these values we can find our A and B matrices

Initialising Kalman Filter

Using a sampling time of 0.065s (time between first two samples) I discretized the A and B matrices. I initialised the C matrix to be [-1, 0] since only ToF data is used and negative distance is measured. The state vector was initialised with the initial distance from the wall and 0 velocity as it was stationary.

I initialised the covariance matrices for the process Σ_u and sensor Σ_z noise. σ₃ was given to us and the formulas for the trust in the modelled position and speed were used for σ₁ and σ₂.

Implementing and Testing in Jupyter

I ran the Kalman filter on Jupyter to check that my parameters were correct. I used the code provided and my data from lab 6.

Initial test: σ₁ = σ₂ = 39 and σ₃ = 20

To my surprise it worked extremely well (so surprisingly well that I really thought I must have done something wrong so I went through and checked my code and calculations multiple times), the predicted values were very close to the measured values. I think they match so well because the sensor uncertainty is not very high, we can see that the KF steps when the sensor readings step although it also does predict value in between. I experimented with different sigma values because I was curious about how the filter would be affected. I noticed that higher σ₁ and σ₂ or lower σ₃ made the KF prediction match the measured data better. This makes sense because by doing this we are putting more trust in our sensor data.

σ₁ = σ₂ = 10 and σ₃ = 100

σ₁ = σ₂ = 1 and σ₃ = 20

Kalman Filter on Robot

The goal was to recreate the task in lab 6 (speed towards a wall and stop 1 foot in front of it) using the KF so that it would be faster. I increased my kp to 0.06 (the speed of the KF allowed for this). I also increased my minimum PWM from 40 to 50 since for some reason my robot was no longer moving at 40 PWM input even though the battery was fully charged. This worked incredibly well with very little overshoot and quite precisely stopping at 1 foot.