Utilisation de PyKalman sur des données d'accélération brutes pour calculer la position
Ceci est ma première question sur Stackoverflow, donc je m'excuse si je le dis mal. J'écris du code pour prendre des données d'accélération brutes d'une IMU puis les intégrer pour mettre à jour la position d'un objet. Actuellement, ce code prend un nouvel accéléromètre lisant chaque milliseconde, et l'utilise pour mettre à jour la position. Mon système a beaucoup de bruit, ce qui entraîne des lectures folles en raison d'une erreur de composition, même avec le schéma ZUPT que j'ai mis en œuvre. Je sais qu'un filtre de Kalman est théoriquement idéal pour ce scénario, et je voudrais utiliser le module pykalman au lieu d'en créer un moi-même.
Ma première question est la suivante: pykalman peut-il être utilisé en temps réel comme celui-ci? D'après la documentation, il me semble que vous devez avoir un enregistrement de toutes les mesures, puis effectuer le bon fonctionnement, ce qui ne serait pas pratique car je veux filtrer récursivement toutes les millisecondes.
Ma deuxième question est la suivante: pour la matrice de transition, puis-je appliquer uniquement pykalman aux données d'accélération par lui-même, ou puis-je en quelque sorte inclure la double intégration à la position? À quoi ressemblerait cette matrice?
Si pykalman n'est pas pratique dans cette situation, existe-t-il une autre façon de mettre en œuvre un filtre de Kalman? Merci d'avance!
Vous pouvez utiliser un filtre de Kalman dans ce cas, mais votre estimation de position dépendra fortement de la précision de votre signal d'accélération. Le filtre de Kalman est en fait utile pour une fusion de plusieurs signaux. Ainsi, l'erreur d'un signal peut être compensée par un autre signal. Idéalement, vous devez utiliser des capteurs basés sur différents effets physiques (par exemple, un IMU pour l'accélération, le GPS pour la position, l'odométrie pour la vitesse).
Dans cette réponse, je vais utiliser les lectures de deux capteurs d'accélération (tous deux dans la direction X). L'un de ces capteurs est vaste et précis. Le second est beaucoup moins cher. Vous verrez donc l'influence de la précision du capteur sur les estimations de position et de vitesse.
Vous avez déjà mentionné le programme ZUPT. Je veux juste ajouter quelques notes: il est très important d'avoir une bonne estimation de l'angle de tangage, pour se débarrasser de la composante gravitationnelle de votre accélération X. Si vous utilisez les accélérations Y et Z, vous avez besoin des angles de tangage et de roulis.
Commençons par la modélisation. Supposons que vous n'ayez que des lectures d'accélération dans la direction X. Donc, votre observation ressemblera à
Vous devez maintenant définir le plus petit ensemble de données, qui décrit complètement votre système à chaque instant. Ce sera l'état du système.
La cartographie entre les domaines de mesure et d'état est définie par la matrice d'observation:
Vous devez maintenant décrire la dynamique du système. Selon ces informations, le filtre prédira un nouvel état basé sur le précédent.
Dans mon cas, dt = 0,01 s. En utilisant cette matrice, le filtre intégrera le signal d'accélération pour estimer la vitesse et la position.
La covariance d'observation R peut être décrite par la variance de vos lectures de capteur. Dans mon cas, je n'ai qu'un seul signal dans mon observation, donc la covariance d'observation est égale à la variance de l'accélération X (la valeur peut être calculée en fonction de la fiche technique de vos capteurs).
A travers la covariance de transition Q, vous décrivez le bruit du système. Plus les valeurs matricielles sont petites, plus le bruit du système est faible. Le filtre deviendra plus rigide et l'estimation sera retardée. Le poids du passé du système sera plus élevé par rapport à une nouvelle mesure. Sinon, le filtre sera plus flexible et réagira fortement à chaque nouvelle mesure.
Maintenant, tout est prêt pour configurer le Pykalman. Pour l'utiliser en temps réel, vous devez utiliser la fonction 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()
Lorsque vous utilisez le meilleur capteur IMU, la position estimée est exactement la même que la vérité au sol:
Le capteur moins cher donne des résultats nettement moins bons:
J'espère que je pourrais vous aider. Si vous avez des questions, je vais essayer d'y répondre.
[~ # ~] mise à jour [~ # ~]
Si vous voulez expérimenter avec différentes données, vous pouvez les générer facilement (malheureusement je n'ai plus les données originales).
Voici un simple script matlab pour générer un ensemble de capteurs de référence, bons et mauvais.
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');
Les données simulées ressemblent à peu près aux données ci-dessus.
