Question

我的卡尔曼滤波器有问题。我使用Ros来实时接收来自莫尔斯的数据。并且，我使用传输的数据来计算位置。

但是，在我的具体情况下，我使用一个名为X_0的初始化步骤，我只想使用它一次。在这种情况下我不能使用while循环来隔离初始化步骤，因为我正在实时工作。你有任何想法解决这个问题吗？我有点失落了;）

class ListenerVilma:

def __init__(self):
    self.orientation = None
    self.velocity = None
    self.gps = None

def orientation_callback(self, msg):

    self.orientation = msg
    self.compute_stuff()

def velocidade_callback(self, msg):

    self.velocity = msg
    #self.compute_stuff()

def gps_callback(self, msg):

    self.gps = msg
    #self.compute_stuff()


def compute_stuff(self):

# **********************
    # Latitude of the veicule
# **********************

    latitude = self.gps.latitude

# *************************
    # Longitude of the veicule
# ************************ 

    longitude = self.gps.longitude

# *********************
    # wheel orientation 
# *********************

    Orientation_gauche = self.orientation.data[0]

    Orientation_droite = self.orientation.data[1]

    # *****************
    # wheel velocity 
# *****************


    vitesse_avant_gauche = np.sqrt((self.velocity.data[0]*self.velocity.data[0]) + (self.velocity.data[1]*self.velocity.data[1]) + (self.velocity.data[2]*self.velocity.data[2]))

    vitesse_avant_droite = np.sqrt((self.velocity.data[3]*self.velocity.data[3]) + (self.velocity.data[4]*self.velocity.data[4]) + (self.velocity.data[5]*self.velocity.data[5]))

    vitesse_arriere_droite = np.sqrt((self.velocity.data[6]*self.velocity.data[6]) + (self.velocity.data[7]*self.velocity.data[7]) + (self.velocity.data[8]*self.velocity.data[8]))

    vitesse_arriere_gauche = np.sqrt((self.velocity.data[9]*self.velocity.data[9])+ (self.velocity.data[10]*self.velocity.data[10]) + (self.velocity.data[11]*self.velocity.data[11]))



    # *****
# Data
# *****

# Les donnees sont inscrites dans un tableau et elles sont classees
# dans l'ordre suivant:

z_k1 = np.array([[Orientation_gauche],       # rear left wheel orientation
            [Orientation_droite],            # rear right wheel orientation
            [vitesse_arriere_gauche],        # rear left wheel velocity
            [vitesse_arriere_droite],        # rear right wheel velocity
            [vitesse_avant_gauche],          # front left wheel velocity
            [vitesse_avant_droite]])        # front right wheel velocity


#angle

angle_moyen = (z_k1[0]+z_k1[1])/2



# Matrix Q

Q_k1 = np.array([[0.1,0],   # erreur de process pour la distance parcourue     
                [0,0.1]])    # erreur de process pourll'angle ed estarcamento





# Matrix F

F_k1 = np.array([[1,0],       
                [0,1]])

# Matrix R

R_k1 = np.array([[1, 0, 0, 0, 0],       
                [0, 1, 0, 0, 0],        
                [0, 0, 1, 0, 0],        
                [0, 0, 0, 1, 0],        
                [0, 0, 0, 0, 1]])


# lenght veicule

L = 3.5


# lenght between two wheel

e = 0.375

# Delta t. 

delta_t = 0

# Fréquence d'acquisition de la mesure de distance (ici 1Hz)

f = 1

    #####################################
    # INitialsation step 
#####################################


# I would like to use the matrix P and the variable X_0 only one time>



# MATRIX P. 

P_0 = np.array([[0,0],       
                [0,0]])


# Matrix X 

X_0 = np.array([[0.0000000001],       # Distance      
                [0.0000000001]])       # Angle

# ************************
# ************************
# KALMAN 1
# ************************
# ************************


# ***************
# Prédiction
# ***************        

X_k1_K_1 = np.dot(F_k1,X_0)
X_k1_K_11 = np.dot(F_k1,X_00)

P_k1_K_1 =np.dot(F_k1,np.dot(P_0,np.transpose(F_k1))) + Q_k1
P_k1_K_1 =np.dot(F_k1,np.dot(P_00,np.transpose(F_k1))) + Q_k1


# ***************
# Actualization
# ***************


# Delta t

delta_t = delta_t + f


# Matrice H

H1 = (-L * X_k1_K_1[1])/(X_k1_K_1[0]*X_k1_K_1[0])
H11 = np.asscalar(H1)      # transform array to float 
H2 = L/X_k1_K_1[0]
H12 = np.asscalar(H2)      # transforme array to float 
H3 = 1/delta_t
H4 = -e/delta_t
H5 = 1/delta_t
H6 = e/delta_t
H7 = 1/delta_t
H8 = -e/delta_t
H9 = 1/delta_t
H10 = e/delta_t

H_k1 = np.array([[H11,H12],
                [H3,H4],
                [H5,H6],
                [H7,H8],
                [H9,H10]])


# Matrice z 

Z1 = np.tan(angle_moyen)
Z2 = z_k1[2]
Z2 = np.asscalar(Z2)
Z3 = z_k1[3]
Z3 = np.asscalar(Z3)
Z4 = z_k1[4]*np.cos(z_k1[0])
Z4 = np.asscalar(Z4)
Z5 = z_k1[5]*np.cos(z_k1[1])
Z5 = np.asscalar(Z5)



Z_k1 = np.array([[Z1],       
                [Z2],        
                [Z3],        
                [Z4],        
                [Z5]])


# Calcul de la la matrice K


KA = inv(np.dot(H_k1,np.dot(P_k1_K_1,np.transpose(H_k1)))+R_k1)


K_k1 = np.dot(P_k1_K_1,np.dot(np.transpose(H_k1),KA))



# Calcul de la matrice X


XA = Z_k1 - np.dot(H_k1, X_k1_K_1)


X_k1_K = X_k1_K_1 + np.dot(K_k1,XA)


# Calcul de la matrice P


P_k1_K = P_k1_K_1 - np.dot(K_k1,np.dot(H_k1,P_k1_K_1))



# etape de retour en arrière

X_k1_K[0]=np.asscalar(X_k1_K[0])
X_k1_K[1]=np.asscalar(X_k1_K[1])






X_0 = X_k1_K
P_0 = P_k1_K







if __name__ == '__main__':
    rospy.init_node('listener', anonymous=True)


    ListenerVilma = ListenerVilma()

    rospy.Subscriber('/orientation', Float64MultiArray , ListenerVilma.orientation_callback)
    rospy.Subscriber('/velocidade', Float64MultiArray, ListenerVilma.velocidade_callback)
    rospy.Subscriber('/gps1', NavSatFix, ListenerVilma.gps_callback)

    rospy.spin()

如何在卡尔曼滤波器中使用一次变量实时数据

0 个答案: