Использование PyKalman на необработанных данных ускорения для расчета позиции

Question

Это мой первый вопрос о Stackru, поэтому я прошу прощения, если я скажу это плохо. Я пишу код для получения необработанных данных ускорения из IMU, а затем интегрирую их для обновления положения объекта. В настоящее время этот код берет новое показание акселерометра каждую миллисекунду и использует его для обновления положения. В моей системе много шума, что приводит к сумасшедшим показаниям из-за ошибки компоновки, даже с реализованной мной схемой ZUPT. Я знаю, что фильтр Калмана теоретически идеален для этого сценария, и я хотел бы использовать модуль pykalman вместо его создания самостоятельно.

Мой первый вопрос: можно ли использовать pykalman в режиме реального времени, как это? Из документации мне кажется, что вам нужно записать все измерения и затем выполнить плавную операцию, что было бы непрактично, так как я хочу рекурсивно фильтровать каждую миллисекунду.

Мой второй вопрос: для матрицы перехода я могу только применить pykalman к данным ускорения сам по себе, или я могу как-то включить двойную интеграцию в положение? Как будет выглядеть эта матрица?

Если pykalman не подходит для этой ситуации, есть ли другой способ реализовать фильтр Калмана? Заранее спасибо!

Answer 1

В этом случае вы можете использовать фильтр Калмана, но ваша оценка положения будет сильно зависеть от точности вашего сигнала ускорения. Фильтр Калмана полезен для объединения нескольких сигналов. Таким образом, ошибка одного сигнала может быть компенсирована другим сигналом. В идеале вам нужно использовать датчики, основанные на различных физических эффектах (например, IMU для ускорения, GPS для положения, одометрия для скорости).

В этом ответе я собираюсь использовать показания двух датчиков ускорения (оба в направлении X). Один из этих датчиков является дорогим и точным. Второй намного дешевле. Таким образом, вы увидите влияние точности датчика на оценки положения и скорости.

Вы уже упоминали схему ZUPT. Я просто хочу добавить некоторые примечания: очень важно иметь хорошую оценку угла наклона, чтобы избавиться от гравитационной составляющей в вашем X-ускорении. Если вы используете Y- и Z-ускорение, вам нужны углы тангажа и крена.

Начнем с моделирования. Предположим, у вас есть только показания ускорения в направлении X. Таким образом, ваше наблюдение будет выглядеть

Теперь вам нужно определить наименьший набор данных, который полностью описывает вашу систему в каждый момент времени. Это будет состояние системы.

Отображение между областью измерения и состояния определяется матрицей наблюдения:

Теперь вам нужно описать динамику системы. Согласно этой информации, фильтр будет предсказывать новое состояние на основе предыдущего.

В моем случае dt=0,01 с. Используя эту матрицу, фильтр будет интегрировать сигнал ускорения для оценки скорости и положения.

Ковариация R наблюдения может быть описана дисперсией показаний вашего датчика. В моем случае у меня есть только один сигнал в моем наблюдении, поэтому ковариация наблюдения равна дисперсии X-ускорения (значение может быть рассчитано на основе таблицы ваших датчиков).

Через ковариацию перехода Q вы описываете шум системы. Чем меньше значения матрицы, тем меньше шум системы. Фильтр станет жестче, и оценка будет отложена. Вес прошлого системы будет выше по сравнению с новым измерением. В противном случае фильтр будет более гибким и будет сильно реагировать на каждое новое измерение.

Теперь все готово для настройки Пикальмана. Чтобы использовать его в реальном времени, вы должны использовать функцию filter_update.

from pykalman import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt

load_data()

# Data description
#  Time
#  AccX_HP - high precision acceleration signal
#  AccX_LP - low precision acceleration signal
#  RefPosX - real position (ground truth)
#  RefVelX - real velocity (ground truth)

# switch between two acceleration signals
use_HP_signal = 1

if use_HP_signal:
    AccX_Value = AccX_HP
    AccX_Variance = 0.0007
else:    
    AccX_Value = AccX_LP
    AccX_Variance = 0.0020


# time step
dt = 0.01

# transition_matrix  
F = [[1, dt, 0.5*dt**2], 
     [0,  1,       dt],
     [0,  0,        1]]

# observation_matrix   
H = [0, 0, 1]

# transition_covariance 
Q = [[0.2,    0,      0], 
     [  0,  0.1,      0],
     [  0,    0,  10e-4]]

# observation_covariance 
R = AccX_Variance

# initial_state_mean
X0 = [0,
      0,
      AccX_Value[0, 0]]

# initial_state_covariance
P0 = [[  0,    0,               0], 
      [  0,    0,               0],
      [  0,    0,   AccX_Variance]]

n_timesteps = AccX_Value.shape[0]
n_dim_state = 3
filtered_state_means = np.zeros((n_timesteps, n_dim_state))
filtered_state_covariances = np.zeros((n_timesteps, n_dim_state, n_dim_state))

kf = KalmanFilter(transition_matrices = F, 
                  observation_matrices = H, 
                  transition_covariance = Q, 
                  observation_covariance = R, 
                  initial_state_mean = X0, 
                  initial_state_covariance = P0)

# iterative estimation for each new measurement
for t in range(n_timesteps):
    if t == 0:
        filtered_state_means[t] = X0
        filtered_state_covariances[t] = P0
    else:
        filtered_state_means[t], filtered_state_covariances[t] = (
        kf.filter_update(
            filtered_state_means[t-1],
            filtered_state_covariances[t-1],
            AccX_Value[t, 0]
        )
    )


f, axarr = plt.subplots(3, sharex=True)

axarr[0].plot(Time, AccX_Value, label="Input AccX")
axarr[0].plot(Time, filtered_state_means[:, 2], "r-", label="Estimated AccX")
axarr[0].set_title('Acceleration X')
axarr[0].grid()
axarr[0].legend()
axarr[0].set_ylim([-4, 4])

axarr[1].plot(Time, RefVelX, label="Reference VelX")
axarr[1].plot(Time, filtered_state_means[:, 1], "r-", label="Estimated VelX")
axarr[1].set_title('Velocity X')
axarr[1].grid()
axarr[1].legend()
axarr[1].set_ylim([-1, 20])

axarr[2].plot(Time, RefPosX, label="Reference PosX")
axarr[2].plot(Time, filtered_state_means[:, 0], "r-", label="Estimated PosX")
axarr[2].set_title('Position X')
axarr[2].grid()
axarr[2].legend()
axarr[2].set_ylim([-10, 1000])

plt.show()

При использовании лучшего IMU-датчика расчетное положение точно совпадает с истинным значением:

Более дешевый датчик дает значительно худшие результаты:

Я надеюсь, что смогу помочь тебе. Если у вас есть вопросы, я постараюсь на них ответить.