Cette page a été traduite par traduction automatique. Cliquez ici pour voir la dernière version en anglais.

estimateStates

Fusion par lots et lissage des données des capteurs

Depuis R2022a

Syntaxe

estimates = estimateStates(filter,sensorData,measurementNoise)

[estimates,smoothEstimates] = estimateStates(___)

Description

estimates = estimateStates(filter,sensorData,measurementNoise) renvoie les estimations d'état basées sur le modèle de mouvement utilisé dans le filtre, les données du capteur et le bruit de mesure. La fonction prédit les estimations de l'état du filtre dans le temps en fonction des temps de ligne dans sensorData et fusionne les données de chaque colonne du tableau une par une.

[estimates,smoothEstimates] = estimateStates(___) renvoie en outre les estimations d'état lissées en utilisant le lisseur de Kalman non linéaire Rach-Tung-Striebel (RTS). Pour plus de détails sur l'algorithme, voir Algorithmes et [1].

Conseil

Le lissage nécessite généralement beaucoup plus de mémoire et de temps de calcul. Utilisez cette syntaxe uniquement lorsque vous avez besoin des états estimés lissés.

Exemples

Fusion par lots et lissage d'état à l'aide de insEKF

Chargez les données de mesure d’un accéléromètre et d’un gyroscope.

load("accelGyroINSEKFData.mat");

Créez un objet filtre insEKF . Spécifiez la partie orientation de l’état dans le filtre en utilisant l’orientation initiale des données de mesure. Spécifiez les éléments diagonaux de la matrice de covariance d'erreur d'estimation d'état correspondant à l'état d'orientation comme 0.01.

filt = insEKF;
stateparts(filt,"Orientation",compact(initOrient));
statecovparts(filt,"Orientation",1e-2);

Spécifiez le bruit de mesure et le bruit de processus additif. Vous pouvez obtenir ces valeurs en utilisant la fonction objet tune de l'objet filtre.

measureNoise = struct("AccelerometerNoise", 0.1739, ...
    "GyroscopeNoise", 1.1129);
processNoise = diag([ ...
    2.8586 1.3718 0.8956 3.2148 4.3574 2.5411 3.2148 0.5465 0.2811 ...
    1.7149 0.1739 0.7752 0.1739]);
filt.AdditiveProcessNoise = processNoise;

Estimez par lots les états à l'aide de la fonction objet estimateStates . Obtenez également les estimations après lissage.

[estimates,smoothEstimates] = estimateStates(filt,sensorData,measureNoise);

Visualisez l'orientation estimée dans les angles d'Euler.

figure
t = estimates.Properties.RowTimes;
plot(t,eulerd(estimates.Orientation,"ZYX","frame"));
title("Estimated Orientation");
ylabel("Degrees")

Visualisez l'orientation estimée après lissage aux angles d'Euler.

figure
plot(t,eulerd(smoothEstimates.Orientation,"ZYX","frame"));
title("Smoothed Orientation");
ylabel("Degrees")

Visualisez l'erreur d'estimation, en distance quaternion, en utilisant la fonction objet dist de l'objet quaternion .

trueOrient = groundTruth.Orientation;
plot(t,rad2deg(dist(estimates.Orientation, trueOrient)), ...
     t,rad2deg(dist(smoothEstimates.Orientation, trueOrient)));
title("Estimated and Smoother Error");
legend("Estimation Error","Smoothed Error")
xlabel("Time");
ylabel("Degrees")

Arguments d'entrée

filter — Filtre INS

Objet insEKF

Filtre INS, spécifié comme objet insEKF .

sensorData — Données du capteur

timetable

Données du capteur, spécifiées sous la forme timetable. Chaque nom de variable (sous forme de colonne) dans le calendrier doit correspondre à l'un des noms de capteurs spécifiés dans la propriété SensorNames du filter. Chaque entrée du tableau correspond à la mesure du capteur au temps de ligne correspondant.

Si un capteur ne produit pas de mesures à un moment donné, spécifiez l'entrée correspondante comme NaN.

measurementNoise — Bruit de mesure

structure

