Add Kalman filter from chapter 3, exercise 1 and 2