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

ahrs10filter

Hauteur et orientation à partir des lectures MARG et altimétriques

Description

L'objet ahrs10filter fusionne les données du capteur MARG et de l'altimètre pour estimer la hauteur et l'orientation de l'appareil. Les données MARG (magnétique, vitesse angulaire, gravité) sont généralement dérivées de capteurs magnétomètres, gyroscopes et accéléromètres. Le filtre utilise un vecteur d'état à 18 éléments pour suivre le quaternion d'orientation, la vitesse verticale, la position verticale, les biais du capteur MARG et le vecteur géomagnétique. L'objet ahrs10filter utilise un filtre de Kalman étendu pour estimer ces quantités.

Création

Syntaxe

FUSE = ahrs10filter

FUSE = ahrs10filter('ReferenceFrame',RF)

FUSE = ahrs10filter(___,Name,Value)

Description

FUSE = ahrs10filter renvoie un objet filtre de Kalman étendu, FUSE, pour la fusion des capteurs de MARG et des lectures d'altimètre afin d'estimer la hauteur et l'orientation de l'appareil.

FUSE = ahrs10filter('ReferenceFrame',RF) renvoie un objet filtre de Kalman étendu qui estime la hauteur et l'orientation de l'appareil par rapport au cadre de référence RF. Spécifiez RF comme 'NED' (Nord-Est-Bas) ou 'ENU' (Est-Nord-Up). La valeur par défaut est 'NED'.

FUSE = ahrs10filter(___,Name,Value) définit chaque propriété Name au Value spécifié. Les propriétés non spécifiées ont des valeurs par défaut.

Propriétés

IMUSampleRate — Taux d'échantillonnage de l'IMU (Hz)

100 (par défaut) | scalaire positif

Taux d'échantillonnage de l'IMU en Hz, spécifié sous forme de scalaire positif.

Types de données : single | double

GyroscopeNoise — Variation multiplicative du bruit de processus par rapport au gyroscope ((rad/s) ²)

[1e-9,1e-9,1e-9] (par défaut) | scalaire | vecteur de ligne à trois éléments

Variation multiplicative du bruit de processus du gyroscope en (rad/s) ², spécifié sous forme de nombres réels finis positifs.

Types de données : single | double

AccelerometerNoise — Variation multiplicative du bruit de processus par rapport à l'accéléromètre ((m/s²) ²)

[1e-4,1e-4,1e-4] (par défaut) | scalaire | vecteur de ligne à trois éléments

Variation multiplicative du bruit de processus par rapport à l'accéléromètre en (m/s²) ², spécifié sous forme de nombres réels finis positifs.

Types de données : single | double

GyroscopeBiasNoise — Variation multiplicative du bruit de processus due au biais du gyroscope ((rad/s²) ²)

[1e-10,1e-10,1e-10] (par défaut) | scalaire | vecteur de ligne à trois éléments

Variation multiplicative du bruit de processus par rapport au biais du gyroscope dans (rad/s²) ², spécifié sous forme de nombres réels finis positifs.

Types de données : single | double

AccelerometerBiasNoise — Variation multiplicative du bruit de processus par rapport au biais de l'accéléromètre ((m/s²) ²)

[1e-4,1e-4,1e-4] (par défaut) | scalaire | vecteur de ligne à trois éléments

Variation multiplicative du bruit de processus par rapport au biais de l'accéléromètre dans (m/s²) ², spécifié sous forme de nombres réels finis positifs.

Types de données : single | double

GeomagneticVectorNoise — Bruit de processus additif pour le vecteur géomagnétique (μT²)

[1e-6,1e-6,1e-6] (par défaut) | scalaire | vecteur de ligne à trois éléments

Bruit de processus additif pour le vecteur géomagnétique en μT², spécifié sous forme de nombres réels finis positifs.

Types de données : single | double

MagnetometerBiasNoise — Bruit de processus additif pour la polarisation du magnétomètre (μT²)

[0.1,0.1,0.1] (par défaut) | scalaire | vecteur de ligne à trois éléments

Bruit de processus additif pour le biais du magnétomètre en μT², spécifié sous forme de nombres réels finis positifs.

Types de données : single | double

State — Vecteur d'état du filtre de Kalman étendu

Vecteur de colonne à 18 éléments

Vecteur d'état du filtre de Kalman étendu. Les valeurs d'état représentent :

État	Unités	Indice
Orientation (parties du quaternion)	N / A	1:4
Altitude (NED ou ENU)	m	5
Vitesse verticale (NED ou ENU)	MS	6
Biais d'angle delta (XYZ)	rad/s	7:9
Biais de vitesse delta (XYZ)	MS	10h12
Vecteur de champ géomagnétique (NED ou ENU)	µT	13h15
Biais du magnétomètre (XYZ)	µT	16h18

L'état initial par défaut correspond à un objet au repos situé à [0 0 0] en coordonnées géodésiques LLA.

