5D Le filtre de Kalman ne fonctionne pas, et nous ne savons pas où nous allons mal

Question

En ce moment, nous essayons de créer un filtre Kalman 5 dimensions qui reçoit en entrée les coordonnées x et y d'un petit bug qui est errant dans la boîte. Nous observons ce bug car il fait environ 2000 mouvements, puis prédire sa position à partir de là. Les dimensions sont x-coord, y-coord, vélocité, cap et accélération angulaire. Voici le code que nous avons jusqu'ici.

#x is a list of five variables - x, y, velocity, angularVelocity, angularAcceleration 
#currentmeasurement is the x and y that were observed 
def kalman_filter(x, P, currentmeasurement, lastMeasurement = None): 
     prevmeasurement = [] 

    #if there is a lastMeasurement argument, it becomes measurement 
    if lastMeasurement: 
     prevmeasurement = [lastMeasurement[0], lastMeasurement[1], x.value[3][0]] 

    #if there is no lastMeasurement argument, the current measurement becomes measurement. 
    else: 
     prevmeasurement = [x.value[0][0], x.value[1][0], x.value[3][0]] 

    #Prediction Step 
    a = x.value[3][0] 
    F = matrix([ 
     [1., 0., cos(a), 0., 0.], 
     [0., 1., sin(a), 0., 0.], 
     [0., 0., 1., 0., 0.], 
     [0., 0., 0., 1., dt], 
     [0., 0., 0., 0., 1.]]) 

    x = (F * x) + u 
    P = F * P * F.transpose() 

    #we can calculate the heading from our observations. 
    heading = atan2(currentmeasurement[0][1] - prevmeasurement[1], currentmeasurement[0][0] - prevmeasurement[0]) 
    while(abs(heading - prevmeasurement[2]) > pi): 
     heading = heading + 2*pi*((prevmeasurement[2]-heading)/abs(prevmeasurement[2]-heading)) 

    #perhaps the velocity should also be calculated? 

    # measurement update 
    dt = 1. 
    u = matrix([[0.], [0.], [0.], [0.], [0.]])  # external motion 
    H = matrix([[1., 0., 0., 0., 0.],    #the measurement function 
      [0., 1., 0., 0., 0.], 
      [0., 0., 0., 1., 0.]]) 

    R = matrix([[1., 0., .0],      #measurement uncertainty 
      [0., 1., .0], 
      [0., 0., 1.]]) 
    I = matrix([[]])        #a 5x5 identity matrix 
    I.identity(5) 


    prevmeasurement = [currentmeasurement[0][0], currentmeasurement[0][1], heading] 
    Z = matrix([prevmeasurement]) 
    y = Z.transpose() - (H * x) 
    S = H * P * H.transpose() + R 
    K = P * H.transpose() * S.inverse() 
    x = x + (K * y) 
    P = (I - (K * H)) * P 

    return x,P 

Ceci aboutit à des estimations extrêmement incorrectes. Nous ne sommes pas sûrs de ce que nous faisons mal ici - je pense que nous suivons toutes les étapes correctement, mais nous ne sommes pas sûrs si toutes les matrices requises sont correctes. Toute contribution serait utile!

Answer 1

Le commentaire du haut a la mauvaise description. Votre état doit être x, y, velocity, angle, angularVelocity

Il vous manque Q, la covariance du processus. Il devrait refléter combien votre état peut changer entre les mises à jour, et est ajouté à votre mise à jour de P dans l'étape prédire.

Vous construisez un EKF (puisque votre mise à jour nécessite trig, elle est non linéaire). Vous avez construit une matrice F qui effectue votre mise à jour d'état, mais ce dont vous avez besoin pour la mise à jour de covariance de processus est le Jacobian de votre fonction de mise à jour. Dans votre cas, il ressemble à:

J = matrix([ 
    [1., 0., cos(a), -sin(a), 0.], 
    [0., 1., sin(a), cos(a), 0.], 
    [0., 0., 1., 0., 0.], 
    [0., 0., 0., 1., dt], 
    [0., 0., 0., 0., 1.]]) 

Si votre seule mesure directe est la position, vous ne devriez pas calculer cap et l'utiliser comme une mesure. Laissez le KF le faire.

Vous devez définir R comme étant les véritables incertitudes de la mesure. Le bruit représenté par Q, R, et propagé dans P est plus important que votre état x si vous voulez que le KF fonctionne.

Answer 2

1. Ajouter Q la matrice de bruit modèle
2. Votre matrice H devrait être le jacobien. Pas votre matrice Q.
   3. N'estimez rien pour le filtre. Laissez le Kalman le faire pour vous et vous vous en tenez à l'espace observé Si vous envoyez le script principal, je peux vous aider davantage.

Answer 3

je travaillais sur un problème très similaire, et ayant exactement les mêmes questions. Compte tenu des mesures bruitées (x, y) de la position d'un robot, et la connaissance qu'il tourne à vitesse constante dans un cercle de rayon constant, prédire sa position suivante.

J'ai également essayé un EKF 5D, qui exploserait après quelques itérations. Mon état est composée de:

[x, y, distance_per_timestep, heading, heading_delta_per_timestep]

I essayé de nombreuses formulations pour la matrice de transition d'état (jacobienne de f (x), version modifiée du jacobien, etc). En désespoir de plus en plus, j'ai essayé de modifier manuellement les gains de Kalman, les covariances, tout ... rien ne semblait fonctionner.

Ma solution finale, qui fonctionne étonnamment bien, était d'utiliser le filtre 2D Kalman sur x et y positions, avec la vitesse, le cap et la vitesse de braquage calculé à partir des 3 dernières positions et utilisées comme entrées de commande à la mise à jour d'estimation d'état. J'ai essayé d'autres variations sur ce thème (état de [x, y, vx, vy, ax, ay] et ainsi de suite, mais rien n'a dépassé l'approche KISS (keep it simple, stupid)

J'ai d'abord utilisé les mesures brutes et bruitées pour calculer la vélocité, l'en-tête et le delta_heading J'ai essayé d'utiliser mes estimations d'état de x et et après que le filtre ait fonctionné pendant un certain temps, mais cela a mené à l'instabilité aussi - apparition très lente, mais un tissage doucement croissant en avant et en arrière à travers la piste de vérité au sol transformé en un morceau de Spirograph art! Plus tard, j'ai trouvé que si je faisais ma mise à jour de mesure, puis utilisé ces valeurs d'état (après mesure, mais avant la mise à jour de l'état), j'obtiendrais des performances légèrement supérieures aux mesures brutes . Je ne suis pas sûr que mon approche résoudrait votre problème (je continue à obtenir des mesures bruyantes, et je n'ai besoin que de prévoir quelques mouvements pour «attraper» mon fugueur), mais en tirer ce que vous pouvez.

Je suis aussi très intéressé à connaître pourquoi mon EKF a explosé, et aussi comment formuler la matrice F correcte dans un problème non linéaire comme this- la « bonne » jacobienne, si je calcule correctement , fait exploser l'EKF presque immédiatement! Je serais très intéressé par votre solution, ou par toute autre avancée que vous auriez pu réaliser! Bonne chance!