Kalman Filter - Boussole et Gyro

Question

J'essaie de construire une boussole avec gyroscope, accéléromètre et magnétomètre.

Je fusionne les valeurs acc avec les valeurs du magnomètre pour obtenir l'orientation (en utilisant la matrice de rotation) et cela fonctionne plutôt bien. Mais maintenant, je veux ajouter un gyroscope pour aider à compenser lorsque le capteur magnétique n'est pas précis. Donc je veux utiliser le filtre kalman pour fusionner les deux résultats et obtenir un filtre bien filtré (les acc et mag sont déjà filtrés en utilisant lpf).

Mes matrices sont:

state(Xk) => {Compass Heading, Rate from the gyro in that axis}. 
transition(Fk) => {{1,dt},{0,1}} 
measurement(Zk) => {Compass Heading, Rate from the gyro in that axis} 
Hk => {{1,0},{0,1}} 
Qk = > {0,0},{0,0} 
Rk => {e^2(compass),0},{0,e^2(gyro)} 

Et voici ma mise en œuvre du filtre de Kalman:

public class KalmanFilter { 

private Matrix x,F,Q,P,H,K,R; 
private Matrix y,s; 

public KalmanFilter(){ 
} 

public void setInitialState(Matrix _x, Matrix _p){ 
    this.x = _x; 
    this.P = _p; 
} 

public void update(Matrix z){ 
    try { 
     y = MatrixMath.subtract(z, MatrixMath.multiply(H, x)); 
     s = MatrixMath.add(MatrixMath.multiply(MatrixMath.multiply(H, P), 
         MatrixMath.transpose(H)), R); 
     K = MatrixMath.multiply(MatrixMath.multiply(P, H), MatrixMath.inverse(s)); 
     x = MatrixMath.add(x, MatrixMath.multiply(K, y)); 
     P = MatrixMath.subtract(P, 
         MatrixMath.multiply(MatrixMath.multiply(K, H), P)); 
    } catch (IllegalDimensionException e) { 
     e.printStackTrace(); 
    } catch (NoSquareException e) { 
     e.printStackTrace(); 
    } 
    predict(); 
} 

private void predict(){ 
    try { 
     x = MatrixMath.multiply(F, x); 
     P = MatrixMath.add(Q, MatrixMath.multiply(MatrixMath.multiply(F, P), 
         MatrixMath.transpose(F))); 
    } catch (IllegalDimensionException e) { 
     e.printStackTrace(); 
    } 
} 

public Matrix getStateMatirx(){ 
    return x; 
} 

public Matrix getCovarianceMatrix(){ 
    return P; 
} 

public void setMeasurementMatrix(Matrix h){ 
    this.H = h; 
} 

public void setProcessNoiseMatrix(Matrix q){ 
    this.Q = q; 
} 

public void setMeasurementNoiseMatrix(Matrix r){ 
    this.R = r; 
} 

public void setTransformationMatrix(Matrix f){ 
    this.F = f; 
} 
} 

Au début, ces valeurs de départ sont données:

Xk => {0,0} 
Pk => {1000,0},{0,1000} 

Puis je regarde la deux résultats (le kalman et le compas). Le kalman commence à partir de 0 et augmente à un certain rythme indépendamment de celui mesuré (boussole) et il ne cesse pas de continuer à augmenter ...

Je ne peux pas comprendre ce que je fais mal?

Answer 1

Le problème que vous voyez est que même si les gyroscopes ont un bruit très faible, ce n'est pas zéro. Lorsque vous utilisez votre terme e^2(gyro), vous implémentez un filtre où vous prétendez que z_gyro = true_gyro + v où v ~ N(0, e^2) La vérité est plus comme v ~ N(bias, e^2) où même le biais a quelques termes (principalement statique parti d'activation plus un décalage de polarisation causé par la dérive de la température). Par conséquent, vous intégrez le biais et vous tournez constamment.

Si vous étalonnez ce biais (mesurez simplement la sortie du gyro à l'arrêt), vous pouvez appeler update(imu - bias) au lieu de simplement update(imu). Vous devrez peut-être augmenter e^2(gyro) pour tenir compte des changements de biais, mais pas autant que si vous essayiez d'en tenir compte (le décalage non compensé se transformera en un déplacement de cap fixe en proportion des termes du magnétomètre et du gyro.).

Le meilleur moyen est d'ajouter le biais à votre vecteur d'état. Vous obtenez quelque chose comme Hk = {{1,0,0},{0,1,1}}, ce qui signifie que votre mesure de gyro prédite est le taux réel plus votre terme de biais. La magie du filtre de Kalman est que même si vous avez dit que votre mesure est simplement la somme des deux termes, ils sont différents, de plusieurs façons:

• Dans F la rubrique est liée au taux de rotation réel (par dt), et donc la covariance d'état P évolue hors termes diagonaux reliant le cap et le taux de virage chaque fois que vous mettez à jour P. De même dans H vous avez décrit une relation entre le biais et le taux gyroscopique qui exprime l'idée «soit je tourne plus vite ou j'ai plus de biais», donc le filtre met à jour l'état pour équilibrer les covariances de bruit.
• Dans Q, le bruit de processus de taux de rotation doit être réglé assez haut pour tenir compte de tout mouvement inattendu que vous mesurez.Mais le biais de Q est beaucoup, beaucoup plus petit parce que le biais n'évolue pas très rapidement (en fait, le meilleur modèle est probablement un processus de Gauss-Markov de premier ordre, que je n'expliquerai pas ici). dans "filtre de mémoire limitée"). Dans la limite, vous pouvez imaginer le Q terme pour le biais d'être 0 (biais de modélisation comme constante aléatoire) mais cela ne fonctionne pas bien numériquement dans un EKF, et n'est pas strictement vrai en raison de la dérive de biais.
• De même, le P_0 initial du système est beaucoup plus petit pour le terme de polarisation (sa plage totale possible étant documentée dans la fiche technique) que pour la vitesse angulaire/angulaire totalement inconnue.
• Dans un système multi-axes, la polarisation est toujours associée à l'axe (c'est une propriété du matériel non liée à son orientation) mais l'influence du gyro sur un état tel que "heading" est inversée en raison du IMU sanglée.

Observer un EKF "apprendre" une valeur comme le biais gyroscopique est encore plus magique pour moi que la prédiction du reste de l'état.