В этом случае вы можете использовать фильтр Калмана, но оценка вашего положения будет сильно зависеть от точности вашего сигнала ускорения. Фильтр Калмана на самом деле полезен для слияния нескольких сигналов. Таким образом, ошибка одного сигнала может быть компенсирована другим сигналом. В идеале вам нужно использовать датчики, основанные на различных физических эффектах (например, IMU для ускорения, GPS для положения, одометрия для скорости).
В этом ответе я буду использовать показания двух датчиков ускорения (оба в направлении X). Один из этих датчиков является экспансивным и точным. Второй намного дешевле. Таким образом, вы увидите влияние точности датчика на оценку положения и скорости.
Вы уже упомянули схему ЗУПТ. Я просто хочу добавить несколько замечаний: очень важно иметь хорошую оценку угла тангажа, чтобы избавиться от гравитационной составляющей в вашем X-ускорении. Если вы используете Y- и Z-ускорение, вам нужны углы как тангажа, так и крена.
Начнем с моделирования. Предположим, у вас есть только показания ускорения в направлении X. Таким образом, ваше наблюдение будет выглядеть
Теперь вам нужно определить наименьший набор данных, который полностью описывает вашу систему в каждый момент времени. Это будет состояние системы.
Отображение между областями измерения и состояния определяется матрицей наблюдения:
Теперь нужно описать системную динамику. По этой информации Фильтр будет предсказывать новое состояние на основе предыдущего.
В моем случае dt=0,01 с. Используя эту матрицу, фильтр будет интегрировать сигнал ускорения для оценки скорости и положения.
Ковариация наблюдения R может быть описана дисперсией показаний вашего датчика. В моем случае у меня есть только один сигнал в моем наблюдении, поэтому ковариация наблюдения равна дисперсии X-ускорения (значение может быть рассчитано на основе таблицы данных ваших датчиков).
Через переходную ковариацию Q вы описываете системный шум. Чем меньше значения матрицы, тем меньше системный шум. Фильтр станет жестче, и оценка будет отложена. Вес прошлого системы будет выше по сравнению с новым измерением. В противном случае фильтр будет более гибким и будет сильно реагировать на каждое новое измерение.
Теперь все готово для настройки Pykalman. Чтобы использовать его в режиме реального времени, вы должны использовать функцию 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-сенсора предполагаемое положение точно такое же, как и наземная правда:
Более дешевый датчик дает значительно худшие результаты:
Я надеюсь, что смог помочь вам. Если у вас есть какие-то вопросы, я постараюсь на них ответить.
ОБНОВЛЕНИЕ
Если вы хотите поэкспериментировать с другими данными, вы можете легко их сгенерировать (к сожалению, у меня больше нет исходных данных).
Вот простой скрипт Matlab для создания эталонного, хорошего и плохого набора датчиков.
clear;
dt = 0.01;
t=0:dt:70;
accX_var_best = 0.0005; % (m/s^2)^2
accX_var_good = 0.0007; % (m/s^2)^2
accX_var_worst = 0.001; % (m/s^2)^2
accX_ref_noise = randn(size(t))*sqrt(accX_var_best);
accX_good_noise = randn(size(t))*sqrt(accX_var_good);
accX_worst_noise = randn(size(t))*sqrt(accX_var_worst);
accX_basesignal = sin(0.3*t) + 0.5*sin(0.04*t);
accX_ref = accX_basesignal + accX_ref_noise;
velX_ref = cumsum(accX_ref)*dt;
distX_ref = cumsum(velX_ref)*dt;
accX_good_offset = 0.001 + 0.0004*sin(0.05*t);
accX_good = accX_basesignal + accX_good_noise + accX_good_offset;
velX_good = cumsum(accX_good)*dt;
distX_good = cumsum(velX_good)*dt;
accX_worst_offset = -0.08 + 0.004*sin(0.07*t);
accX_worst = accX_basesignal + accX_worst_noise + accX_worst_offset;
velX_worst = cumsum(accX_worst)*dt;
distX_worst = cumsum(velX_worst)*dt;
subplot(3,1,1);
plot(t, accX_ref);
hold on;
plot(t, accX_good);
plot(t, accX_worst);
hold off;
grid minor;
legend('ref', 'good', 'worst');
title('AccX');
subplot(3,1,2);
plot(t, velX_ref);
hold on;
plot(t, velX_good);
plot(t, velX_worst);
hold off;
grid minor;
legend('ref', 'good', 'worst');
title('VelX');
subplot(3,1,3);
plot(t, distX_ref);
hold on;
plot(t, distX_good);
plot(t, distX_worst);
hold off;
grid minor;
legend('ref', 'good', 'worst');
title('DistX');
Смоделированные данные выглядят почти так же, как данные выше.
person
Anton
schedule
03.01.2018
