Kalman Filter and Particle Filter Based Pose Estimation for Differential Drive Robot
In this implementation, a Kalman filter and then a particle filter was used to estimate the pose of a differential drive robot. The kinematic model of the robot as well as ground truth data, it’s corresponding timestamps, control inputs and GPS inputs were available for estimation.
The ground truth data along with the corresponding timesteps were used to first estimate the noise in the process model (kinematic model mapping controls to the state update). Since the recorded control inputs were noise free, they were used along with the kinematic model to extract the deviation for each time-step and the mean and covariance of the process noise was estimated. Following this, the noise for the GPS measurements were estimated by comparing the measurements to the ground truth in a similar manner. The mean and covariance for these measurements were also extracted. For comparison, the control inputs were used as is to estimate robot trajectory through dead reckoning. The result of this is shown below.
As can be seen, the estimated trajectory is way off from the ground truth.
An Extended Kalman filter was then implemented using the process and measurement covariance matrices. The kinematic model was first used to estimate the state update at every time-step. At every tenth time-step – which was the frequency of the measurement input – the predicted state was fused with the GPS measurement through the Kalman update step. The estimation using the filter is shown below. The green points represent inputs from the GPS measurements. These measurements were fused with the odometry to generate the trajectory shown in blue which is much better than the dead reckoner trajectory shown in red.
In the second part of the assignment, a particle filter was implemented in order to estimate the trajectory of a robot moving between two beacons. The measurement inputs in this case were distance measurements from the robot to two fixed beacons located in the environment. At each time-step, the kinematic model was used to calculate the change in state after which the particle distribution was weighted and sampled to reflect the confidence in the position estimate.
The filter was initialized with 10,000 particles uniformly distributed throughout space.
The re-weighting step was performed using a product of Gaussians where the difference in the expected beacon measurements and the actual measurements were used to calculate the new weights. The particles were then re-sampled based on the new weights and the state update applied to each particle. Over time, the non-representative particles die off with their weights tending to zero, while the particles of higher likelihood converge to the actual state of the robot. This animation is shown below. The weighted mean of the top 1000 particles was used to represent the estimated state of the robot.
All code related to the assignment is maintained in a private Github repo in order to honor academic integrity.