Python中车辆位置估计的卡尔曼滤波器参数定义

Question

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 x ₀ , 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 P ₀ , 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

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.

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()

Answer 1

For the Kalman filter, as with any physics related porblem, the unit of the measurement matters. If you are using velocity as meters per second, the position should not be in latitude/longitude. You must convert them to meters.

One way you can do that is by selecting the first latitude/longitude pair as the base point and treating all the other points as a distance traveled from the base point. This is not a simple calculation as it is a function of multiple things.

For really short distances, you can approximate the relative position in meters using the following equations with r being the radius of the earth:

• distance along latitude = r * deg_to_rad(latitude - base latitude)
• distance along longitude = 2 * r * asin(cos(base latitude)) * sin(pi / 180 / 2)) * deg_to_rad(longitude - base longitude)

This is tricky for two main reasons though.

1. This is only valid for short distances.
2. Earth's radius changes with latitude.

Answer 2

Setting Q - process noise covariance matrix, R - measurement noise covariance matrix and P - error covariance matrix depends on what you are trying to track and its conditions and its quite difficult. Especially error covariance matrix (P).

I suggest that you take a look at the jupyter notebook the creator of filterpy library has created to explain how correctly implement Kalman filters.