[1]:
%pylab inline
from gps_helper import gps_helper as GPS
from IPython.display import Image, SVG
Populating the interactive namespace from numpy and matplotlib
/home/docs/checkouts/readthedocs.org/user_builds/gps-helper/envs/latest/lib/python3.7/site-packages/gps_helper-1.1.4-py3.7.egg/gps_helper/gps_helper.py:16: UserWarning: mayavi not imported due to import error
  warnings.warn("mayavi not imported due to import error")
[2]:
pylab.rcParams['savefig.dpi'] = 100 # default 72
%config InlineBackend.figure_formats=['svg'] # SVG inline viewing

Kalman Filter Variables

2f050bfbca7b40459a0a366db6925197

[3]:
Image('figs/Kalman_variables.png',width='60%')
[3]:
../_images/nb_examples_Kalman_GPS_practice_4_0.png

General Extended Kalman Filter (EKF) Block Diagram

47f760a87d464d1a8a92db8b99a62833

[4]:
Image('figs/EKF_Filter.png',width='70%')
[4]:
../_images/nb_examples_Kalman_GPS_practice_7_0.png

Application to GPS using Simulated User and Satellite Trajectories

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 is also required.

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 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.

8844e76ef06e4abb88ac7a129698e8bf

[5]:
Image('figs/Earth_Centered_Inertial_Coordinate_System.png',width='50%')
[5]:
../_images/nb_examples_Kalman_GPS_practice_10_0.png

A Simple Example

  • Generate a user trajectory
  • Pair the trajectory with a collection of appropriately chosen GPS satellites keyed to the C/A code PRN number, e.g., PRN 21, etc.
[6]:
# Line segment User Trajectory
rl1 = [('e',.2),('n',.4),('e',-0.1),('n',-0.2),('e',-0.1),('n',-0.1)]
rl1
[6]:
[('e', 0.2), ('n', 0.4), ('e', -0.1), ('n', -0.2), ('e', -0.1), ('n', -0.1)]
[7]:
# Create a GPS data source
GPS_ds1 = GPS.GPSDataSource('GPS_tle_1_10_2018.txt',
                            rx_sv_list = \
                                ('PRN 32','PRN 21','PRN 10','PRN 18'),
                                ref_lla=(38.8454167, -104.7215556, 1903.0),
                                ts=1)
[8]:
# Populate User and SV trajectory matrices
USER_vel = 5 # mph
USER_Pos_enu, USER_Pos_ecf, SV_Pos, SV_Vel = \
              GPS_ds1.user_traj_gen(route_list=rl1,
                                    vmph=USER_vel,
                                    yr2=18, # the 2k year, so 2018 is 18
                                    mon=1,
                                    day=15,
                                    hr=8+7,
                                    minute=45) # Jan 18, 2018, 8:45 AM
[9]:
# Check a user position
USER_Pos_ecf[0,:]
[9]:
array([-1264404.16643545, -4812254.11855508,  3980159.53945133])

Generate a 3D View of the Trajectories

[10]:
GPS.sv_user_traj_3d(GPS_ds1,SV_Pos,USER_Pos_ecf,ele=20,azim=-40)
Duration: 13.20 min
../_images/nb_examples_Kalman_GPS_practice_17_1.svg
[11]:
plot(USER_Pos_enu[:,0],USER_Pos_enu[:,1])
plot(USER_Pos_enu[0,0],USER_Pos_enu[0,1],'g.')
plot(USER_Pos_enu[-1,0],USER_Pos_enu[-1,1],'r.')
title(r'User Trajectory in ENU Coordinates')
xlabel(r'East (mi)')
ylabel(r'North (mi)')
grid();
../_images/nb_examples_Kalman_GPS_practice_18_0.svg

Develop a GPS EKF

The constant velocity process model of [2] is adopted for this project. The step is defining the eight element state vector \(\mathbf{x}\):

317a48eca2a9444598fc5f1404904a4d

[12]:
Image('figs/Process_Model1.png',width='80%')
[12]:
../_images/nb_examples_Kalman_GPS_practice_21_0.png

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:

5b9d967eec3b42188d252dacbf518a13

[13]:
Image('figs/Process_Model2.png',width='80%')
[13]:
../_images/nb_examples_Kalman_GPS_practice_24_0.png

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}\):

5e84ae8b9ace4cfa995ea5c7e9d159a3

[14]:
Image('figs/Process_Model3.png',width='80%')
[14]:
../_images/nb_examples_Kalman_GPS_practice_27_0.png

Simulated Pseudo Range from the User and Satellite Trajectories

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.

