# Kalman Filter Parameter Definition for Vehicle Position Estimation in Python

I'm relatively new to Kalman filter concepts and I would like to use it for estimating and tracking the accuracy of the position of a vehicle with GPS measurements (As a first step). However, I am not sure of the assumptions and parameter values that I have considered , and would like to know by other users if I'm headed in the right direction. Thanks!!

I've considered a standard motion model: Constant Velocity (Assuming that acceleration plays no effect on this vehicle's position estimation) and therefore, my states consist of only position and velocity.
𝑥𝑘+1 = 𝑥𝑘 + 𝑥˙𝑘 Δ𝑡
𝑥˙𝑘+1 = 𝑥˙𝑘

Therefore, the state transition matrix would be (Considering 2D positioning (x,y) with latitude and longitude coordinates):

A = [[1.0, 0.0, Δ𝑡, 0.0],
[0.0, 1.0, 0.0, Δ𝑡],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]]


Since we only have position measurement data available, we can correspondingly write the measurement matrix as:

H = [[1.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0]]


Initial Conditions:
For the initial starting vehicle state x0, I assumed all zeroes for the position and velocity (I did read a couple of implementations where they've entered a non-zero value for the position (Usually set to 100), but am not sure about the reason for this)

For the initial uncertainty P0, I assumed an identity matrix with diagonals set to 100 since we are uncertain about the initial position and velocity. Should this value be set higher? What exactly does it mean when the initial position and velocity are known perfectly with respect to the model? Is it the world coordinates or just some arbitrary position?

Time Step ( Δ𝑡 ):
Since GPS updates at 1 Hz or every 1 second, I have correspondingly assumed the same for the time step of the filter

Noise Values:
Process Noise: I just assumed an identity matrix for the process noise of the model. But in other implementations, it is also assumed that process noise is zero. Does that mean, there are no random fluctuations of the system states?

Measurement Noise: Since GPS is the measurement under consideration, the standard deviation for a GPS reading is approximately 6 metres and is considered to be the measurement noise for the system.

Measurement:
I'm using a GPX file exported from an app (Strava) that gives the positioning for latitude and longitude. Should this be converted to metres or can I directly use the positioning data from the GPX file?

Please let me know if the above assumptions and implementations are correct :)

UPDATE

As David pointed out, I was carrying out my implementation by directly considering the lat long coordinates as measurement data without first converting them to Cartesian. My filter was therefore showing an incorrect behaviour and this is now rectified by converting the lat long data to projected 2D cartesian coordinates through the UTM package. While the Kalman filter is behaving as expected, there are still a lot of question marks with regards to the values to be set for the initial uncertainty, process noise and the measurement noise.

1. In the initial uncertainty, should the position values Px and Py be set to 0 since we already know the first position from the measurement? Should the velocity be set to a very high value to account for unknown initial velocity?
2. For the values of the process noise, should we consider that the velocity can change even though we assume a constant velocity model and therefore, needs to be set to a high value? And the position is also affected by the change in velocity (As defined in the Kinematic equation) and hence, needs to be set to a suitably high value?
3. With regards to the measurement noise, should we consider the Standard deviation of signal received at the GPS receiver (Usually about 6 metres) as the measurement noise or is it to be calculated from the data points (Eg: Calculate from 100 points)?
• One issue you have - the constant velocity model you have is for a Cartesian coordinate system. Unfortunately, Lat & Long is not a Cartesian system. One of the common problems in angular coordinates is dealing with the wrap around. Consider a bearing measurement going 358$^\circ$, 359$^\circ$, 0$^\circ$. If the kalman predictis 359.9$^\circ$ but the measurement is 0.1$^\circ$ then it looks like a big error, but there really isn't. – David Aug 24 '19 at 11:42
• Thanks David, I was not aware of this problem! How do I correct this issue? Do I need to convert my positional data from degrees into metres? – surajr Aug 25 '19 at 8:45
• This is my code so far that I have implemented: But, it is not working as expected and don't know the root cause of that problem: dsp.stackexchange.com/questions/60307/… – surajr Aug 25 '19 at 8:47
• There are two ways. 1- Come up with a version of the constant velocity model in the lat/long coordinates. You might want to look and see if this had been done before. 2 - Convert the lat/long measure to a local Cartesian system. This keeps the state transition as a linear model, by the measurement model is non-linear, so you'll need to use an Extended Kalman filter or some other method to handle the non-linearity. Not sure if I'll have time to look at your code to figure out what might be the issue. Someone has a Python tutorial on the Kalman filters and more on Github. – David Aug 25 '19 at 12:48
• Perfect David. After converting the lat long data to UTM, the Kalman output was as expected. Thanks for the hint :) In addition, I have 2 queries to your comment: 1. This is a really silly question but I did not understand what you meant by the measurement model being non-linear. Isn't that the case even before converting the lat long to Cartesian? 2. For the initialisation of the uncertainty matrix in the Extended Kalman Filter, can we directly set the value of the position uncertainty to 0 (Since we know the position from the data) and only set the uncertaity of velocity to be really high? – surajr Aug 27 '19 at 19:11