title: Kalman Filter Parameter Definition for Vehicle Position Estimation in Python
body: <p>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!!</p>
  
  <p>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.<br>
  <sub>+1</sub> = <sub></sub> + ˙<sub></sub> Δ<br>
   ˙<sub>+1</sub> = ˙<sub></sub>   </p>
  
  <p>Therefore, the state transition matrix would be (Considering 2D positioning (x,y) with latitude and longitude coordinates):</p>
  
  <pre><code>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]]
  </code></pre>
  
  <p>Since we only have position measurement data available, we can correspondingly write the measurement matrix as:  </p>
  
  <pre><code>H = [[1.0, 0.0, 0.0, 0.0],
       [0.0, 1.0, 0.0, 0.0]]
  </code></pre>
  
  <p>Initial Conditions:<br>
  For the initial starting vehicle state x<sub>0</sub>, 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)  </p>
  
  <p>For the initial uncertainty P<sub>0</sub>, 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?  </p>
  
  <p>Time Step ( Δ ):<br>
  Since GPS updates at 1 Hz or every 1 second, I have correspondingly assumed the same for the time step of the filter</p>
  
  <p>Noise Values:<br>
  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?  </p>
  
  <p>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.  </p>
  
  <p>Measurement:<br>
  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? </p>
  
  <p>Please let me know if the above assumptions and implementations are correct :)</p>
  
  <p><strong>UPDATE</strong></p>
  
  <p>I directly considered the Lat Long data given by the GPS as a measurement input to the Kalman without first converting it to Cartesian. In the below code implementation, the data is now first converted to UTM before given as a measurement Input. As suggested by Kani, I will check the calculation conversion given for latitude and longitude and the difference obtained between the 2 techniques. </p>
  
  <pre><code>import gpxpy
  import pandas as pd
  import numpy as np
  import utm
  import matplotlib.pyplot as plt
  
  with open('test3.gpx') as fh:
      gpx_file = gpxpy.parse(fh)
  segment = gpx_file.tracks[0].segments[0]
  coords = pd.DataFrame([
      {'lat': p.latitude,
       'lon': p.longitude,
       'ele': p.elevation,
       'time': p.time} for p in segment.points])
  coords.head(3)
  plt.plot(coords.lon[::18], coords.lat[::18],'ro')
  plt.show()
  #plt.plot(coords.lon, coords.lat)
  
  def lat_log_posx_posy(coords):
  
       px, py = [], []
       for i in range(len(coords.lat)):
           dx = utm.from_latlon(coords.lat[i], coords.lon[i])
           px.append(dx[0])
           py.append(dx[1])
       return px, py
  
  def kalman_xy(x, P, measurement, R,
                Q = np.array(np.eye(4))):
  
      return kalman(x, P, measurement, R, Q,
                    F=np.array([[1.0, 0.0, 1.0, 0.0],
                                [0.0, 1.0, 0.0, 1.0],
                                [0.0, 0.0, 1.0, 0.0],
                                [0.0, 0.0, 0.0, 1.0]]),
                    H=np.array([[1.0, 0.0, 0.0, 0.0],
                                [0.0, 1.0, 0.0, 0.0]]))
  
  def kalman(x, P, measurement, R, Q, F, H):
  
      y = np.array(measurement).T - np.dot(H,x)
      S = H.dot(P).dot(H.T) + R  # residual convariance
      K = np.dot((P.dot(H.T)), np.linalg.pinv(S))
      x = x + K.dot(y)
      I = np.array(np.eye(F.shape[0]))  # identity matrix
      P = np.dot((I - np.dot(K,H)),P)
  
      # PREDICT x, P
      x = np.dot(F,x)
      P = F.dot(P).dot(F.T) + Q
  
      return x, P
  
  def demo_kalman_xy():
  
      px, py = lat_log_posx_posy(coords)
      plt.plot(px[::18], py[::18], 'ro')
      plt.show()
  
      x = np.array([px[0], py[0], 0.01, 0.01]).T
      P = np.array(np.eye(4))*1000 # initial uncertainty
      result = []
      R = 0.01**2
      for meas in zip(px, py):
          x, P = kalman_xy(x, P, meas, R)
          result.append((x[:2]).tolist())
      kalman_x, kalman_y = zip(*result)
      plt.plot(px[::18], py[::18], 'ro')
      plt.plot(kalman_x, kalman_y, 'g-')
      plt.show()
  
  demo_kalman_xy()
  </code></pre>