Exploring the Extended Kalman Filter for GPS Positioning Using Simulated User and Satellite Track Data

This paper describes a Python computational tool for exploring the use of the extended Kalman filter (EKF) for position estimation using the Global Positioning System (GPS) pseudorange measurements. The development was motivated by the need for an example generator in a training class on Kalman filtering, with emphasis on GPS. In operation of the simulation framework both user and satellite trajectories are played through the simulation. The User trajectory is input in local east-north-up (ENU) coordinates and satellites tracks, specified by the C/A code PRN number, are propagated using the Python package SGP4 using two-line element (TLE) data available from [Celestrak].


Introduction
The Global Positioning System (GPS) allows user position estimation using time difference of arrival (TDOA) measurements from signals received from a constellation of 24 medium earth orbit satellites of space vehicles (SVs).The Kalman filter is a popular optimal state estimation algorithm [Simon2006] used by a variety of engineering and science disciplines.In particular the extended Kalman filter (EKF) is able to deal with nonlinearities related to both the measurement equations and state vector process update model.The EKF used in GPS has a linear process model, but a nonlinear measurement model [Brown2012].This paper describes a Python computational tool for exploring the use of the EKF for GPS position estimation using pseudorange measurements.The development was motivated by the need for an example generator in a training class on Kalman filtering, with emphasis on GPS.What is special about the tool created here is that both User and satellite trajectories are custom generated for input to a Kalman filter implemented in a Jupyter notebook.The steps followed are logical and clear.You first enter a desired User trajectory/route, then choose appropriate in-view GPS satellites, and then using actual GPS satellite orbital mechanics information, create a simulated receiver measurement stream.A 3D plot shows you the satellite tracks in space and the User trajectory on the surface of the earth, over time.The Kalman filter code, also defined in the Jupyter notebook, uses the matrix math commonly found in textbooks, but it is easy to follow as we make use of the PEP 465 @ infix operator for matrix multiplication.As the final step, the data set is played through the Kalman filter in earth-centered earth-fixed (ECEF) coordinates.The User trajectory is input in local east-north-up (ENU) coordinates, and the SVs in view by the User to form the location estimate, are specified by the coarse acquisition (C/A) code pseudo-random noise (PRN) number.The ECEF coordinates of the SVs are then propagated using [SGP4] using the two-line element (TLE) data available from [Celestrak], in time step with the User trajectory.The relationship between ECEF and ENU is explained in Figure 1.For convenience, this computational tool, is housed in a Jupyter notebook.Data set generation and 3D trajectory plotting is provided with the assistance of a single module, [GPS_helper].
Fig. 1: The earth centric earth fixed (ECEF) coordinate system compared with the local east-north-up (ENU) coordinate system.

GPS Background
GPS was started in 1973 with the first block of satellites launched over the 1978 to 1985 time interval [GPS].The formal name became NAVSTAR, which stands for NAVigation Satellite Timing And Ranging system, in the early days.At the present time there are 31 GPS satellites in orbit.The original design called for 24 satellites.The satellites orbit at an altitude of about 20,350 km (~12,600 mi).This altitude classifies the satellites as being in a medium earth orbit (MEO), as opposed to low earth orbit (LEO), or geostationary above the equator (GEO), or high earth orbit (HEO).The orbit period is 11 hours 58 minutes with six SVs in view at any time from the surface of the earth.Clock accuracy is key to the operation of GPS and the satellite clocks are very accurate.Four satellites are needed for a complete position determination since the user clock is an uncertainty that must be resolved.The maximum SV velocity relative to an earth user is 800m/s (the satellite itself is traveling at ~7000 mph), thus the induced Doppler is up to kHz on the L1 carrier frequency of 1.57542 GHz.This frequency uncertainty plus any motion of the user itself, creates additional challenges in processing the received GPS signals.

