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

Question

我对卡尔曼滤波器概念比较陌生，我想用它来估计和跟踪具有 GPS 测量的车辆位置的准确性（作为第一步）。但是，我不确定我考虑过的假设和参数值，如果我朝着正确的方向前进，我想知道其他用户。 谢谢！！

我考虑了一个标准运动模型：恒定速度（假设加速度对这辆车的位置估计没有影响），因此，我的状态仅包含位置和速度。
????_????+1 = ????_???? + ????˙_???? Δ????
????˙_??????+1 = ????˙_????

因此，状态转移矩阵将是（考虑具有经纬度坐标的 2D 定位 (x,y)）：

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]]

由于我们只有位置测量数据，我们可以将测量矩阵对应地写为：

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

初始条件：
对于初始启动车辆状态 x₀，我假设位置和速度全为零（我确实阅读了几个实现，它们为位置输入了非零值（通常设置为100），但不确定原因）

对于初始不确定性 P₀，我假设了一个对角线设置为 100 的单位矩阵，因为我们不确定初始位置和速度。这个值应该设置得更高吗？当初始位置和速度相对于模型完全已知时，这到底意味着什么？它是世界坐标还是只是某个任意位置？

时间步长（Δ????）：
由于 GPS 以 1 Hz 或每 1 秒更新一次，因此我相应假设滤波器的时间步长相同

噪声值：
过程噪声：我只是假设模型的过程噪声有一个单位矩阵。但在其他实施方式中，还假设过程噪声为零。这是否意味着系统状态没有随机波动？

测量噪声： 由于 GPS 是正在考虑的测量，因此 GPS 读数的标准偏差约为 6 米，被认为是系统的测量噪声。

测量：
我正在使用从应用程序（Strava）导出的 GPX 文件，该文件提供纬度和经度的定位。应该将其转换为米还是我可以直接使用 GPX 文件中的定位数据？

请让我知道上述假设和实现是否正确:)

更新

我直接将 GPS 给出的 Lat Long 数据视为卡尔曼的测量输入，而无需先将其转换为笛卡尔坐标。在下面的代码实现中，数据现在首先转换为 UTM，然后作为测量输入。正如 Kani 所建议的，我将检查为纬度和经度给出的计算转换以及两种技术之间获得的差异。

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

对于卡尔曼滤波器，与任何与物理相关的问题一样，测量单位很重要。如果您使用速度为米/秒，则位置不应该是纬度/经度。您必须将它们转换为米。

您可以做到这一点的一种方法是选择第一个纬度/经度对作为基点，并将所有其他点视为距基点的距离。这不是一个简单的计算，因为它是多个事物的函数。

对于非常短的距离，您可以使用以下等式近似以米为单位的相对位置，r 是地球的半径：

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

这很棘手，但主要有两个原因。

1. 这仅适用于短距离。
2. 地球半径随纬度变化。

Answer 2

设置 Q - 过程噪声协方差矩阵、R - 测量噪声协方差矩阵和 P - 误差协方差矩阵取决于您要跟踪的内容及其条件和难度。尤其是误差协方差矩阵（P）。

我建议你看看 filterpy 库的创建者创建的 jupyter notebook 来解​​释如何正确实现卡尔曼滤波器。