************************************ Assignment #1 - Tracking an Airplane ************************************ * **Worth**: 10% * **DUE**: March 13th at 11:55pm; submitted on MOODLE. .. warning:: For this assignment, you may **not** work with anyone else. You need to work independently. In this assignment you will solve your first nonlinear problem. You will write a filter to track an airplane using a radar as a sensor. 2D problem ========== To stay simple, you will consider a 2D problem first: * The position relative to the ground * The altitude. Problem modelisation -------------------- Radars work by emitting radio waves or microwaves. Anything in the beam's path will reflect some of the signal back to the radar. By timing how long it takes for the reflected signal to return, it can compute the *slant distance* to the target. Slant distance is the straight-line distance from the radar to the object. Bearing is computed using the *directive gain* of the antenna. We compute the (x,y) position of the aircraft from the slant distance and elevation angle as illustrated by this diagram: .. figure:: ../pyplots/ASN1_Radar.png :align: center The elevation angle :math:`\epsilon` is the angle above the line of sight formed by the ground. You will assume that the aircraft is flying at the constant altitude. Thus we have a three variable state vector: .. math:: \mathbf x = \begin{bmatrix}\mathtt{distance} \\\mathtt{velocity}\\ \mathtt{altitude}\end{bmatrix}= \begin{bmatrix}x \\ \dot x\\ y\end{bmatrix} The state transition function is linear: .. math:: \mathbf{\bar x} = \begin{bmatrix} 1 & \Delta t & 0 \\ 0& 1& 0 \\ 0&0&1\end{bmatrix} \begin{bmatrix}x \\ \dot x\\ y\end{bmatrix} 1. Implement the transition function: .. code-block:: python def f_radar(x, dt): """ state transition function for a constant velocity aircraft with the state vector [distance, velocity, altitude]""" After you need to design the measurement function. Remember that the measurement function converts the filter's prior into a measurement. You need to convert the position and velocity of the aircraft into elevation angle and range from the radar station. You need to calculate the slant range and the elevation angle. It is not very hard it just basic trigonometry. 2. Implement the measurement function: .. code-block:: python def h_radar(x, radar_pos): """Measurement function, return a list [slant_range, elevation_angle] """ Radar and Aircraft Simulator ---------------------------- Now you need to simulate the radar and the aircraft. 3. Implement the following two classes: .. code-block:: python class RadarStation: def __init__(self, pos, range_std, elev_angle_std): pass def reading_of(self, ac_pos): """ Returns (range, elevation angle) to aircraft. Elevation angle is in radians. """ def noisy_reading(self, ac_pos): """ Compute range and elevation angle to aircraft with simulated noise""" class ACSim: def __init__(self, pos, vel, vel_std): def update(self, dt): """ Compute and returns next position. Incorporates random variation in velocity. """ Implementing the filter ----------------------- Now you need to implement the UKF. Don't worry it is something that you have already done before, just not in a class. I am giving you the structure of the class: .. code-block:: python class UKF: """ Implements the Scaled Unscented Kalman filter (UKF). This filter scales the sigma points to avoid strong nonlinearities. Parameters ---------- dim_x : int Number of state variables for the filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4. dim_z : int Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim_z would be 2. This is for convience, so everything is sized correctly on creation. If you are using multiple sensors the size of `z` can change based on the sensor. Just provide the appropriate hx function dt : float Time between steps in seconds. hx : function(x,**hx_args) Measurement function. Converts state vector x into a measurement vector of shape (dim_z). fx : function(x,dt,**fx_args) function that returns the state x transformed by the state transition function. dt is the time step in seconds. radar_pos: radar position Attributes ---------- mu : numpy.array(dim_x) state estimate vector Sigma : numpy.array(dim_x, dim_x) covariance estimate matrix mup : numpy.array(dim_x) Prior (predicted) state estimate. The *_prior and *_post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only. Sigmap : numpy.array(dim_x, dim_x) Prior (predicted) state covariance matrix. Read Only. z : ndarray Last measurement used in update(). Read only. Q : numpy.array(dim_z, dim_z) measurement noise matrix R : numpy.array(dim_x, dim_x) process noise matrix K : numpy.array Kalman gain y : numpy.array innovation residual """ def __init__(self, dim_x, dim_z, dt, hx, fx, radar_pos): """ Create a Kalman filter. You are responsible for setting the various state variables to reasonable values; the defaults below will not give you a functional filter. """ def predict(self): """ Performs the predict step of the UKF. On return, self.mup and self.Sigmap contain the predicted state (mu) and covariance (Sigma). 'p' stands for prediction. """ def update(self, z): def MerweWeightSigmaPoints(self, alpha=.1, beta=2., kappa=0.): def SigmaPoints(self): def unscented_transform(self, sigmas, cov): After that you need to execute the following code: .. code-block:: python import math import random dt = 3. # 12 seconds between readings range_std = 5 # meters elevation_angle_std = math.radians(0.5) ac_pos = (0., 1000.) ac_vel = (100., 0.) radar_pos = (0., 0.) kf = UKF(3, 2, dt, fx=f_radar, hx=h_radar, radar_pos=radar_pos) kf.R[0:2, 0:2] = np.array([[2.025, 1.35 ], [1.35 , 0.9 ]]) kf.R[2,2] = 0.1 kf.Q = np.diag([range_std**2, elevation_angle_std**2]) kf.mu = np.array([0., 90., 1000.]) kf.Sigma = np.diag([300**2, 30**2, 150**2]) np.random.seed(200) pos = (0, 0) radar = RadarStation(pos, range_std, elevation_angle_std) ac = ACSim(ac_pos, (100, 0), 0.02) time = np.arange(0, 360 + dt, dt) xs = [] for _ in time: ac.update(dt) r = radar.noisy_reading(ac.pos) kf.predict() kf.update([r[0], r[1]]) xs.append(kf.mu) plot_radar(xs, time) If everything is correct you should obtain something similar: .. figure:: ../pyplots/ASN1_part_1.png :align: center .. figure:: ../pyplots/ASN1_part_1_2.png :align: center .. figure:: ../pyplots/ASN1_part_1_3.png :align: center .. note:: To plot the graph you need to follow function. .. hidden-code-block:: python def plot_radar(xs, t, plot_x=True, plot_vel=True, plot_alt=True): xs = np.asarray(xs) if plot_x: plt.figure() plt.plot(t, xs[:, 0]/1000.) plt.xlabel('time(sec)') plt.ylabel('position(km)') plt.tight_layout() if plot_vel: plt.figure() plt.plot(t, xs[:, 1]) plt.xlabel('time(sec)') plt.ylabel('velocity') plt.tight_layout() if plot_alt: plt.figure() plt.plot(t, xs[:,2]) plt.xlabel('time(sec)') plt.ylabel('altitude') plt.tight_layout() plt.show() Tracking Maneuvering Aircraft ============================== If you look at the results the filter is working very well, but we assumed that the airplane is not changing altitude. If we apply the same filter with an airplane that is climbing, we obtain the following: .. code-block:: python kf.x = np.array([0., 90., 1100.]) kf.P = np.diag([300**2, 30**2, 150**2]) ac = ACSim(ac_pos, (100, 0), 0.02) np.random.seed(200) time = np.arange(0, 360 + dt, dt) xs, ys = [], [] for t in time: if t >= 60: ac.vel[1] = 300/60 # 300 meters/minute climb ac.update(dt) r = radar.noisy_reading(ac.pos) ys.append(ac.pos[1]) kf.predict() kf.update([r[0], r[1]]) xs.append(kf.mu) plot_altitude(xs, time, ys) .. figure:: ../pyplots/ASN1_part_2_0.png :align: center The state and the transition function need to be changed to take into account the climbing rate: .. math:: \mathbf x = \begin{bmatrix}\mathtt{distance} \\\mathtt{velocity}\\ \mathtt{altitude} \\ \mathtt{climb\, rate}\end{bmatrix}= \begin{bmatrix}x \\\dot x\\ y \\ \dot y\end{bmatrix} .. math:: A = \begin{bmatrix} 1 & \Delta t & 0 &0 \\ 0& 1& 0 &0\\ 0&0&1&\Delta t \\ 0&0&0&1\end{bmatrix} \begin{bmatrix}x \\\dot x\\ y\\ \dot y\end{bmatrix} 4. Implement the new transition function: .. code-block:: python def f_cv_radar(x, dt): """ state transition function for a constant velocity aircraft""" 5. You need to adapt the filter: .. code-block:: python def cv_UKF(fx, hx, Q_std): # Implement the function here... return kf * First create a UKF * Secondly set R to the correct value (provided below) * Then, you need to create Q. Remember that you only have a float representing the **std**, but you want the **variance** as a matrix. * Set :math:`\mu` and :math:`\Sigma` (provided below) .. code-block:: python kf.R[0:2, 0:2] = np.array([[2.025, 1.35 ], [1.35 , 0.9 ]]) kf.R[2:4, 2:4] = np.array([[2.025, 1.35 ], [1.35 , 0.9 ]]) mu = np.array([0., 90., 1000., 0.]) Sigma = np.diag([300**2, 3**2, 150**2, 3**2]) Now you can test it with the following code: .. code-block:: python np.random.seed(200) ac = ACSim(ac_pos, (100, 0), 0.02) kf_cv = cv_UKF(f_cv_radar, h_radar, R_std=[range_std, elevation_angle_std]) time = np.arange(0, 360 + dt, dt) xs, ys = [], [] for t in time: if t >= 60: ac.vel[1] = 300/60 # 300 meters/minute climb ac.update(dt) r = radar.noisy_reading(ac.pos) ys.append(ac.pos[1]) kf_cv.predict() kf_cv.update([r[0], r[1]]) xs.append(kf_cv.mu) plot_altitude(xs, time, ys) .. note:: If you want to plot if you need the following code: .. hidden-code-block:: python def plot_altitude(xs, t, track): xs = np.asarray(xs) plt.plot(t, xs[:,2], label='filter', ) plt.plot(t, track, label='Aircraft', lw=2, ls='--', c='k') plt.xlabel('time(sec)') plt.ylabel('altitude') plt.legend(loc=4) You should obtain something similar: .. figure:: ../pyplots/ASN1_part_2_1.png :align: center How to submit your work ======================= You need to submit your unique python (.py) file directly on moodle. The first two lines of the python file should be as follows: .. code-block:: python # Student name: # Student number: