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，以解释如何正确实现卡尔曼滤波器。