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提供的纬度数据作为对卡尔曼的测量输入，而没有先将其转换为笛卡尔坐标。在下面的代码实现中，现在首先将数据转换为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)

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

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

Answer 2

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

我建议您看一下filterpy库的创建者创建的jupyter笔记本，以解释如何正确实现Kalman过滤器。

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

2 个答案: