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

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

转载作者：行者123 更新时间：2023-11-28 17:57:16

我对卡尔曼滤波器的概念比较陌生，我想用它来估计和跟踪带有 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 数据作为 Kalman 的测量输入，而没有先将其转换为笛卡尔坐标。在下面的代码实现中，数据现在首先转换为 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()