Waveform Design and Pseudorange Measurements
Time difference of arrival (TDOA) is the key to forming the User position estimates.This starts by assigning a unique repeating code of 1023 bits to each SV and corresponds to the L1 carrier waveform it transmits.As the User receives the superposition of all the in-view satellites, the code known by its PRN number assigned to a particular satellite, is discernable by cross-correlating the composite received L1 signal and a locally generated PRN waveform.The correlation peak and its associated TDOA, become the pseudorange or approximate radial distance between the User and SV when multipled by c, the speed of light.
The pseudorange contains error due to the receiver clock offset from the satellite time and other error components [Brown2012].The noise-free pseudorange takes the form where (x i , y i , z i ), i = 1, . . .4, is the satellite ECEF location and (x u , y u , z u ) is the user ECEF location, c is the speed of light, and ∆t is the receiver offset from satellite time.The product c∆t can be thought of as the range equivalent timing error.There are three geometry unknowns and time offset, thus at minimum there are four non-linear equations of (1) are what must be solved to obtain the User location.

Solving the Nonlinear Position Equations
Two techniques are widely discussed in the literature and applied in practice [GPS] and [Kaplan]: (1) nonlinear least squares and (2) the extended Kalman filter (EKF).In this paper we focus on the use of the EKF.The EKF is an extension to the linear Kalman filter, so we start by briefly describing the linear model case and move quickly to the nonlinear case.

Kalman Filter and State Estimation
It was back in 1960 that R. E. Kalman introduced his filter [Kalman].It immediately became popular in guidance, navigation, and control applications.The Kalman filter is an optimal, in the minimum mean-squared error sense, as means to estimate the state of a dynamical system [Simon2006].By state we mean a vector of variables that adequately describes the dynamical behavior of a system over time.For the GPS problem a simplifying assumption regarding the state model is to assume that the User has approximately constant velocity, so a position-velocity (PV) only state model is adequate.The Kalman filter is recursive, meaning that the estimate of the state is refined with each new input measurement and without the need to store all of the past measurements.
Within the Kalman filter we have a process model and a measurement model.The process equation associated with the process model, describes how the state is updated through a state transition matrix plus a process noise vector having covariance matrix Q.The measurement model contains the measurement equation that abstractly produces the measurement vector as a matrix times the state vector plus a measurement noise vector having covariance matrix R. The optimal recursive filter algorithm is formed using the quantities that make up the process and measurement models.For details the reader is referred to the references.
For readers wanting a hands-on beginners introduction to the Kalman filter, a good starting point is the book by Kim [Kim2011].In Kim's book the Kalman filter is neatly represented input/output block diagram form as shown in Figure 2, with the input being the vector of measurements z k , at time k, and the output xk an updated estimate of the state vector.The Kalman filter variables are defined in Table 1.Note the dimensions seen in Table 1 are n = number of state variables and m = number of measurements.
Compute the Kalman gain: 4. Compute the error covariance : Compute the state estimate:

State Vector for the GPS Problem
For a PV model the User state vector position and velocity in x, y, z and clock equivalent range and range velocity error [Brown2012]: where ECEF coordinates are assumed and the over dots denote the time derivative, e.g., ẋ = dx/dt.We further assume that there is no coupling between x, y, z, c∆t, thus the state transition matrix A is a 4 × 4 block diagonal matrix of the form where

Process Model Covariance Matrix
The process covariance matrix for the GPS problem is a block diagonal Matrix, with three identical blocks for the position-velocity pairs and one matrix for the clock-clock drift pair.The block diagonal form means that the states are assumed be statistically coupled only in pairs and outside of the pairs uncorrelated.In the model of [Brown2012] each position-velocity state-pair has two variance terms and one covariance term describing an upper triangle 2 × 2 submatrix where σ 2 xyz is a white noise spectral density representing random walk velocity error.The clock state variable pair has a 2 × 2 covariance matrix governed by S p , the white noise spectral density leading to random walk velocity error.The clock and clock drift has a more complex 2 × 2 covariance submatrix, Q b , with S g the white noise spectral density leading to a random walk clock frequency error plus white noise clock drift, thus two components of clock phase error In final form Q is a 4 × 4 block covariance matrix The covariance matrix of the pseudorange measurement error is assumed to be diagonal with equal variance σ 2 r , thus we have for the case of m = 4 measurements.Being diagonal means that all measurements are assumed statistically uncorrelated, which is reasonable.

