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

Question

我对卡尔曼滤波器概念相对较新，我想用它来通过 GPS 测量来估计和跟踪车辆位置的准确性（作为第一步）。但是，我不确定我所考虑的假设和参数值，并且希望其他用户知道我是否朝着正确的方向前进。\n谢谢！！

\n\n

我考虑了一个标准运动模型：恒定速度（假设加速度对该车辆的位置估计没有影响），因此，我的状态仅包含位置和速度。
\n ₊₁ =_{+ \xcb\x99\xce\x94
\n \xcb\x99 +1 = \xcb\x99}

\n\n

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

\n\n

A = [[1.0, 0.0, \xce\x94, 0.0],\n     [0.0, 1.0, 0.0, \xce\x94],\n     [0.0, 0.0, 1.0, 0.0],\n     [0.0, 0.0, 0.0, 1.0]]\n

\n\n

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

\n\n

H = [[1.0, 0.0, 0.0, 0.0],\n     [0.0, 1.0, 0.0, 0.0]]\n

\n\n

初始条件：
\n对于初始启动车辆状态 x ₀，我假设位置和速度全为零（我确实阅读了一些实现，其中他们输入了非零位置值（通常设置为 100），但我不确定这样做的原因）

\n\n

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

\n\n

时间步长 ( \xce\x94 ):
\n由于 GPS 以 1 Hz 或每 1 秒更新一次，因此我相应地假设滤波器的时间步长相同

\n\n

噪声值：
\n过程噪声：我只是假设模型过程噪声的单位矩阵。但在其他实现中，也假设过程噪声为零。这是否意味着系统状态不存在随机波动？

\n\n

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

\n\n

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

\n\n

请告诉我上述假设和实现是否正确:)

\n\n

更新

\n\n

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

\n\n

import gpxpy\nimport pandas as pd\nimport numpy as np\nimport utm\nimport matplotlib.pyplot as plt\n\nwith open('test3.gpx') as fh:\n    gpx_file = gpxpy.parse(fh)\nsegment = gpx_file.tracks[0].segments[0]\ncoords = pd.DataFrame([\n    {'lat': p.latitude,\n     'lon': p.longitude,\n     'ele': p.elevation,\n     'time': p.time} for p in segment.points])\ncoords.head(3)\nplt.plot(coords.lon[::18], coords.lat[::18],'ro')\nplt.show()\n#plt.plot(coords.lon, coords.lat)\n\ndef lat_log_posx_posy(coords):\n\n     px, py = [], []\n     for i in range(len(coords.lat)):\n         dx = utm.from_latlon(coords.lat[i], coords.lon[i])\n         px.append(dx[0])\n         py.append(dx[1])\n     return px, py\n\ndef kalman_xy(x, P, measurement, R,\n              Q = np.array(np.eye(4))):\n\n    return kalman(x, P, measurement, R, Q,\n                  F=np.array([[1.0, 0.0, 1.0, 0.0],\n                              [0.0, 1.0, 0.0, 1.0],\n                              [0.0, 0.0, 1.0, 0.0],\n                              [0.0, 0.0, 0.0, 1.0]]),\n                  H=np.array([[1.0, 0.0, 0.0, 0.0],\n                              [0.0, 1.0, 0.0, 0.0]]))\n\ndef kalman(x, P, measurement, R, Q, F, H):\n\n    y = np.array(measurement).T - np.dot(H,x)\n    S = H.dot(P).dot(H.T) + R  # residual convariance\n    K = np.dot((P.dot(H.T)), np.linalg.pinv(S))\n    x = x + K.dot(y)\n    I = np.array(np.eye(F.shape[0]))  # identity matrix\n    P = np.dot((I - np.dot(K,H)),P)\n\n    # PREDICT x, P\n    x = np.dot(F,x)\n    P = F.dot(P).dot(F.T) + Q\n\n    return x, P\n\ndef demo_kalman_xy():\n\n    px, py = lat_log_posx_posy(coords)\n    plt.plot(px[::18], py[::18], 'ro')\n    plt.show()\n\n    x = np.array([px[0], py[0], 0.01, 0.01]).T\n    P = np.array(np.eye(4))*1000 # initial uncertainty\n    result = []\n    R = 0.01**2\n    for meas in zip(px, py):\n        x, P = kalman_xy(x, P, meas, R)\n        result.append((x[:2]).tolist())\n    kalman_x, kalman_y = zip(*result)\n    plt.plot(px[::18], py[::18], 'ro')\n    plt.plot(kalman_x, kalman_y, 'g-')\n    plt.show()\n\ndemo_kalman_xy()\n

\n

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. 地球半径随纬度变化。