To do this project, we collected our own data for a run around a track.
We collected gyroscope and accelerometer data from the phyphox app (commercially available via the App Store) on an iPhone. This app allows the data to be readily exportable as a csv which made the data collection simple. We collected GPS data using a running smartwatch.
The inputs to the prediction step of the EKF are the heading velocity and the x acceleration. The heading velocity is simply calculated from the gyroscope data while the x acceleration comes straight from the accelerometer data.
The inputs to the correction step are the GPS heading and position data. The position data can be calculated from converting the GPS latitude and longitude into x and y position. The heading data can be calculated using these x and y positions.
The correction step also inputs the theoretical path (for one of our state estimation applications), but there is no sensor data that contributes to this calculation. To create the theoretical path, we can use the known distances of the track shape (using the inside lane). The figure below shows our theoretical track model.
This project uses an Extended Kalman Filter to perform state estimation. Heading velocity, acceleration data, and GPS data are passed into the filter. The output is the estimated path.
Extended Kalman Filters consist of an iterative algorithm between a prediction and a correction step. The prediction step uses a motion model to calculate the predicted state estimation given the measured acceleration and heading. The correction step uses the GPS data to update this prediction based on the measured values. Equations 1-5 below make up this iterative algorithm where the prediction step is shown in Equations 1 and 2, and the correction step is given in Equations 3, 4, and 5.
𝜇t, pr = g ( 𝜇t-1 , ut )
Σx,t, pr = Gx,t Σx,t-1 GTx,t+ Gu,t Σu,t GTu,t
Kt = Σx,t, pr HTt ( Ht Σx,t, pr HTt + Σz,t )-1
𝜇t = 𝜇t, pr + Kt (zt - h ( 𝜇t, pr ))
Σx,t = (I - KtHt) Σx,t, pr
To design the EKF, there are several parameters that were determined. These are the state vector, the motion model, the measurement model, and all vector and matrix calculations that are needed for the EKF localization algorithm. These are presented and described below.
The state vector for the system is 5x1 and is defined in Equation 6. In this state vector, x and y represent the (x,y) position of the runner, ẋ and ẏ represent the x and y speed of the runner, and θ represents the heading of the runner.
𝜇= [x; y; ẋ; ẏ; θ]
To model the motion of the runner in physical terms, a motion model must be incorporated into the design. The equations dictating the motion model for each term in the state vector is given in Equations 7-11.
x(t) = x(t-1) + ẋ delta(t)
y(t) = y(t-1) + ẏ delta(t)
ẋ(t)= ẋ(t-1) + ax delta(t) cosθt-1
ẏ(t)= ẏ(t-1) + ax delta(t) sinθt-1
θ(t) = θ(t-1) + (dθ/dt)delta(t)
These equations can be derived with the knowledge that the input to the system is the x acceleration and heading velocity (both derived from the sensors). We can then use basic motion rules to perform double integration on the acceleration data as shown in Equations 7 and 8 and trigonometry to describe the current positions and velocities of the runner given their previous states and the inputs.
The predicted mean state estimate is simply given by the relationship in Equation 1 where the previous mean state and the inputs are passed through the motion model in Equations 7-11. To calculate the predicted covariance associated with this predicted state, Equation 2 is used. The matrices Gx,t and Gu,t describe the Jacobian for the associated term, namely the state vector and the input vector. Additionally, the covariances for each of these terms as seen in Equation 2 are given as the identity matrices. The Jacobian matrices described are presented below.
Gx,t = [ 1, 0, t, 0, 0;
0, 1, 0, t, 0;
0, 0, 1, 0, -axtsinθt-1;
0, 0, 0, 1, axtcosθt-1;
0, 0, 0, 0, 1]
Gu,t = [ 0, 0;
0, 0;
delta(t) cosθt-1, 0;
delta(t) sinθt-1, 0;
0, t]
At this point in the algorithm, the prediction step is complete. We have the predicted mean state vector for the current time step and the associated covariance. The next step moves into the correction step.
In the correction step, the Kalman Gain must be calculated to aid in weighting either the prediction or measured calculation more. The calculation for the Kalman Gain is presented in Equation 3. As shown, it relies on the calculated covariance from the prediction step, the covariance for the measurements, Σz,t, and the Jacobian for the transformation matrix transforming the measurement space to the state space, Ht. The covariance matrix for the measurements, like before, is given as the identity matrix for simplicity. The h transformation from the measurement to the state space is shown in Equation 12. Its Jacobian is shown directly below.
h(𝜇) = [x(𝜇); y(𝜇); θ(𝜇)]
Ht = [ 1, 0, 0, 0, 0;
0, 1, 0, 0, 0;
0, 0, 0, 0, 1 ]
Note that h(𝜇) is not the same size as the state vector. This is because it must correspond to the size of the measurement model, zt. However, when the Jacobian Ht is calculated, the size must correspond to the size of the covariance matrices, which accounts for the incorporation of the two columns of zeros in Ht.
For this application, the measurement model is given as a vector of the x, y, and θ measurements derived from the GPS data. This can be seen in Equation 13. While the x and y data for the GPS is simply derived from the latitude and longitude, the heading must be calculated. This calculation is presented in Equation 14.
zt = [zx; zy; zθ]
zθ = atan2(y_gps[t]-y_gps[t-1], x_gps[t]-x_gps[t-1])
As seen from Equation 4, this h transformation in Equation 12 is used to compare the measurement model in Equation 13 to the predicted state values. The Kalman Gain then weights this term according to the covariances and produces a final mean state vector. Similarly in Equation 5, the Kalman Gain is used to calculate the state vector’s associated covariance given the estimated covariance and the Jacobian Ht. Thus, with the predicted and then corrected mean state vector and associated covariance, this iteration of the algorithm is complete, and these values, along with the next time step’s input and measurement terms, are passed into the algorithm as the previous time step’s values.
We have experience with implementing Extended Kalman Filters through previous work in E205 State Estimation [Shia, 2022]. Specifically, Lab 3 focused on EKFs [Bloss and Fascetti, 2022].
Additionally, Kalman Filters have been implemented with phone sensor data previously [Wang, 2014]. Specifically, Wang et al. demonstrate that street-level localization accuracy can be improved by using phone sensor data instead of solely GPS.
We seek to build on this work to demonstrate a specific case of phone sensor data as input into an EKF to improve state estimation around a track.
We perform two specific iterations of our EKF for this application.
1) "Normal" implementation of the EKF. In this implementation, the correction step relies only on the GPS data. The block diagram for this system is shown below.
2) EKF correction based on the theoretical track shape. In this implementation, the correction step uses a combination of the GPS data along with the theoretical track shape to output a state estimation. This logic relies on the fact that position can be determined more accurately given that the heading data shows that the runner is on one of the curves of the track. The heading measurement in combination with the predicted (x,y) location gives more confidence that the runner's location on the curve is known as a heading angle maps directly to a (x,y) location on the track. The block diagram for this system is shown below.
