# SDC Homework 3 - Kalman Filter

## Part 1 : warm-up
### Environment Setup
In the following homeworks , we would use container to finish courseworks to prevent package dependancy or environment version issues. Please make sure to have some basic knowledge of Docker.

**Prerequisite**
- Docker version >= 19.03
([Download Docker Engine in your computer](https://docs.docker.com/engine/install/ubuntu/))
- Make sure **~/catkin_ws/src** existing
- Place **sdc_hw3/** into **~/catkin_ws/sdc_hw3**

**Using pre-built images**

We wrap ROS noetic, Python 3.8 and the package we use this time e.g. NumPy, imageio into pre-built images. Please follow two commands to enter the environment.
- Main Terminal

xhost +local:

docker run \
-it \
--env="DISPLAY" \
--env="QT_X11_NO_MITSHM=1" \
--volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
-p 2233:22 \
--rm \
--name ros \
--user root \
-e GRANT_SUDO=yes \
-v ~/catkin_ws:/root/catkin_ws \
softmac/sdc-course-docker:latest \
bash

- Enter container in other terminals

docker exec -it ros bash 

You can try out Rviz and tutorials in hw1 inside the container.

You don't need to hand in anything for Part 1. Please do make sure to get familiar with Docker.

## Part 2

### Objective
In this homework, you will need to understand and implement Kalman filter. Notice that you can only use Numpy and the python standard libraries, other packages(ex: filterpy) are not allowed.

### Requirements
1. Implement Kalman filter with robot state [x, y, yaw].
2. Plot the outputs of Kalman filter and measurements.


### Implementation Details
- Description
Inputs generation and plotting functions are provided in the sample code. You will only need to complete the algorithm in `kalman_filter.py`.


Inputs are given as:
Control term (u): displacement of robot and yaw change [delta_x, delta_y, delta_yaw] (with 0 mean, 0.1 variance Gaussian noise added to delta_x, delta_y, and delta_yaw, respectively)

Measurement (z): Position of [x, y] (with 0 mean, 0.75 variance added to x and y, respectively).

- Finish the red blocks
- To test your algorithm

    python3 Kalman2D.py

---

### Report
1. Introduction.
2. Briefly explain your code.
3. Include the output image.
4. How you design the covariance matrices(Q, R)?
5. How will the value of Q and R affect the output of Kalman filter?

### Hand In
Please zip `kalman_filter.py` and your report (**<student_id>_hw3.pdf**) and upload to new e3.

### Preview the next homework (sdc_hw4)
We will annouce sdc_hw4 next week, and sdc_hw4 will utilize the Kalman Filter algorithm you've done in sdc_hw3 with NuScenes dataset to do radar odometry.

Please make sure that start homework early as you can.

### Schedule
- sdc_hw3 : due day **10/20** (2 weeks)
- sdc_hw4 : due day **10/26** (2 weeks)