Я относительно новичок в концепциях фильтра Калмана, и я хотел бы использовать его для оценки и отслеживания точности положения транспортного средства с помощью измерений 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]]
Начальные условия:
Для начального начального состояния транспортного средства x0 я принял все нули для позиции и скорости (я читал пару реализаций, где они ввели ненулевое значение для позиция (обычно устанавливается на 100, но я не уверен в причине этого)
Для начальной неопределенности P0 я принял единичную матрицу с диагоналями, равными 100, поскольку мы не уверены в начальном положении и скорости. Следует ли установить это значение выше? Что именно это означает, когда начальное положение и скорость точно известны по отношению к модели? Это мировые координаты или просто какая-то произвольная позиция?
Временной шаг ( Δ???? ):
Поскольку GPS обновляется с частотой 1 Гц или каждую 1 секунду, я соответственно предположил то же самое для временного шага фильтра.
Значения шума:
Шум процесса: я только что принял единичную матрицу для шума процесса модели. Но в других реализациях также предполагается, что шум процесса равен нулю. Значит ли это, что нет случайных колебаний состояний системы?
Шум измерения: поскольку GPS является рассматриваемым измерением, стандартное отклонение показаний GPS составляет приблизительно 6 метров и считается шумом измерения для системы.
Измерение:
я использую файл GPX, экспортированный из приложения (Strava), который дает позиционирование по широте и долготе. Должен ли он быть преобразован в метры или я могу напрямую использовать данные позиционирования из файла GPX?
Пожалуйста, дайте мне знать, верны ли приведенные выше предположения и реализации :)
ОБНОВЛЕНИЕ
Я напрямую рассматривал данные широты и долготы, предоставленные GPS, как входные данные измерений для Калмана, не преобразовывая их сначала в декартовы значения. В приведенной ниже реализации кода данные теперь сначала преобразуются в UTM, а затем передаются в качестве входных данных измерения. Как предложил Кани, я проверю преобразование вычислений, данное для широты и долготы, и разницу, полученную между двумя методами.
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()