Types de données : single | double

StateCovariance — Covariance d'erreur d'état pour le filtre de Kalman étendu

eye(18)*1e-6 (par défaut) | Matrice 18 x 18

Covariance d'erreur d'état pour le filtre de Kalman, spécifiée comme une matrice de 18 éléments sur 18 de nombres réels.

Types de données : single | double

Fonctions d'objet

predict	Mettre à jour les états à l'aide des données de l'accéléromètre et du gyroscope pour ahrs10filter
fusemag	Corriger les états à l'aide des données du magnétomètre pour ahrs10filter
fusealtimeter	Corriger les états à l'aide des données altimétriques pour ahrs10filter
correct	Corriger les états à l'aide de mesures d'état directes pour ahrs10filter
residual	Residuals and residual covariances from direct state measurements for ahrs10filter
residualmag	Résidus et covariance résiduelle des mesures du magnétomètre pour ahrs10filter
residualaltimeter	Résidus et covariance résiduelle des mesures altimétriques pour ahrs10filter
pose	Orientation actuelle et estimation de position pour ahrs10filter
reset	Reset internal states for ahrs10filter
stateinfo	Afficher les informations vectorielles d'état pour ahrs10filter
tune	Ajustez les paramètres ahrs10filter pour réduire l'erreur d'estimation
copy	Créer une copie de ahrs10filter

Exemples

Estimation de la pose du drone

Chargez les données de capteur enregistrées, la pose de vérité terrain et l'état initial et la covariance de l'état initial. Calculez le nombre d’échantillons IMU par échantillon d’altimètre et le nombre d’échantillons IMU par échantillon de magnétomètre.

load('fuse10exampledata.mat', ...
     'imuFs','accelData','gyroData', ...
     'magnetometerFs','magData', ...
     'altimeterFs','altData', ...
     'expectedHeight','expectedOrient', ...
     'initstate','initcov');

imuSamplesPerAlt = fix(imuFs/altimeterFs);
imuSamplesPerMag = fix(imuFs/magnetometerFs);

Créez un filtre AHRS qui fusionne les lectures MARG et altimétriques pour estimer la hauteur et l'orientation. Définissez le taux d'échantillonnage et les bruits de mesure des capteurs. Les valeurs ont été déterminées à partir de fiches techniques et d’expérimentations.

filt = ahrs10filter('IMUSampleRate',imuFs, ...
                    'AccelerometerNoise',0.1, ...
                    'State',initstate, ...
                    'StateCovariance',initcov);

Ralt = 0.24;
Rmag = 0.9;

Pré-attribuez des variables à la hauteur et à l'orientation du journal.

numIMUSamples = size(accelData,1);
estHeight = zeros(numIMUSamples,1);
estOrient = zeros(numIMUSamples,1,'quaternion');

Fusionner les données de l'accéléromètre, du gyroscope, du magnétomètre et de l'altimètre. La boucle externe prédit le filtre en avant à la fréquence d'échantillonnage la plus rapide (la fréquence d'échantillonnage IMU).

for ii = 1:numIMUSamples
    
    % Use predict to estimate the filter state based on the accelometer and
    % gyroscope data.
    predict(filt,accelData(ii,:),gyroData(ii,:));
    
    % Magnetometer data is collected at a lower rate than IMU data. Fuse
    % magnetometer data at the lower rate.
    if ~mod(ii,imuSamplesPerMag)
        fusemag(filt,magData(ii,:),Rmag);
    end
    
    % Altimeter data is collected at a lower rate than IMU data. Fuse
    % altimeter data at the lower rate.
    if ~mod(ii, imuSamplesPerAlt)
        fusealtimeter(filt,altData(ii),Ralt);
    end
    
    % Log the current height and orientation estimate.
    [estHeight(ii),estOrient(ii)] = pose(filt);
end

Calculez les erreurs RMS entre la hauteur et l’orientation réelles connues et la sortie du filtre AHRS.

pErr = expectedHeight - estHeight;
qErr = rad2deg(dist(expectedOrient,estOrient));

pRMS = sqrt(mean(pErr.^2));
qRMS = sqrt(mean(qErr.^2));

fprintf('Altitude RMS Error\n');

Altitude RMS Error

fprintf('\t%.2f (meters)\n\n',pRMS);

	0.38 (meters)

Visualisez la hauteur réelle et estimée au fil du temps.

t = (0:(numIMUSamples-1))/imuFs;
plot(t,expectedHeight);hold on
plot(t,estHeight);hold off
legend('Ground Truth','Estimated Height','location','best')
ylabel('Height (m)')
xlabel('Time (s)')
grid on

fprintf('Quaternion Distance RMS Error\n');

Quaternion Distance RMS Error

fprintf('\t%.2f (degrees)\n\n',qRMS);

	2.93 (degrees)

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 R2019a

Voir aussi

ahrsfilter | insfilter