{ "cells": [ { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "%pylab inline\n", "from gps_helper import gps_helper as GPS\n", "from IPython.display import Image, SVG" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "pylab.rcParams['savefig.dpi'] = 100 # default 72\n", "%config InlineBackend.figure_formats=['svg'] # SVG inline viewing" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Kalman Filter Variables" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "Image('figs/Kalman_variables.png',width='60%')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# General Extended Kalman Filter (EKF) Block Diagram" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "Image('figs/EKF_Filter.png',width='70%')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Application to GPS using Simulated User and Satellite Trajectories \n", "To make a realistic and flexible simulation environment we use a User trajectory generator that converts East-north-up (ENU) coordinates into earth-centric earth fixed coordinates (ECEF) (see details of ECEF in a later figure). A conversion from [ENU to ECEF](http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates) is also required.\n", "\n", "The GPS satellites are in a medium earth orbit and hence have significant motion during most *User* tracking error experiments. To provide a realistic trajectory model we use the Python package [SGP4](https://pypi.python.org/pypi/sgp4/) along with two-line elements (TLEs) sets for the GPS satellites to provides the needed ECEF coordinates versus time. Since SGP4 delivers satellite ephemeris in earth centered interial coordinates, a conversion from ECI to ECEF is also required. The TLEs are obtain from [celestrak](https://www.celestrak.com/NORAD/elements/gps-ops.txt)." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "Image('figs/Earth_Centered_Inertial_Coordinate_System.png',width='50%')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### A Simple Example\n", "* Generate a user trajectory\n", "* Pair the trajectory with a collection of appropriately chosen GPS satellites keyed to the C/A code PRN number, e.g., PRN 21, etc." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Line segment User Trajectory\n", "rl1 = [('e',.2),('n',.4),('e',-0.1),('n',-0.2),('e',-0.1),('n',-0.1)]\n", "rl1" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Create a GPS data source\n", "GPS_ds1 = GPS.GPSDataSource('GPS_tle_1_10_2018.txt',\n", " rx_sv_list = \\\n", " ('PRN 32','PRN 21','PRN 10','PRN 18'),\n", " ref_lla=(38.8454167, -104.7215556, 1903.0),\n", " ts=1)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Populate User and SV trajectory matrices\n", "USER_vel = 5 # mph\n", "USER_Pos_enu, USER_Pos_ecf, SV_Pos, SV_Vel = \\\n", " GPS_ds1.user_traj_gen(route_list=rl1,\n", " vmph=USER_vel,\n", " yr2=18, # the 2k year, so 2018 is 18\n", " mon=1,\n", " day=15,\n", " hr=8+7,\n", " minute=45) # Jan 18, 2018, 8:45 AM" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Check a user position\n", "USER_Pos_ecf[0,:]" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "#### Generate a 3D View of the Trajectories" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": false }, "outputs": [], "source": [ "GPS.sv_user_traj_3d(GPS_ds1,SV_Pos,USER_Pos_ecf,ele=20,azim=-40)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "plot(USER_Pos_enu[:,0],USER_Pos_enu[:,1])\n", "plot(USER_Pos_enu[0,0],USER_Pos_enu[0,1],'g.')\n", "plot(USER_Pos_enu[-1,0],USER_Pos_enu[-1,1],'r.')\n", "title(r'User Trajectory in ENU Coordinates')\n", "xlabel(r'East (mi)')\n", "ylabel(r'North (mi)')\n", "grid();" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Develop a GPS EKF\n", "The constant velocity process model of [2] is adopted for this project. The step is defining the eight element state vector $\\mathbf{x}$:" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "Image('figs/Process_Model1.png',width='80%')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The EKF allows nonlinearities in both the process model and the measurement model. For the case of GPS the state transition model is linear, thus the first calculation of **Step 1**, *predicted state update expression*, is the same as that found in the standard linear Kalman filter. What is needed is a state transition matrix:" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "Image('figs/Process_Model2.png',width='80%')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The second part of **Step 1** is forming the predicted error covariance matrix from the previous error covariance matrix. This calculation on the process model covariance matrix, $\\mathbf{Q}$:" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "Image('figs/Process_Model3.png',width='80%')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Simulated Pseudo Range from the User and Satellite Trajectories\n", "The EKF will use satellite epheneris data from the 50 bps message sent from each satellite. The pseudo range to each satellite is obtained from the cross correlation of the received C/A code for a given satellite the locally generated C/A for the satellite. Clock errors factored into the spedurange measurement." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "Image('figs/Measurement_Model1.png',width='70%')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "A class is created for obtaining the simulated pseudorange from the User and satellite trajectories. The associated update time is defined in the generation of the trajectories." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class GetPseudoRange(object):\n", " \"\"\"\n", " A class for generating simulated Pseudo range measurements from\n", " simulated SV ECEF and simulated User ECEF trajectories. The number of \n", " measurements is 4, but more or less measurements can be specified \n", " assuming the number of SVs configured matches.\n", " \n", " Mark Wickert January 2018\n", " \"\"\"\n", " \n", " \n", " def __init__ (self, PR_std = 0, CDt = 0, N_SV = 4):\n", " \"\"\"\n", " Initialize the object\n", " \"\"\"\n", " self.PR_std = PR_std\n", " self.CDt = CDt\n", " self.N_SV = N_SV\n", " self.USER_PR = zeros((N_SV,1))\n", " \n", " \n", " def measurement(self, USER_Pos_ecef, SV_Pos_ecef):\n", " \"\"\"\n", " Take a measurement by passing in position values at time\n", " index i, i.e., USER_Pos_ecf[i,:] & SV_Pos[:,:,i]\n", " \"\"\"\n", " # Compute the pseudo range to each SV\n", " for k in range(self.N_SV):\n", " self.USER_PR[k,0] = norm(USER_Pos_ecef - SV_Pos_ecef[k])\n", " # Add bias and measurement noise\n", " self.USER_PR[k,0] += self.CDt + self.PR_std*random.randn(1)[0]\n", " return" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## The EKF Class for Estimating Position from the Pseudorange Measurements\n", "Beyond the linear Kalman filter, the EKF needs to implement a linearization of the measurement equations. In particular we need $\\mathbf{H}$, which we get by forming the Jacobian of the measurement model." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "Image('figs/Measurement_Model2.png',width='70%')" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class GPS_EKF(object):\n", " \"\"\"\n", " GPS Extended Kalman Filter User Tracking using SGP4-based SV tracks in ECEF \n", " and ENU user tracks converted to ECEF so that simulated pseudo range \n", " measurements can be made. The overall formulation is based on the book by\n", " Robert Brown & Patrick Hwang, \"Introduction to Random Signals and Applied \n", " Kalman Filtering with MATLAB Exercises\", 4th edition, 2012.\n", " \n", " Python 3.x is assumed so the operator @ can be used for matrix multiply\n", " \n", " Mark Wickert January 2018\n", " \"\"\"\n", "\n", " \n", " def __init__ (self, USER_xyz_init, dt = 1, sigma_xyz = 5, \n", " Sf = 36, Sg = 0.01, Rhoerror = 36, N_SV = 4):\n", " \"\"\"\n", " Initialize the object\n", " \n", " The state vector x is composed of eight components:\n", " \n", " [x_pos, x_vel, y_pos, y_vel, z_pos, z_vel, clk_bias, clk_bias_rate].T\n", " \n", " Futhermore a constant velocity model is chosen for the process model\n", " \"\"\"\n", " self.dt = dt\n", " self.N_SV = N_SV\n", " # Create a block diagonal state transition matrix\n", " A_cv = array([[1, dt],[0, 1]]) # 2x2 block\n", " A_cvr = hstack((A_cv,zeros((2,N_SV+2)))) #block with zeros appended\n", " self.A = vstack((A_cvr,roll(A_cvr,2),roll(A_cvr,4),roll(A_cvr,6)))\n", " # Process model covariance Q [Brown & Hwang] is subblocks Qxyz and Qb \n", " Qxyz = sigma_xyz**2 * array([[dt**3/3, dt**2/2],[dt**2/2, dt]])\n", " Qb = array([[Sf*dt+Sg*dt**3/3, Sg*dt**2/2], [Sg*dt**2/2, Sg*dt]])\n", " # Create a block diagonal matrix to hold 2x2 blocks\n", " Qxyzr = hstack((Qxyz,zeros((2,6))))\n", " Qbr = hstack((Qb,zeros((2,6))))\n", " self.Q = vstack((Qxyzr,roll(Qxyzr,2),roll(Qxyzr,4),roll(Qbr,6)))\n", "\n", " # Measurement model covariance matrix R \n", " # Rhoerror = variance of measurement error(pseudorange error)\n", " self.R = Rhoerror*eye(N_SV) \n", " # Error covariance matrix initialize \n", " self.P = 10*eye(8)\n", " # Initialize state vector\n", " self.x = zeros((8,1))\n", " self.x[0:6:2,0] = USER_xyz_init\n", " self.x[1:7:2,0] = [0, 0, 0] # Initial velocity\n", " self.x[6,0] = 0 #3.575e6 # Initial c*clock bias in m\n", " self.x[7,0] = 0 #4.549e1 # Initial c*clock drift in m/s\n", " \n", " \n", " def update(self, z, SV_Pos):\n", " \"\"\"\n", " Update the Kalman filter state by inputting a new set of \n", " pseudorange measurements.\n", " Return the state array as a tuple.\n", " Update all other Kalman filter quantities\n", " Input SV ephemeris at one time step, e.g., SV_Pos[:,:,i]\n", " \"\"\"\n", " # H = Matrix of partials dh/dx\n", " H = self.Hjacob(self.x, SV_Pos)\n", " \n", " xp = self.A @ self.x\n", " Pp = self.A @ self.P @ self.A.T + self.Q\n", " \n", " self.K = Pp @ H.T @ inv(H @ Pp @ H.T + self.R)\n", " \n", " # zp = h(xp), the predicted pseudorange\n", " zp = self.hx(xp, SV_Pos)\n", " \n", " self.x = xp + self.K @ (z - zp)\n", " self.P = Pp - self.K @ H @ Pp\n", " # Return the x,y,z position (also held in the state vector attribute)\n", " return self.x[0,0], self.x[2,0], self.x[4,0]\n", " \n", " \n", " def Hjacob(self,xp,SV_Pos):\n", " \"\"\"\n", " Jacobian used to linearize the measurement matrix H\n", " given the state vector and the known positions of each SV.\n", " Here we assume 4 SV are used, but more measurements may be\n", " added.\n", " \n", " Parameters\n", " ----------\n", " xp : Predicted state vector\n", " SV_Pos : 4x3xN matrix of SV ECEF coordinates (4 => self.S_SV)\n", " \n", " Returns\n", " -------\n", " H : The linearized 4x8 measurement matrix H for each time update\n", " \n", " Mark Wickert January 2018\n", " \"\"\"\n", " H = zeros((self.N_SV,8))\n", " for i in range(self.N_SV):\n", " den = norm(xp[:6:2].flatten() - SV_Pos[i,:])\n", " H[i,0:6:2] = (xp[0:6:2].flatten() - SV_Pos[i,:])/den\n", " H[i,6] = 1.0 \n", " return H\n", " \n", " \n", " def hx(self,xp,SV_Pos):\n", " \"\"\"\n", " The predicted pseudorange zp computed from xp and the SV\n", " ephemeris stored in SV_Pos array, but in practice the receiver\n", " has SV ephemeris info from the 50 bps message\n", " \n", " Parameters\n", " ----------\n", " xp : Predicted state vector\n", " SV_Pos : 4x3xN matrix of SV ECEF coordinates (4 => self.S_SV)\n", " \n", " Returns\n", " -------\n", " zp : The predicted via h(xp)\n", " \n", " Mark Wickert January 2018 \n", " \"\"\"\n", " zp = zeros((self.N_SV,1))\n", " for i in range(self.N_SV):\n", " den = norm(xp[:6:2].flatten() - SV_Pos[i,:])\n", " zp[i,0] = den + xp[6] \n", " return zp " ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Case #1\n", "\n", "### Run Simulation for Case #1" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": false }, "outputs": [], "source": [ "Nsamples = SV_Pos.shape[2]\n", "print('Sim Seconds = %d' % Nsamples)\n", "dt = 1\n", "# Save user position history\n", "Pos_KF = zeros((Nsamples,3))\n", "# Save history of error covariance matrix diagonal \n", "P_diag = zeros((Nsamples,8))\n", "\n", "Pseudo_ranges1 = GetPseudoRange(PR_std=0.1,CDt=0,N_SV=4)\n", "GPS_EKF1 = GPS_EKF(USER_xyz_init=USER_Pos_ecf[0,:] + 5*randn(3),\n", " dt=1,\n", " sigma_xyz=5,\n", " Sf=36,\n", " Sg=0.01,\n", " Rhoerror=36,\n", " N_SV=4)\n", "for k in range(Nsamples):\n", " Pseudo_ranges1.measurement(USER_Pos_ecf[k,:],SV_Pos[:,:,k])\n", " GPS_EKF1.update(Pseudo_ranges1.USER_PR,SV_Pos[:,:,k])\n", " Pos_KF[k,:] = GPS_EKF1.x[0:6:2,0]\n", " P_diag[k,:] = GPS_EKF1.P.diagonal()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### The ECEF User Track\n", "The error is small as the noise and other uncertainties, at present are small." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "figure(figsize=(6,6))\n", "subplot(311)\n", "Pos_err_KF_ecf = Pos_KF - USER_Pos_ecf\n", "plot(Pos_err_KF_ecf[:,0])\n", "ylabel(r'Error in $x$ (m)')\n", "xlabel(r'Time (s) (given $T_s = 1$s)')\n", "grid()\n", "subplot(312)\n", "plot(Pos_err_KF_ecf[:,1])\n", "ylabel(r'Error in $y$ (m)')\n", "xlabel(r'Time (s) (given $T_s = 1$s)')\n", "grid()\n", "subplot(313)\n", "plot(Pos_err_KF_ecf[:,2])\n", "ylabel(r'Error in $z$ (m)')\n", "xlabel(r'Time (s) (given $T_s = 1$s)')\n", "grid()\n", "tight_layout()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Selected Error Covariance Results for the Simulation Run\n", "The error covariance matrix, $\\mathbf{P}$, is $8\\times 8$, with the diagonal entries beingthe variances of each of the states.\n", "\n", "Convergence looks reasonable as we see an intial error transient and then a gradual reduction in the covariance." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "plot(P_diag[:,0])\n", "plot(P_diag[:,2])\n", "plot(P_diag[:,4])\n", "title(r'Selected Covariance Matrix $\\mathbf{P}$ Diagonal Entries')\n", "ylabel(r'Variance (m$^2$)')\n", "xlabel(r'Time (s) (given $T_s = 1$s)')\n", "legend((r'$\\sigma_x^2$',r'$\\sigma_y^2$',r'$\\sigma_z^2$'),loc='best')\n", "grid();" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "* Consider the $6\\times 6$ submatrix of $\\mathbf{P}$ corresponding to the x, y, and z, position and velocity states, at the final time sample of the simulation run." ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": true }, "outputs": [], "source": [ "print(np.array_str(GPS_EKF1.P[:6,:6], precision=2))" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "GPS_EKF1.P.diagonal()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Convert the ECEF User Trajectory Back to ENU Local Coordinates" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "Npts = Pos_KF.shape[0]\n", "Pos_KF_enu = zeros((Npts,3))\n", "for k in range(Npts):\n", " Pos_KF_enu[k,:] = GPS.ecef2enu(Pos_KF[k,:],\n", " GPS_ds1.ref_ecef,\n", " GPS_ds1.ref_lla[0],\n", " GPS_ds1.ref_lla[1])\n", "plot(Pos_KF_enu[:,0]/1609.344,Pos_KF_enu[:,1]/1609.344,'b')\n", "title(r'KF Estimated Trajectory in ENU \\\n", "Coordinates @ %2.0f mph' % (USER_vel,))\n", "xlabel(r'East (mi)')\n", "ylabel(r'North (mi)')\n", "grid();" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Case #2\n", "\n", "### Run simulation for Case #2" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Line segment User Trajectory\n", "rl1 = [('e',.2),('n',.4),('e',-0.1),('n',-0.2),('e',-0.1),('n',-0.1)]\n", "rl1" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Create a GPS data source\n", "GPS_ds1 = GPS.GPSDataSource('GPS_tle_1_10_2018.txt',\n", " rx_sv_list = \\\n", " ('PRN 32','PRN 21','PRN 10','PRN 18'),\n", " ref_lla=(38.8454167, -104.7215556, 1903.0),\n", " ts=1)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Populate User and SV trajectory matrices\n", "USER_vel = 30 # mph\n", "USER_Pos_enu, USER_Pos_ecf, SV_Pos, SV_Vel = \\\n", " GPS_ds1.user_traj_gen(route_list=rl1,\n", " vmph=USER_vel,\n", " yr2=18, # the 2k year, so 2018 is 18\n", " mon=1,\n", " day=15,\n", " hr=8+7,\n", " minute=45) # Jan 18, 2018, 8:45 AM" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": false }, "outputs": [], "source": [ "GPS.sv_user_traj_3d(GPS_ds1,SV_Pos,USER_Pos_ecf,ele=20,azim=-40)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Simulation for Case #2" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": false }, "outputs": [], "source": [ "Nsamples = SV_Pos.shape[2]\n", "print('Sim Seconds = %d' % Nsamples)\n", "dt = 1\n", "# Save user position history\n", "Pos_KF = zeros((Nsamples,3))\n", "# Save history of error covariance matrix diagonal \n", "P_diag = zeros((Nsamples,8))\n", "\n", "Pseudo_ranges1 = GetPseudoRange(PR_std=0.1,CDt=0,N_SV=4)\n", "GPS_EKF1 = GPS_EKF(USER_xyz_init=USER_Pos_ecf[0,:] + 50*randn(3),\n", " dt=1,\n", " sigma_xyz=5,\n", " Sf=36,\n", " Sg=0.01,\n", " Rhoerror=36,\n", " N_SV=4)\n", "for k in range(Nsamples):\n", " Pseudo_ranges1.measurement(USER_Pos_ecf[k,:],SV_Pos[:,:,k])\n", " GPS_EKF1.update(Pseudo_ranges1.USER_PR,SV_Pos[:,:,k])\n", " Pos_KF[k,:] = GPS_EKF1.x[0:6:2,0]\n", " P_diag[k,:] = GPS_EKF1.P.diagonal()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### The ECEF User Track\n", "The error is small as the noise and other uncertainties, at present are small." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "figure(figsize=(6,6))\n", "subplot(311)\n", "Pos_err_KF_ecf = Pos_KF - USER_Pos_ecf\n", "plot(Pos_err_KF_ecf[:,0])\n", "ylabel(r'Error in $x$ (m)')\n", "xlabel(r'Time (s) (given $T_s = 1$s)')\n", "grid()\n", "subplot(312)\n", "plot(Pos_err_KF_ecf[:,1])\n", "ylabel(r'Error in $y$ (m)')\n", "xlabel(r'Time (s) (given $T_s = 1$s)')\n", "#ylim([-12,12])\n", "grid()\n", "subplot(313)\n", "plot(Pos_err_KF_ecf[:,2])\n", "ylabel(r'Error in $z$ (m)')\n", "xlabel(r'Time (s) (given $T_s = 1$s)')\n", "grid()\n", "tight_layout()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Selected Error Covariance Results for the Simulation Run\n", "The error covariance matrix, $\\mathbf{P}$, is $8\\times 8$, with the diagonal entries beingthe variances of each of the states.\n", "\n", "Convergence looks reasonable as we see an intial error transient and then a gradual reduction in the covariance." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "plot(P_diag[:,0])\n", "plot(P_diag[:,2])\n", "plot(P_diag[:,4])\n", "title(r'Selected Covariance Matrix $\\mathbf{P}$ Diagonal Entries')\n", "ylabel(r'Variance (m$^2$)')\n", "xlabel(r'Time (s) (given $T_s = 1$s)')\n", "legend((r'$\\sigma_x^2$',r'$\\sigma_y^2$',r'$\\sigma_z^2$'),loc='best')\n", "grid();" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "* Consider the $6\\times 6$ submatrix of $\\mathbf{P}$ corresponding to the x, y, and z, position and velocity states, at the final time sample of the simulation run." ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": true }, "outputs": [], "source": [ "print(np.array_str(GPS_EKF1.P[:6,:6], precision=2))" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "GPS_EKF1.P.diagonal()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Convert the ECEF User Trajectory Back to ENU Local Coordinates" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "Npts = Pos_KF.shape[0]\n", "Pos_KF_enu = zeros((Npts,3))\n", "for k in range(Npts):\n", " Pos_KF_enu[k,:] = GPS.ecef2enu(Pos_KF[k,:],\n", " GPS_ds1.ref_ecef,\n", " GPS_ds1.ref_lla[0],\n", " GPS_ds1.ref_lla[1])\n", "plot(Pos_KF_enu[:,0]/1609.344,Pos_KF_enu[:,1]/1609.344,'b')\n", "title(r'KF Estimated Trajectory in ENU \\\n", "Coordinates @ %2.0f mph' % (USER_vel,))\n", "xlabel(r'East (mi)')\n", "ylabel(r'North (mi)')\n", "grid();" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## References\n", "\n", "1. [Phil Kim, *Kalman Filtering for Beginners with MATLAB Examples*, 2011.](https://www.amazon.com/Kalman-Filter-Beginners-MATLAB-Examples/dp/1463648359/ref=sr_1_9?ie=UTF8&qid=1514912616&sr=8-9&keywords=Kalman+Filtering)\n", "2. [Robert Brown and Patrick Hwang, *Introduction to Random Signals and Applied Kalman Filtering*, 4th edition, 2012.](https://www.amazon.com/Introduction-Signals-Applied-Filtering-Exercises/dp/0470609699/ref=sr_1_3?ie=UTF8&qid=1514912616&sr=8-3&keywords=Kalman+Filtering)\n", "3. [Elliot Kaplan, editor, *Understanding GPS Principles and Applications*, 1996 (3rd edition available).](https://www.amazon.com/Understanding-Principles-Applications-Artech-Communications/dp/0890067937/ref=sr_1_2?ie=UTF8&qid=1516027849&sr=8-2&keywords=UNderstanding+GPS)\n", "4. [Dan Simon, *Optimal State Estimation*, 2006.](https://www.amazon.com/Optimal-State-Estimation-Nonlinear-Approaches/dp/0471708585/ref=sr_1_1?s=books&ie=UTF8&qid=1516027927&sr=1-1&keywords=Optimal+State+Estimation)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.6.1" } }, "nbformat": 4, "nbformat_minor": 2 }