bfb45f4fd9344ad9b82d7336d7d2b52c

[15]:
Image('figs/Measurement_Model1.png',width='70%')
[15]:
../_images/nb_examples_Kalman_GPS_practice_30_0.png

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.

[16]:
class GetPseudoRange(object):
    """
    A class for generating simulated Pseudo range measurements from
    simulated SV ECEF and simulated User ECEF trajectories. The number of
    measurements is 4, but more or less measurements can be specified
    assuming the number of SVs configured matches.

    Mark Wickert January 2018
    """


    def __init__ (self, PR_std = 0, CDt = 0, N_SV = 4):
        """
        Initialize the object
        """
        self.PR_std = PR_std
        self.CDt = CDt
        self.N_SV = N_SV
        self.USER_PR = zeros((N_SV,1))


    def measurement(self, USER_Pos_ecef, SV_Pos_ecef):
        """
        Take a measurement by passing in position values at time
        index i, i.e., USER_Pos_ecf[i,:] & SV_Pos[:,:,i]
        """
        # Compute the pseudo range to each SV
        for k in range(self.N_SV):
            self.USER_PR[k,0] = norm(USER_Pos_ecef - SV_Pos_ecef[k])
            # Add bias and measurement noise
            self.USER_PR[k,0] +=  self.CDt + self.PR_std*random.randn(1)[0]
        return

The EKF Class for Estimating Position from the Pseudorange Measurements

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.

d3f04b20cc014914862fad57daa9a64d

[17]:
Image('figs/Measurement_Model2.png',width='70%')
[17]:
../_images/nb_examples_Kalman_GPS_practice_35_0.png
[18]:
class GPS_EKF(object):
    """
    GPS Extended Kalman Filter User Tracking using SGP4-based SV tracks in ECEF
    and ENU user tracks converted to ECEF so that simulated pseudo range
    measurements can be made. The overall formulation is based on the book by
    Robert Brown & Patrick Hwang, "Introduction to Random Signals and Applied
    Kalman Filtering with MATLAB Exercises", 4th edition, 2012.

    Python 3.x is assumed so the operator @ can be used for matrix multiply

    Mark Wickert January 2018
    """


    def __init__ (self, USER_xyz_init, dt = 1, sigma_xyz = 5,
                  Sf = 36, Sg = 0.01, Rhoerror = 36, N_SV = 4):
        """
        Initialize the object

        The state vector x is composed of eight components:

        [x_pos, x_vel, y_pos, y_vel, z_pos, z_vel, clk_bias, clk_bias_rate].T

        Futhermore a constant velocity model is chosen for the process model
        """
        self.dt = dt
        self.N_SV = N_SV
        # Create a block diagonal state transition matrix
        A_cv = array([[1, dt],[0, 1]]) # 2x2 block
        A_cvr = hstack((A_cv,zeros((2,N_SV+2)))) #block with zeros appended
        self.A = vstack((A_cvr,roll(A_cvr,2),roll(A_cvr,4),roll(A_cvr,6)))
        # Process model covariance Q [Brown & Hwang] is subblocks Qxyz and Qb
        Qxyz = sigma_xyz**2 * array([[dt**3/3, dt**2/2],[dt**2/2, dt]])
        Qb = array([[Sf*dt+Sg*dt**3/3, Sg*dt**2/2], [Sg*dt**2/2, Sg*dt]])
        # Create a block diagonal matrix to hold 2x2 blocks
        Qxyzr = hstack((Qxyz,zeros((2,6))))
        Qbr = hstack((Qb,zeros((2,6))))
        self.Q = vstack((Qxyzr,roll(Qxyzr,2),roll(Qxyzr,4),roll(Qbr,6)))

        # Measurement model covariance matrix R
        # Rhoerror = variance of measurement error(pseudorange error)
        self.R = Rhoerror*eye(N_SV)
        # Error covariance matrix initialize
        self.P = 10*eye(8)
        # Initialize state vector
        self.x = zeros((8,1))
        self.x[0:6:2,0] = USER_xyz_init
        self.x[1:7:2,0] = [0, 0, 0]  # Initial velocity
        self.x[6,0] = 0 #3.575e6     # Initial c*clock bias in m
        self.x[7,0] = 0 #4.549e1     # Initial c*clock drift in m/s


    def update(self, z, SV_Pos):
        """
        Update the Kalman filter state by inputting a new set of
        pseudorange measurements.
        Return the state array as a tuple.
        Update all other Kalman filter quantities
        Input SV ephemeris at one time step, e.g., SV_Pos[:,:,i]
        """
        # H = Matrix of partials dh/dx
        H = self.Hjacob(self.x, SV_Pos)

        xp = self.A @ self.x
        Pp = self.A @ self.P @ self.A.T + self.Q

        self.K = Pp @ H.T @ inv(H @ Pp @ H.T + self.R)

        # zp = h(xp), the predicted pseudorange
        zp = self.hx(xp, SV_Pos)

        self.x = xp + self.K @ (z - zp)
        self.P = Pp - self.K @ H @ Pp
        # Return the x,y,z position (also held in the state vector attribute)
        return self.x[0,0], self.x[2,0], self.x[4,0]


    def Hjacob(self,xp,SV_Pos):
        """
        Jacobian used to linearize the measurement matrix H
        given the state vector and the known positions of each SV.
        Here we assume 4 SV are used, but more measurements may be
        added.

        Parameters
        ----------
        xp : Predicted state vector
        SV_Pos : 4x3xN matrix of SV ECEF coordinates (4 => self.S_SV)

        Returns
        -------
        H : The linearized 4x8 measurement matrix H for each time update

        Mark Wickert January 2018
        """
        H = zeros((self.N_SV,8))
        for i in range(self.N_SV):
            den = norm(xp[:6:2].flatten() - SV_Pos[i,:])
            H[i,0:6:2] = (xp[0:6:2].flatten() - SV_Pos[i,:])/den
            H[i,6] = 1.0
        return H


    def hx(self,xp,SV_Pos):
        """
        The predicted pseudorange zp computed from xp and the SV
        ephemeris stored in SV_Pos array, but in practice the receiver
        has SV ephemeris info from the 50 bps message

        Parameters
        ----------
        xp : Predicted state vector
        SV_Pos : 4x3xN matrix of SV ECEF coordinates (4 => self.S_SV)

        Returns
        -------
        zp : The predicted via h(xp)

        Mark Wickert January 2018
        """
        zp = zeros((self.N_SV,1))
        for i in range(self.N_SV):
            den = norm(xp[:6:2].flatten() - SV_Pos[i,:])
            zp[i,0] = den + xp[6]
        return zp

