CS401 Homework 7 Robot Localization-Solved

You will be implementing a 2-Dimensional Kalman Filter for constant velocity model and particle filter for 2D robot localization using measurements from landmarks. Corresponding code skeltons are provided for you to implement the filter. Also, the visulization of the two filters are provided in the framework.

2D Kalman Filter with Constant Velocity Model
A simulated scenario where we consider a robot in 2D and use odometry for prediction and mocked GPS measurement for evaluation.

motion model and obervation motion model.
Velocity-based 2D robot motion model
This is a sensor fusion localization with Particle Filter(PF).

The blue line is true trajectory, the black line is dead reckoning trajectory,

and the red line is estimated trajectory with PF.

In a 2D map with distributed landmarks, we assume that the robot can measure a distance from landmarks,

This measurements are used for PF localization.

Suppose the robot has a state vector includes 3 states at time  :

Meanwhile, the robot can obtain the velocity and orientation information, and their can be used as input vector at each time step:

Thus, the robot motion model is:
where and   is the zero-mean Gaussian process noise vector with covariance matrix Q.

Observation Model:
Also, the robot has a GPS sensor, it means that the robot can observe x, y position at each time:

where,   and   is the zero-mean Gaussian observation noise with variance R.

Implement the kalman filter
If the kalman filter design is complete, we just have to write the code to run the filter and output the data in the format of our choice. The initial uncertanty for the motion noise and observation noise are defined in the code. The comparisons between the odometry, GPS, filter and ground truth are shown in the following figure:

Particle filter 2D localization
In a 2D map with some landmarks, a robot moves with velocity-based motion model and can measure its relative distances from nearby landmarks. You will use the particle filter algorithm to locate the robot based on the distance measurements.

Generic Particle Filter Algorithm
Randomly generate a bunch of particles: particles can have position, heading, and/or whatever other state variable you need to estimate. Each has a weight (probability) indicating how likely it matches the actual state of the system. Initialize each with the same weight.
Predict next state of the particles: Move the particles based on how you predict the real system is behaving.
Update: Update the weighting of the particles based on the measurement. Particles that closely match the measurements are weighted higher than particles which don't match the measurements very well.
Particle Resampling: Discard highly improbable particle and replace them with copies of the more probable particles.
Compute Estimate: Optionally, compute weighted mean and covariance of the set of particles to get a state estimate.
You can refer to Tutorial of particle filter  for better understanding and python implementation of particle filter.

This is a sensor fusion localization with Particle Filter(PF).

The blue line is true trajectory, the black line is dead reckoning trajectory,

and the red line is estimated trajectory with PF.

It is assumed that the robot can measure a distance from landmarks (RFID).

This measurements are used for PF localization.

 
Task 1
Please derivate the predcition and update process of kalman filter.

Task 2
Please implement the "predict" and "update" function in the code kalman-filter.py

Task 3
Please implement the "re-sampling" and "weight calculation" parts in particle_filter.py

Note for tasks
The covariance matrix from particles is calculated as following: