# SDC Homework 4 - Application of Kalman Filter
###### tags: `2022 SDC course`

## Prerequisite
Please refer to [hw3/Environment Setup](https://hackmd.io/@Jerry8137/HkbTCXjMo)

## ❗❗ Sample Code Correction (10/19)
in `src/fusion.py`
**Line 66** inside `odometryCallback` should change to:

self.KF.Q[:2, :2] = ???

**Line 93** inside `gtCallback` should change to:

gt_position = np.array([data.pose.pose.position.x, data.pose.pose.position.y])
self.gt_list.append(gt_position)
if self.KF is not None:
    kf_position = self.KF.x[:2]
    self.est_list.append(kf_position)

## Get Started
### Objective
In this homework, you are asked to implement the Kalman filter you built in homework 3 on a real data and then visualize it with rviz. You can modify the **template** provided in **Package section** to complete the homework.

### Localization
In the SDC localization task, Kalman filter is a tool to fuse information from different sensors. In this homework, we use **radar odometry** and **GPS** with covariance. **Radar odometry** provides position and heading overtime, which is obtained from radar sensor. **GPS** provides position overtime. Noted that the frequency of sensors are different.

### Data
- A rosbag from NuScenes Dataset ([Download](https://drive.google.com/file/d/1BaXcNDqouajZkWEhRYTJiWbrpbRp6VLx/view?usp=sharing))
- Play the bag with the following command
`rosbag play sdc_hw4_noetic.bag --clock`

The `--clock` argument is for overwriting ROSTime with the time when the bag is recorded. If you play a bag with this command, remember to set a rosparam of use_sim_time by the following command.
`rosparam set use_sim_time true`
This command is covered in the `KF_fusion.launch` launch file this time.

- Topic

**/radar_odometry** is the **local measurement** which have accumulated error.

**/gps** is the **global measurement** which only provides x,y position at 1Hz.

### Package
[KF_Fusion_pkg.zip](https://drive.google.com/file/d/1czBDukmzhEfjFPTLPyKWCyTCkCOKHdem/view?usp=sharing)

Includes
- `KF_fusion.launch`
- `rviz_config.rviz`
- `fusion.py` (To be done)

To run the code after you finish it,
1. Make sure `~/catkin_ws/src` existing
2. Unzip the package and put the package in `catkin_ws/src/`
3. `catkin_make`
4. `source devel/setup.bash`
5. `roslaunch kalman_filter KF_fusion.launch`
6. New terminal in data path: `rosbag play sdc_hw4.bag --clock`

### Useful Command
- To visualize: `rviz`
- rosbag: https://wiki.ros.org/rosbag/Commandline
- roslaunch: https://wiki.ros.org/roslaunch
- `rotopic list`, `rostopic echo /topic_name`
https://wiki.ros.org/rostopic
- ROS odometry msg structure: http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html
- ROS pose msg structure: http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Pose.html

### FAQ & Hint
> Q: Covariance沒有如影片中顯示, 隨著KF的Predict顯著的放大?
Hint: 先試著估測 [x, y, yaw]
Hint: 再試著去比較估6個維度狀態 [x, y, yaw, dx, dy, dyaw], 試著調整x, A, B, H的維度

### Demo video
[Video](https://youtu.be/loTOlVbimGU)
{%youtube loTOlVbimGU %}

### Discussion

1. How do you design the Kalman filter and the parameters.
2. What is the **covariance** matrix of **GPS**, **radar odometry** and what does it mean?

### Submission
<!-- Please zip fusion.py, kalman_filter.py and your report (<student_id>_hw4.pdf) and upload it to new e3. -->
Please Hand-in The Following files:

1. Result and Discussion

- Screenshot of Kalman Filter Fusion Results Along with Your Personal Information (terminal name) (**60%**)
- Discussion Answers (**40%**)

2. Source code
- `fusion.py`
- `Kalman_filter.py`

**Pack all files in a zip file: <student_id>_hw4.zip
Daeadline: 10/26 23:59**