Bruit de mesure des capteurs, spécifié comme structure. Chaque nom de champ doit correspondre à l'un des noms de capteur spécifiés dans la propriété SensorNames du filter. La valeur du champ est la matrice de covariance du bruit de mesure correspondante. Si vous spécifiez une valeur de champ comme scalaire, la fonction étend le scalaire jusqu'à la diagonale de la matrice.

Types de données : struct

Arguments de sortie

estimates — Estimations de l'État

timetable

Estimations de l'État, renvoyées sous la forme timetable. Le nom de chaque variable du tableau représente un état. Vous pouvez obtenir les noms des variables en utilisant la fonction objet stateinfo du filtre. La dernière colonne du tableau est la matrice de covariance d'erreur d'estimation d'état pour le vecteur d'état complet du filtre à chacun des instants de ligne.

smoothEstimates — Estimations d’état lissées

timetable

Estimations d'état lissées, renvoyées sous la forme d'un timetable. Le nom de chaque variable du tableau représente un état. Vous pouvez obtenir les noms des variables en utilisant la fonction objet stateinfo du filtre. La dernière colonne du tableau est la matrice de covariance d'erreur d'estimation d'état pour le vecteur d'état complet du filtre à chacun des instants de ligne.

Algorithmes

RTS plus fluide

Considérons un modèle non linéaire discret continu comme suit.

$$\begin{array}{l}\frac{d}{dt}x(t)=f(x(t),t)+w(t),w(t)~N(0,Q(t))\\ y_k=h(x_k)+v_k,v_k~N(0,R_k)\end{array}$$

Dans l'équation, t représente le temps système continu, x est l'état du système, f est l'équation d'état, et w est le bruit de processus qui suit une distribution normale de moyenne 0 et de covariance Q. k est le pas de temps discret, y est la mesure, h est la fonction de mesure, v est le bruit de mesure qui suit une distribution normale de moyenne 0 et de covariance R.

Considérons une période de temps [0, T], où T est la durée totale prise en compte pour le lissage. Le plus fluide effectue d'abord un filtrage direct pour t ∈ [0, T] en utilisant un filtre de Kalman étendu discret continu régulier. Finalement, le lisseur obtient l'estimation de l'état prévisionnel x f (T) et l'estimation de la covariance prospective P f (T) à la dernière heure. Le lisseur enregistre également les estimations d'état et les covariances aux étapes intermédiaires lorsqu'il corrige l'état estimé avec des mesures.

Ensuite, le lisseur obtient l’état lissé en utilisant un filtre arrière. Pour plus de commodité, définissez une variable τ = T - t représentant le temps de retour. Le filtre arrière obtient l'état lissé x s et la covariance P s à chaque instant de mesure en utilisant l'intégration descendante avec ces équations.

$$\begin{array}{l}{K}_f(t)=Q{P}_f^{-1}(t)\\ \frac{d}{d\tau }{P}_s(t)=-\left[F\left(x_f(t),t\right)+K_f(t)\right]P_f(t)-P_f(t){\left[F\left(x_f(t),t\right)+K_f(t)\right]}^T+Q(t),P_s(T)=P_f(T)\\ \frac{d}{d\tau }{x}_s(t)=-\left[F\left(x_f(t),t\right)+K_f(t)\right]\left[x_s(t)-x_f(t)\right]-f(x_f(t),t),x_s(T)=x_f(T)\end{array}$$

Dans ces équations, K f (t) est le gain de Kalman et F = ∂ f(x, t)/ ∂ x est le Matrice jacobienne du modèle d'état.

Références

[1] Crassidis, John L., and John L. Junkins. "Optimal Estimation of Dynamic Systems". 2nd ed, CRC Press, pp. 349- 352, 2012.

Capacités étendues

Génération de code C/C++
Générez du code C et C++ avec MATLAB® Coder™.

Historique des versions

Introduit dans R2022a

R2023a: Estimations d'état fluides à l'aide de insEKF

Vous pouvez désormais obtenir des estimations d'état lissées comme deuxième sortie de la fonction estimateStates . La fonction utilise l'algorithme de lissage Rauch-Tung-Striebel (RTS) pour obtenir les estimations d'état lissées.

Voir aussi

predict | fuse | residual | correct | stateparts | statecovparts | stateinfo | tune | createTunerCostTemplate | tunerCostFcnParam