Extended Kalman Filter
The extended Kalman filter (EKF) allows both the state update equation, Step 1 in Figure 2, to be a nonlinear function of the state, and the measurement model, Step 3 in Figure 2, to be a nonlinear function of the state.Thus the EKF block diagram replaces two expressions in Figure 2 as follows: For the case of the GPS problem we have already seen that 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.For Step 3, the state estimate, we need to linearize the equations h(x − k ).This is done by forming a matrix of partials or Jacobian matrix, which then generates an equivalent H matrix as found in the linear Kalman filter, but in the EKF is updated at each iteration of the algorithm. where for i = 1, 2, 3 and 4.

Computational Tool
The Python computational tool is composed of a Jupyter notebook and a helper module GPS_helper.py.The key elements of the helper are described in Figure 3.Here we see that the class GPS_data_source is responsible for propagating the SVs in view by the User in time-step with a constant velocity line segment User trajectory.The end result is a collection of matrices (ndarrays) that contain the ECEF User coordinates as the triples (x u , y u , z u ) versus times (also the ENU version) and for each SV indexed as i = 1, 2, 3, 4, the ECEF triples (x i , y i , z i ), also as a function of time.The time step value is T s s.
It is important to note that in creating a data set the developer must choose satellite PRNs that place the SVs in view of the user for the given start time and date.One approach is by trial and error.Pick a particular time and date, choose four PRNs, and produce the data set and create a 3D plot using GPS_helper.SV_User_Traj_3D().This is quite tedious!A better approach is to use a GPS cell phone app, or better yet a With a data set generated the next step is to generate pseudorange measurements, as the real GPS receiver would obtain TDOAs via waveform cross-correlation with a local version of the SVs PRN sequence.Finally, we estimate the user position using the EKF.Classes for both these calculations are contained the Jupyter notebook Kalman_GPS_practice.A brief description of the two classes in given in Figure 5.
The mathematical details of the EKF were discussed earlier, the Python code implementation is found in the public and private methods of the GPS_EKF class.The essence of Figure 2   Note the above code uses the Python 3.5+ matrix multiplication operator, @, to make the code nearly match the matrix algebra expressions of Figure 2.

Simulation Examples
In this section we consider two examples of using the Python framework to estimate a time-varying User trajectory using a timevarying set of GPS satellites.In the code snippets that follow were extracted from a Jupyter notebook that begins with the magic %pylab inline, hence the namespace is filled with numpy and matplotlib.

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 : x Fig. 7: The ideal user trajectory as defined by rl1 in the above code snippet.
The 3D plot 6 shows clearly the motion of the SVs, even though the simulation run-time is only 13.2 min.The User trajectory on the earth, in this case a location in Colorado Springs, CO appears as a red blob, unless the plot is zoomed in.From the ENU User trajectory we now have a clear view of the route taken by the user.The velocity is only 5 mph in straight line segments.

Selected Error Covariance Results for the Simulation Run
The error covariance matrix, , is , 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.
Figure 9 shows selected error covariance matrix terms from P k throughout the simulation.The terms displayed are the position diagonal terms, that is σ 2 x , σ 2 , and σ 2 z .The initial conditions of the EKF make these variance terms initially large.Settling begins about 50s into the simulation, and the decay continues as the 13.2 m simulation comes to an end.The EKF is behaving as expected.
Finally, in Figure 10 we have a plot of the User trajectory estimate in ENU, as a map-like 2D plot showing just the eastwest and north-south axes.The units are tenths of miles, so with the User moving along linear line segments at just 5 mph, the trajectory looks perfect.In the next example parameters will be varied to see the impact.

