Dans le cadre d’un projet il m’est demande d’appliquer un filtre de kalman sur une situation bien precise. Nous utilisons pour cela un environnement ros et gazebo. Gazebo nous permet de voir s’afficher le deplacement de notre voiture, et un tag mis a une position de [8 0]. Le depart s’effectue au point [0 0]. Notre objectif est que notre valeur estime de la position en x soit a la fin proche de celle reelle. Le soucis actuel c’est que l’on comprend pas pourquoi notre valeur de la position estime x augmente autant ainsi que celle de la vitesse.
Voici le code ci dessous, je suis preneuse de tous les conseils possibles :
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.parameter import Parameter
from sensor_msgs.msg import Image, CameraInfo, ChannelFloat32 # Image is the message type
from geometry_msgs.msg import Twist, Pose2D
from nav_msgs.msg import Odometry
import numpy as np
from math import sqrt, asin, atan2
import matplotlib.pyplot as plt
def euler_from_quaternion(q):
"""
Convert a quaternion into euler angles (roll, pitch, yaw)
roll is rotation around x in radians (counterclockwise)
pitch is rotation around y in radians (counterclockwise)
yaw is rotation around z in radians (counterclockwise)
"""
t0 = +2.0 * (q.w * q.x + q.y * q.z)
t1 = +1.0 - 2.0 * (q.x * q.x + q.y * q.y)
roll_x = atan2(t0, t1)
t2 = +2.0 * (q.w * q.y - q.z * q.x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch_y = asin(t2)
t3 = +2.0 * (q.w * q.z + q.x * q.y)
t4 = +1.0 - 2.0 * (q.y * q.y + q.z * q.z)
yaw_z = atan2(t3, t4)
return roll_x, pitch_y, yaw_z # in radians
class Kalman(Node):
def __init__(self):
super().__init__('Kalman')
self.set_parameters([Parameter('use_sim_time', Parameter.Type.BOOL, True)])
self.get_logger().info("Kalman start")
# Subscriptions and Publications
self.sub_odom = self.create_subscription(Odometry, '/odom', self.cb_odom, 1)
self.sub_tag = self.create_subscription(ChannelFloat32, "/tag", self.cb_tag, 1)
self.pub_cmd_vel = self.create_publisher(Twist, "/cmd_vel", 1)
self.pub_estim = self.create_publisher(Pose2D, "/odom_estim", 1)
self.cmd=Twist()
self.position = Pose2D()
self.estim = Pose2D()
self.tag = None
self.tag_recept = False
self.z = 0.0
self.dt = 0.5
self.t = 0.0
self.position.x = 0.0
self.position.y = 0.0
self.get_logger().info(f"\n\n\n\ninit : ")
self.timer_run = self.create_timer(self.dt, self.run)
self.create_timer(self.dt, self.commande)
self.H = np.array([[1.0, 0.0]])
self.Sigma = np.array([[1.0, 0.0], [0.0, 1.0]])
self.X = np.array([[0.0], [0.5]])
self.y_calcule = 0.0
self.R = np.array([[0.1, 0], [0, 0.1]])
self.Q = np.array([2.0])
# self.S = np.array([[0.0, 0.0],[0.0, 0.0]])
self.G = np.array([[1, self.dt], [0, 1]])
self.K = 0.0
self.z = 0.0
self.positions_x = []
self.positions_y = []
self.estimations_x = []
self.estimations_y = []
def commande(self):
self.t += self.dt
if self.t > 0.5:
self.cmd.linear.x = 0.5
if self.t > 25.0:
self.cmd.linear.x = 0.0
# self.X[1, 0] = self.cmd.linear.x
self.pub_cmd_vel.publish(self.cmd)
def predict(self):
self.X= np.dot(np.array([[1, self.dt], [0, 1]]), self.X)
self.Sigma= np.dot(self.G,np.dot(self.Sigma, self.G.T)+ self.R)
self.get_logger().info(f"\n\n\n\nX : {self.X}")
self.get_logger().info(f"Sigma : {self.Sigma}")
def estimate(self):
y= self.z- (8 - np.array([[self.X[0, 0]]]))# ???
S= np.dot(self.H, np.dot(self.Sigma, self.H.T)) + self.Q
K= np.dot(self.Sigma, np.dot(self.H.T, np.linalg.inv(S)))
self.X= self.X + np.dot(K,y)
# if self.tag != None :
# self.y_calcule = sqrt(self.z**2 - (8 - self.X[0, 0])**2)
self.Sigma= np.dot((np.eye(2) - np.dot(K,self.H)),self.Sigma)
self.get_logger().info(f"\n\ny : {y}")
self.get_logger().info(f"S : {S}")
self.get_logger().info(f"K : {K}")
self.get_logger().info(f"X final : {self.X}")
self.get_logger().info(f"y calcule : {self.y_calcule}")
# Callback for odometry data
def cb_odom(self, msg):
self.position.x = msg.pose.pose.position.x
self.position.y = msg.pose.pose.position.y
roll, pitch, yaw = euler_from_quaternion(msg.pose.pose.orientation)
self.position.theta = yaw
self.sim_time = msg.header.stamp.sec + 1e-9 * msg.header.stamp.nanosec
def cb_tag(self, msg):
self.tag = msg
self.tag_recept = True
self.get_logger().info(f"\n\nla distance réel: x={self.tag.values[1]}")
self.z= self.tag.values[1]
self.get_logger().info(f" on parle de Z:{self.z}")
def plot_position(self):
plt.figure(figsize=(10, 6))
# Tracé de la trajectoire réelle
plt.plot(self.positions_x, self.positions_y, label="Trajectoire réelle", color="black")
# Tracé de la trajectoire estimée
plt.plot(self.estimations_x, self.estimations_y, label="Trajectoire estimée", color="blue", linestyle="--")
# Personnalisation du graphique
plt.xlabel("Position X")
plt.ylabel("Position Y")
plt.title("Trajectoires réelle vs estimée")
plt.legend()
plt.grid()
# Affichage du graphique
plt.show()
def run(self):
# if self.tag == None :
# self.X[0 0]= self.X[0 0]+ self.cmd.linear.x*self.dt
if self.t<25.0: #stockage valeurs
if self.tag == None : #avant le capteur, pas d'update kalman
self.predict()
else :
self.get_logger().info(f"{self.t}")
self.predict()
self.estimate()
self.estimations_x.append(self.X[0, 0])
self.positions_x.append(self.position.x)
self.estimations_y.append(self.y_calcule)
self.positions_y.append(self.position.y)
else :
self.destroy_timer(self.timer_run)
self.plot_position()
def main(args=None):
rclpy.init(args=args)
node = Kalman()
rclpy.spin(node)
rclpy.shutdown()
#ici on lance les def pour update avec certaines modifictaions
if __name__ == "__main__":
main()
+0
-0
