Solved 16-833: Robot Localization and Mapping Homework 2 - SLAM using Extended Kalman Filter

16-833: Robot Localization and Mapping Homework 2 - SLAM using Extended Kalman Filter (EKF-SLAM)
Theory (40 points) In this part we are going to guide you through some steps of EKF-SLAM in math. This will be helpful in your implementation in the next problem. A robot is moving on the 2D ground plane. In each time step t, the robot is controlled to move forward (the x-direction of the robot’s coordinates) dt meters, and then rotate αt radian. The pose of the robot in the global coordinates at time t is written as a vector pt = h xt yt θt i> , where xt and yt are the 2D coordinates of the robot’s position, and θt is the robot’s orientation. 1. Based on the assumption that there is no noise or error in the control system, predict the next pose pt+1 as a nonlinear function of the current pose pt and the control inputs dt and αt . (5 points) 2. However, in reality there are some errors when the robot moves due to the mechanism and the terrain. Assume the errors follow Gaussian distributions: ex ∼ N (0, σ2 x ) in x-direction, ey ∼ N  0, σ2 y  in y-direction, and eα ∼ N (0, σ2 α ) in rotation respectively (all in robot’s coordinates). For details, please see Fig. 1. Now if the uncertainty of the robot’s pose at time t can be represented as a 3-dimensional Gaussian distribution N (0, Σt), what is the predicted uncertainty of the robot at time t+1? Please express it as a Gaussian distribution with zero mean. (5 points) 3. Consider a landmark l being observed by the robot at time t with a laser sensor which gives a measurement of the bearing angle β (in the interval (−π, π]) and the range r, with noise nβ ∼ N  0, σ2 β  and nr ∼ N (0, σ2 r ) respectively. Write 1 Figure 1: 0) Robot state; 1) robot moves dt along its x-axis; 2) due to noise it shifted ex and ey along its x and y axises respectively; 3) robot rotates αt after shifting; 4) due to noise the rotation is disturbed by eα. down the estimated position (lx, ly) of landmark l in global coordinates as a function of pt , β, r, and the noise terms. (5 points) 4. According to 1.3, if we now know that l is at (lx, ly) in the global coordinates, please predict the measurement of bearing and range based on lx, ly, pt , and the noise terms (use functions np.arctan2 (.) and warp2pi(.) that warps an arbitrary angular value into the range (−π, π] if needed). (5 points) 5. An important step in EKF-SLAM is to find the measurement Jacobian Hp with respect to the robot pose. Please apply the results in 1.4 to derive the analytical form of Hp (Note: Hp should be a 2 × 3 matrix represented by pt and l). (10 points) 6. For each measurement of bearing and range, we also need a measurement Jacobian Hl with respect to its corresponding landmark l. Please again derive the analytical form of Hl (Note: Hl should be a 2 × 2 matrix represented by pt and l). Why do we not need to calculate the measurement Jacobian with respect to other landmarks except for itself (Hint: based on what assumption)? (10 points) Note in this section, in addition to conventional linearization with additive noise (in the form of xt+1 = g(xt , ut) + t), you may encounter non-additive noise (in the form of xt+1 = g(xt , ut , t)). Derivation of the linearization for this formulation can be helpful. Also, all the noise reading (, n) can be regarded as infinitismal. Second and higher order terms comprised by their products can be ignored. 2 Implementation and Evaluation (45 points) In this part you will implement your own 2D EKF-SLAM solver in Python. The robot in problem 1 starts with observing the surrounding environment and mea2 suring some landmarks, then executes a control input to move. The measurement and control steps are repeated several times. For simplicity, we assume the robot observes the same set of landmarks in the same order at each pose. We want to use EKF-SLAM to recover the trajectory of the robot and the positions of the landmarks from the control input and measurements. 1. Find the data file data/data.txt that contains the control inputs and measurements. Open the file to take a look. The data is in the format of measurements= h β1 r1 β2 r2 · · · i , where βi , ri correspond to landmark li , and control= h d α i (refer to problem 1 for the notations). The lines are in sequential time order. What is the fixed number of landmarks being observed over the entire sequence? (5 points) 2. The code/ folder contains the Python code file ekf slam.py. Follow the comments and fill in the functions warp2pi, init landmarks, predict, update to enable the system. The visualization will show the trajectory and the landmarks with their covariances in the shape of 3-σ ellises. It will be updated sequentially, and you may process the next step by clicking your mouse or hitting any key. Attach a clear figure of your visualization in the write up once all the steps are finished. (25 points) 3. In the output figure, the magenta and blue ellipses represent the predicted and updated uncertainties of the robot’s position (orientation not shown here) at each time respectively. Also, the red and green ellipses represent the initial and all the updated uncertainties of the landmarks, respectively. Describe how EKF-SLAM improves the estimation of both the trajectory and the map by comparing the uncertainty ellipses. (5 points) 4. Now let’s evaluate our output map. Suppose we have the ground truth positions of all the landmarks: h l > 1 l > 2 · · · l > k i = h 3 6 3 12 7 8 7 14 11 6 11 12 i . Plot the ground truth positions of the landmarks in the output figure (code already provided in function evaluate) and attach it below. Is each of them inside the smallest corresponding ellipse? What does that mean? Compute and report the Euclidean and Mahalanobis distances of each landmark estimation with respect to the ground truth. What do the numbers tell you? (10 points) 3 Discussion (15 points) 1. Explain why the zero terms in the initial landmark covariance matrix become non-zero in the final state covariance matrix (print out the final P matrix to check it). Additionally, when setting the initial value for the full covariance matrix P (line 201 in ekf slam.py) an assumption is made regarding certain cross-covariances that is not necessarily correct. Can you point out what that is? (5 points) 3 2. Play with the parameters (line 163-167). Modify each parameter σx, σy, σασb, σr at a time and fix the others. In particular, you should set each parameter to be 10 times larger/smaller to the original parameters to discuss how each parameters influence the results. Attach figures for better explanation. (10 points) 3. With the same set of landmarks being observed all the time, the EKF-SLAM system runs in constant time in each iteration. However, for real-world problems, the total number of landmarks can grow unbounded if the robot keeps exploring new environments. This will slow down the EKF-SLAM system a lot and become a crucial problem for real-time applications. What changes can we make to the EKF-SLAM framework to achieve constant computation time in each cycle or at least speed up the process (list as many possible solutions as you can think of)? (Bonus 5 points) 4 Code Submission Upload your ekf slam.py. Please do not upload the data/ folder or any other data or image. 4