[Solved] CS440 Machine Problem5-2D Kalman Filter and an Application

Problem 1 . 2D Kalman Filter and an Application.

1. a) Implementation of a 2D Kalman filter . You are to implement a (relatively simple) 2D Kalman filter. Suppose that there is a point mass moving in 2D with the system equation being

Assume that the system noise w is a zero mean Gaussian with the covariance matrix

and the measurement noise is a zero mean Gaussian with the covariance matrix

(10^-2 5 x 10^-3

R = 5 x 10^-3 2 x 10^-2 )

a.1 To start, derive the required update equations for the 2D Kalman filter. For this task, you may simply write down the equations and provide the needed term with brief explanation of how these are obtained. This should go into your report.

a.2 In the provided main.py file, the function _load_data() loads a set of 2D data with each item in the list being a list of the form [u_i,k__i, u₂,k_₁, z1,k, z2,k]. You are to implement the equations you derived earlier in

kalman2d (data)

that computes the estimated Xi*, X2_,k Your function should return a list of list, e.g., in the form of

[[x1,1, x2,1], , [x₁,k,x₂,k]]

Your function will be tested on new data, with the same xk, zk, Q and R, to check its correctness. Note that you will need to fix the initial P matrix, which should be of the form of AI where A is a scaling factor and I is the 2D identity matrix.

a.3 Implement the function plot (data, output)

using matplotlib that plots both the observed 2D points as well as the estimated ones. That is, for the set of _(zl,k, z₂,k) in the data, plot them and connect consecutive data points using straight line segments. Do the same for the estimated _(x1,k, x₂,k) on the same plot. Draw the observations with a blue line and the estimates with a red line.

a.4 Report what value of P = AI that you found to work well for the given data. Also, include a plot produced by your plotting function for the included data set.

The data that is included is also listed in the table at the end of this document. As mentioned, your program should work for data that is different from this provided data set.

1. b) An application of a 2D Kalman filter. Suppose you are to shoot a cloaked target in a 2D region of 500 x 500 (pixels). You may shoot a laser anywhere along the bottom of the region and while the laser has a short burst, is quite powerful and travels through the region vertically At the same time, the laser uses lots of energy which means you only have a single shot. While the target is cloaked, as it moves, it sends energy signatures that allows the prediction of its next move. After the move is complete, the target also sends a signal that can be tracked with some Gaussian noise. Your goal is to track the target as it moves and decide when and where (alone the base) to fire the laser. If you enable the part of the code in the main function in main . py, the program will draw the observed target.

b.1 In this problem, you are to implement the function kalman2d_shoot (ux , uy, , ox, oy, , reset)

that takes the current _{ul,k_1,} u₂,k_₁, _z1,k, Z2_,k values and based on your estimate, decide whether to fire the laser and where. The argument reset is set to true when the function is called the first time in a new iteration (i.e., you should in such case reset your internal states). Note that in the python code we use x and y to represent the two coordinates instead of using xi and x2, to make it easy to represent the two coordinates. The return value for the function is a 3-tuple of the format

(ex, ey, shoot)

where ex and ey are the estimated x and y location and shoot, a boolean variable, indicates whether

the laser should be fired (at ex). Whenever shoot is set to be true when returning, the program will stop and show the laser and the actual target location. On the console screen, the relevant information will also be displayed. You are to fire a shot within 200 iterations, which is about 40 seconds since the system update is performed at around 5Hz. The absolute error in the x coordinate, which will be averaged over about 50 runs, should be below 8 (pixels) to get 80% of the credits. For full credits, an accuracy of 6 pixels is needed. A screenshot is provided in the figure below showing what you would see after firing the shot.

ghost hunt

Note that your program will be tested along five trajectories; the provided trajectory is only one of them. They will have the same noise settings.

b.2 Note that you may do anything you want in kalman2d_shoot, i.e., unlike in kalman2d, you are not limited to only using Kalman filter in making your decision in kalman2d_shoot. In the report, state how you decide when to shoot.

b.3 Finally, implement another version of your shooting routine in kalman2d_adv_shoot (ux , uy, , ox, oy, , reset)

which will be evaluated based on the time that has passed and the accuracy. For each trial, let the time passed before you decide to shoot be t and the absolute error in x be e_x, then the score for the trial (you must make a decision within 200 iterations as before) is given by

ex

score = t e 2 .

Your goal is to minimize this score; the code will be subject to the same 50 tests used for evaluating kalman2d_shoot and the average will be taken. The top 10 teams will be awarded 7 4 4 4 2 2 2-2-2-2 bonus points with better performing implementation given higher score.

Table 1: Sample control input and observations for running the Kalman filter.