Chaînage des filtres de Kalman

Mon équipe construit un robot pour naviguer de manière autonome dans un environnement extérieur. Nous avons récemment obtenu un nouveau capteur IMU / GPS intégré qui effectue apparemment un filtrage Kalman étendu sur puce. Il donne le tangage, le roulis et le lacet, les vitesses nord, est et descente, ainsi que la latitude et la longitude.

Cependant, nous avons également des encodeurs attachés à nos roues, qui fournissent des vitesses linéaires et angulaires. Avant d'obtenir ce nouveau capteur IMU / GPS, nous avons créé notre propre EKF pour estimer notre état à l'aide des encodeurs et d'autres capteurs à faible coût. Nous voulons utiliser le filtre sur puce de ce nouveau capteur, mais également intégrer nos encodeurs dans le mix.

Y a-t-il un problème avec le chaînage des filtres? Ce que je veux dire, c'est que nous utiliserions la sortie de l'EKF intégré du capteur IMU / GPS comme une mise à jour de notre propre EKF, tout comme nous utilisons les données lues à partir des encodeurs comme une mise à jour de notre EKF. Cela me semble raisonnable, mais je me demandais ce qui est généralement censé être fait dans ce cas.

Vous pouvez utiliser l'INS / GPS comme mises à jour de la sortie de votre premier EKF. Il ne s'agit pas, en fait, de chaîner, mais simplement de conditionner l'estimation basée sur les informations ajoutées de l'INS / GPS.

Supposons que nous ayons les fonctions suivantes:

$x_{t+1|t}$ , = EKF_PREDICT ( , , ), pour les entrées comme état , covariance et entrées de commande (estimées par odométrie) .$P_{t+1|t}$$x_t$$P_t$$u_t$$x$$P$$u_t$

et

$x_{t+1|t+1}$ , = EKF_UPDATE ( , , ). $P_{t+1|t+1}$$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}$

Les estimations des capteurs sont les . Nous avons des choses comme:$\hat{x}_{t+1}$

$\hat{x}^{gps}_{t+1} = f(GPS)$

$\hat{x}^{map}_{t+1} = f(map)$

$\hat{x}^{ins}_{t+1} = f(INS)$

etc. pour toutes les autres façons d'estimer l'état du robot. L'exécution de la fonction EKF_UPDATE pour tous ces capteurs est donc suffisante.

Votre boucle ressemblera à ceci:

pour tous les temps$t$

• Soit l'estimation odométrique / cinématique actuelle de la pose, et le bruit sur cette estimation.$u_t$$R_u$

• $x_{t+1|t}$ , = EKF_PREDICT ( , , , )$P_{t+1|t}$$x_t$$P_t$$u_t$$R_u$

• pour tous les capteurs ,$S$

  • Soit l'estimation de la pose de ce capteur, et le bruit sur cette estimation R$\hat{x}^{S}_{t+1}$$R_{S}$

  • P t + 1 | t + 1 x t + $x_{t+1|t+1}$ , = EKF_UPDATE ( , , ). $P_{t+1|t+1}$ P t + 1 | t x t + 1 ,R$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}, R_S$

  • fin pour

• fin pour

Certaines mises en garde sont les suivantes:

• Puisque nous utilisons l'EKF, il n'y a aucune garantie que l'estimation est indépendante de la commande des mises à jour. Autrement dit, si vous faites INS puis GPS, l'estimation résultante peut être différente de celle si vous mettez à jour avec GPS puis INS. Ce n'est généralement pas un gros problème, mais le filtre nécessitera beaucoup plus de réglages.

• Veuillez noter que votre INS présente un biais et une dérive, ce qui pourrait affecter votre fiabilité à long terme. Le GPS peut vous aider beaucoup ici. La plupart des publications estiment simultanément le biais et la dérive de l'INS.