Implémentation du filtre de Kalman en python pour l'estimation de la vitesse

Question

J'essaie d'implémenter le filtre de Kalman pour prédire la vitesse d'avance. mise en oeuvre en python H = np.diag ([1,1]) H

Résultat: tableau ([[1, 0], [0, 1]]) Pour vecteur de mesure est datafile fichier CSV contenant moment où une colonne et la vitesse dans une autre colonne

measurements=np.vstack((mx,my,datafile.speed)) 
#length of meassurement 
m=measurements.shape[1] 
print(measurements.shape) 

sortie (3, 1069)

Kalman

for filterstep in range(m-1): 
     #Time Update 
      #============================= 
     #Project the state ahead 
     x=A*x 

     #Project the error covariance ahead 
     P=A*P*A.T+Q 

     #Measurement Update(correction) 
     #=================================== 
     #if there is GPS measurement 
     if GPS[filterstep]: 
     #COmpute the Kalman Gain 
     S =(H*P*H).T + R 
     S_inv=S.inv() 
     K=(P*H.T)*S_inv 

     #Update the estimate via z 
     Z = measurements[:,filterstep].reshape(H.shape[0],1) 
     y=Z-(H*x) 
     x = x + (K*y) 

     #Update the error covariance 
     P=(I-(K*H))*P 


# Save states for Plotting 
    x0.append(float(x[0])) 
    x1.append(float(x[1])) 


    Zx.append(float(Z[0])) 
    Zy.append(float(Z[1])) 

    Px.append(float(P[0,0])) 
    Py.append(float(P[1,1])) 



    Kx.append(float(K[0,0])) 
    Ky.append(float(K[1,0])) 

erreur se présente comme:

--------------------------------------------------------------------------- 
ValueError        Traceback (most recent call last) 
<ipython-input-80-9b15fccbaca8> in <module>() 
    20 
    21   #Update the estimate via z 
---> 22   Z = measurements[:,filterstep].reshape(H.shape[0],1) 
    23   y=Z-(H*x) 
    24   x = x + (K*y) 

ValueError: total size of new array must be unchanged 

Comment puis-je supprimer une telle erreur

Answer 1

Cette ligne est incorrecte:

S =(H*P*H).T + R 

Le code correct est:

S =(H*P*H.T) + R 

I 'ai du mal à suivre ce que sont les mesures . Vous avez déclaré "array ([[1, 0], [0, 1]]) Pour le fichier vectoriel de mesure, le fichier csv contient le temps comme colonne et vitesse dans une autre colonne"

Alors ça me lit comme CSV fichier avec deux colonnes, une fois et une vitesse. Dans ce cas, vous n'avez qu'une seule mesure à chaque fois, la vitesse. Pour une seule mesure, votre matrice H doit être un vecteur ligne.