Case #2
In this case we still consider high quality satellite signals and a 1s update period, but now the user velocity is increased to 30 mph, so the time to traverse the User trajectory is reduced from 13.2 min down to 2.2 min.The random initial (xyz) position is set to a error standard deviation of 50 m compared with 5 m in the first case.We expect to see some difference in performance.
In Figure 11 we again plot the ECEF errors in m.The large initial position error variance forces the plot axes scale to change from Case #1.The initial errors are now very large, but do settle to small values with the exception of blips that occur every time the user changes direction by making a 90 • turn.The blips are somewhat artificial, since making a perfect right-angle turn without slowing or rounding the corner is more practical.Still it is interesting to see this behavior and also see that the EKF recovers from these errors.
Figure 12 again shows the error covariance terms for σ 2 x , σ 2 y , and σ 2 z .The results here are very similar to Case #1.The variance peaks at about 50 s into the simulation and then rapidly decays.This is not too surprising as the EKF tuning has changed from

Selected Error Covariance Results for the Simulation Run
The error covariance matrix, , is , 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.
Case #1, with the exception of the initial position error.Since the simulation only runs for 2.2 min which is 132 s, we have to compare the variances at this time to the Case #2 end results.They appear to be about the same, once again the EKF appears to be working correctly.Consider the submatrix of corresponding to the x, y, and z, position and velocity states, at the final time sample of the simulation run.
Finally, Figure 13 plots the ENU trajectory estimate in the plane EN (ignoring the UP coordinate as before).The speed is upped by a factor six compared to case #1.The most notable change is trajectory overshoot at each of the right-angle turns.No surprise here as the EKF is asked to handle very abrupt (and impractical) position changes.The EKF recovers quickly.
Overall the results for both cases are very good.There a lot of knobs to turn in this framework, so many options to explore.
It is worthy of note at this point that the Unscented Kalman Filter (UKF) [Wan2006], and the more general class of algorithms known as Sigma-Point Kalman Filters (SPKF), are today much preferred to the EKF of the past.The EKF is sub-optimal, and the linearization approach makes it sensitive to initial conditions.The EKF requires the Jacobian matrix, which may be hard to obtain, and may not converge without carefully chosen initial conditions.In this paper the EKF was chosen for use in a training scenario because it is the next logical step from the linear Kalman filter, and its development is simple to follow.The UKF is harder to get explain.In the end, the UKF is of similar complexity to the EKF,  can offer large performance benefits, and does not require the use of a Jacobian.

Conclusions and Future Work
The objective of creating a Jupyter notebook-based simulation tool for studying the use of the EKF in GPS position estimation has been met.There are many tuning options to explore, which provides a very nice environment for studying a large variety scenarios.The performance results are consistent with expectations.
There are several improvements under consideration.The first is to develop a more realistic user trajectory generator.The second is to make measurement quality a function of the SV range, which would also make the measurement quality SV specific, rather than identical as it is now.A third desire is to move to the UKF to avoid the use of the Jacobian, reduce the sensitivity to initial conditions, and improve performance.

Fig. 3 :Fig. 4 :
Fig. 3: Of significance the helper module, GPS_helper.py, contains a class and a 3D plotting function that supports time-varying data set generation of satellite positions and the corresponding User trajectory.

Fig. 5 :
Fig. 5: Jupyter notebook classes that synthesize pseudorange test vectors from the time-varying data set created by GPS_helper.py, and implement the extended Kalman filter for estimating the timevarying User position.

5Fig. 10 :
Fig. 10: The estimated user trajectory in ENU coordinates and the same scale as Figure 7.

Fig. 13 :
Fig. 13: The estimated user trajectory in ENU coordinates and the same scale as Figure 7.

TABLE 1 :
The Kalman filter variables and a brief description.