Case #1

Run Simulation for Case #1

[19]:
Nsamples = SV_Pos.shape[2]
print('Sim Seconds = %d' % Nsamples)
dt = 1
# Save user position history
Pos_KF = zeros((Nsamples,3))
# Save history of error covariance matrix diagonal
P_diag = zeros((Nsamples,8))

Pseudo_ranges1 = GetPseudoRange(PR_std=0.1,CDt=0,N_SV=4)
GPS_EKF1 = GPS_EKF(USER_xyz_init=USER_Pos_ecf[0,:] + 5*randn(3),
                   dt=1,
                   sigma_xyz=5,
                   Sf=36,
                   Sg=0.01,
                   Rhoerror=36,
                   N_SV=4)
for k in range(Nsamples):
    Pseudo_ranges1.measurement(USER_Pos_ecf[k,:],SV_Pos[:,:,k])
    GPS_EKF1.update(Pseudo_ranges1.USER_PR,SV_Pos[:,:,k])
    Pos_KF[k,:] = GPS_EKF1.x[0:6:2,0]
    P_diag[k,:] = GPS_EKF1.P.diagonal()
Sim Seconds = 792

The ECEF User Track

The error is small as the noise and other uncertainties, at present are small.

[20]:
figure(figsize=(6,6))
subplot(311)
Pos_err_KF_ecf = Pos_KF - USER_Pos_ecf
plot(Pos_err_KF_ecf[:,0])
ylabel(r'Error in $x$ (m)')
xlabel(r'Time (s) (given $T_s = 1$s)')
grid()
subplot(312)
plot(Pos_err_KF_ecf[:,1])
ylabel(r'Error in $y$ (m)')
xlabel(r'Time (s) (given $T_s = 1$s)')
grid()
subplot(313)
plot(Pos_err_KF_ecf[:,2])
ylabel(r'Error in $z$ (m)')
xlabel(r'Time (s) (given $T_s = 1$s)')
grid()
tight_layout()
../_images/nb_examples_Kalman_GPS_practice_40_0.svg

Selected Error Covariance Results for the Simulation Run

The error covariance matrix, \(\mathbf{P}\), is \(8\times 8\), with the diagonal entries beingthe variances of each of the states.

Convergence looks reasonable as we see an intial error transient and then a gradual reduction in the covariance.

