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，以解釋如何正確實現卡爾曼濾波器。