[21]:
plot(P_diag[:,0])
plot(P_diag[:,2])
plot(P_diag[:,4])
title(r'Selected Covariance Matrix $\mathbf{P}$ Diagonal Entries')
ylabel(r'Variance (m$^2$)')
xlabel(r'Time (s) (given $T_s = 1$s)')
legend((r'$\sigma_x^2$',r'$\sigma_y^2$',r'$\sigma_z^2$'),loc='best')
grid();
../_images/nb_examples_Kalman_GPS_practice_42_0.svg
  • 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.
[22]:
print(np.array_str(GPS_EKF1.P[:6,:6], precision=2))
[[ 5.55e+01  2.32e+01  1.43e+02  4.12e+00 -6.38e+01 -1.27e+00]
 [ 2.32e+01  3.46e+01  3.92e+00  6.13e-01 -1.24e+00  1.41e-01]
 [ 1.43e+02  3.92e+00  1.71e+03  6.97e+01 -7.57e+02 -2.11e+01]
 [ 4.12e+00  6.13e-01  6.97e+01  4.12e+01 -2.18e+01 -3.27e+00]
 [-6.38e+01 -1.24e+00 -7.57e+02 -2.18e+01  3.76e+02  2.80e+01]
 [-1.27e+00  1.41e-01 -2.11e+01 -3.27e+00  2.80e+01  3.26e+01]]
[23]:
GPS_EKF1.P.diagonal()
[23]:
array([5.54745370e+01, 3.46397860e+01, 1.70625828e+03, 4.11822187e+01,
       3.75796594e+02, 3.26168028e+01, 1.36063612e+03, 8.29052039e-01])

Convert the ECEF User Trajectory Back to ENU Local Coordinates

[24]:
Npts = Pos_KF.shape[0]
Pos_KF_enu = zeros((Npts,3))
for k in range(Npts):
    Pos_KF_enu[k,:] = GPS.ecef2enu(Pos_KF[k,:],
                                   GPS_ds1.ref_ecef,
                                   GPS_ds1.ref_lla[0],
                                   GPS_ds1.ref_lla[1])
plot(Pos_KF_enu[:,0]/1609.344,Pos_KF_enu[:,1]/1609.344,'b')
title(r'KF Estimated Trajectory in ENU \
Coordinates @ %2.0f mph' % (USER_vel,))
xlabel(r'East (mi)')
ylabel(r'North (mi)')
grid();
../_images/nb_examples_Kalman_GPS_practice_47_0.svg

Run simulation for Case #2

[25]:
# Line segment User Trajectory
rl1 = [('e',.2),('n',.4),('e',-0.1),('n',-0.2),('e',-0.1),('n',-0.1)]
rl1
[25]:
[('e', 0.2), ('n', 0.4), ('e', -0.1), ('n', -0.2), ('e', -0.1), ('n', -0.1)]
[26]:
# Create a GPS data source
GPS_ds1 = GPS.GPSDataSource('GPS_tle_1_10_2018.txt',
                              rx_sv_list = \
                              ('PRN 32','PRN 21','PRN 10','PRN 18'),
                              ref_lla=(38.8454167, -104.7215556, 1903.0),
                              ts=1)
[27]:
# Populate User and SV trajectory matrices
USER_vel = 30 # mph
USER_Pos_enu, USER_Pos_ecf, SV_Pos, SV_Vel = \
              GPS_ds1.user_traj_gen(route_list=rl1,
                                    vmph=USER_vel,
                                    yr2=18, # the 2k year, so 2018 is 18
                                    mon=1,
                                    day=15,
                                    hr=8+7,
                                    minute=45) # Jan 18, 2018, 8:45 AM
[28]:
GPS.sv_user_traj_3d(GPS_ds1,SV_Pos,USER_Pos_ecf,ele=20,azim=-40)
Duration: 2.20 min
../_images/nb_examples_Kalman_GPS_practice_52_1.svg

Simulation for Case #2

[29]:
Nsamples = SV_Pos.shape[2]
print('Sim Seconds = %d' % Nsamples)
dt = 1
# Save user position history
Pos_KF = zeros((Nsamples,3))
# Save history of error covariance matrix diagonal
P_diag = zeros((Nsamples,8))

Pseudo_ranges1 = GetPseudoRange(PR_std=0.1,CDt=0,N_SV=4)
GPS_EKF1 = GPS_EKF(USER_xyz_init=USER_Pos_ecf[0,:] + 50*randn(3),
                   dt=1,
                   sigma_xyz=5,
                   Sf=36,
                   Sg=0.01,
                   Rhoerror=36,
                   N_SV=4)
for k in range(Nsamples):
    Pseudo_ranges1.measurement(USER_Pos_ecf[k,:],SV_Pos[:,:,k])
    GPS_EKF1.update(Pseudo_ranges1.USER_PR,SV_Pos[:,:,k])
    Pos_KF[k,:] = GPS_EKF1.x[0:6:2,0]
    P_diag[k,:] = GPS_EKF1.P.diagonal()
Sim Seconds = 132

The ECEF User Track

The error is small as the noise and other uncertainties, at present are small.

[30]:
figure(figsize=(6,6))
subplot(311)
Pos_err_KF_ecf = Pos_KF - USER_Pos_ecf
plot(Pos_err_KF_ecf[:,0])
ylabel(r'Error in $x$ (m)')
xlabel(r'Time (s) (given $T_s = 1$s)')
grid()
subplot(312)
plot(Pos_err_KF_ecf[:,1])
ylabel(r'Error in $y$ (m)')
xlabel(r'Time (s) (given $T_s = 1$s)')
#ylim([-12,12])
grid()
subplot(313)
plot(Pos_err_KF_ecf[:,2])
ylabel(r'Error in $z$ (m)')
xlabel(r'Time (s) (given $T_s = 1$s)')
grid()
tight_layout()
../_images/nb_examples_Kalman_GPS_practice_56_0.svg

Selected Error Covariance Results for the Simulation Run

The error covariance matrix, \(\mathbf{P}\), is \(8\times 8\), with the diagonal entries beingthe variances of each of the states.

Convergence looks reasonable as we see an intial error transient and then a gradual reduction in the covariance.

[31]:
plot(P_diag[:,0])
plot(P_diag[:,2])
plot(P_diag[:,4])
title(r'Selected Covariance Matrix $\mathbf{P}$ Diagonal Entries')
ylabel(r'Variance (m$^2$)')
xlabel(r'Time (s) (given $T_s = 1$s)')
legend((r'$\sigma_x^2$',r'$\sigma_y^2$',r'$\sigma_z^2$'),loc='best')
grid();
../_images/nb_examples_Kalman_GPS_practice_58_0.svg
  • 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.
[32]:
print(np.array_str(GPS_EKF1.P[:6,:6], precision=2))
[[ 1.29e+02  2.45e+01  5.32e+02  9.55e+00 -2.75e+02 -5.06e+00]
 [ 2.45e+01  3.48e+01  9.11e+00  8.51e-01 -4.93e+00 -6.00e-01]
 [ 5.32e+02  9.11e+00  3.35e+03  8.30e+01 -1.71e+03 -3.12e+01]
 [ 9.55e+00  8.51e-01  8.30e+01  4.06e+01 -3.18e+01 -3.63e+00]
 [-2.75e+02 -4.93e+00 -1.71e+03 -3.18e+01  9.14e+02  3.50e+01]
 [-5.06e+00 -6.00e-01 -3.12e+01 -3.63e+00  3.50e+01  3.34e+01]]
[33]:
GPS_EKF1.P.diagonal()
[33]:
array([1.29140255e+02, 3.47975865e+01, 3.34561106e+03, 4.06004715e+01,
       9.13835850e+02, 3.34143384e+01, 2.90153758e+03, 9.84576029e-01])

Convert the ECEF User Trajectory Back to ENU Local Coordinates

[34]:
Npts = Pos_KF.shape[0]
Pos_KF_enu = zeros((Npts,3))
for k in range(Npts):
    Pos_KF_enu[k,:] = GPS.ecef2enu(Pos_KF[k,:],
                                   GPS_ds1.ref_ecef,
                                   GPS_ds1.ref_lla[0],
                                   GPS_ds1.ref_lla[1])
plot(Pos_KF_enu[:,0]/1609.344,Pos_KF_enu[:,1]/1609.344,'b')
title(r'KF Estimated Trajectory in ENU \
Coordinates @ %2.0f mph' % (USER_vel,))
xlabel(r'East (mi)')
ylabel(r'North (mi)')
grid();
../_images/nb_examples_Kalman_GPS_practice_63_0.svg
  1. Phil Kim, *Kalman Filtering for Beginners with MATLAB Examples*, 2011.
  2. Robert Brown and Patrick Hwang, *Introduction to Random Signals and Applied Kalman Filtering*, 4th edition, 2012.
  3. Elliot Kaplan, editor, *Understanding GPS Principles and Applications*, 1996 (3rd edition available).
  4. Dan Simon, *Optimal State Estimation*, 2006.
[ ]: