# EKF localization with unknown correspondence

I am working on a simulation with the huskyA200. I want to implement the EKF localization with unknown correspondences (CH7.5) in the probabilistic robotics book by Thrun, to understand SLAM better. Here is a picture of the algorithm in the book:

I am a little confused on where to place this algorithm in my python code.

following is my simulation setup for this algorithm, I have created a world with 6 landmarks shown below:

,

then, provided a map using gmapping in rviz shown below: .

Then, I wrote down coordinates of 6 landmarks using the publish point in the rviz GUI.

Now, in my code, I am planning to call ekf_localization() algorithm in my laserCallback function, shown below:

    #!/usr/bin/env python
from std_msgs.msg import String
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
import rospy
import numpy as np
import math

#parameters
alp1 = 1
alp2 = 1
alp3 = 1
alp4 = 1
sigmaR = 1
sigmaT = 1
sigmaS = 1

NUMBER_OF_LANDMARKS = 6

start_angle = -1.57079637051
end_angle = 1.56643295288
dA = 0.00436332309619
inc_angle = 0;

z=0
u = np.matrix(np.zeros((3, 1)))

#lankmark coordinates in the map
m = np.matrix('0.372 1.85; \
3.95 1.95; \
5.33 5.73; \
2.23 -2.77; \
7 -1.95; \
8.36 1.08')

mu = np.matrix(np.zeros((3, 1)))
cov = np.eye(3)
dt = 0.5;
flag = 1
def ekf_localization(hMu, hCov, u, z, m):
#ekf algorithm code from ch7.5
#
#

def odomCallback(msg):
global x, y, theta, u, mu, cov, m

#retrieve the pose from odometry
#x = msg.pose.pose.position.x
#y = msg.pose.pose.position.y
#theta = msg.pose.pose.orientation.z

#retrieve linear and angular velocity
u = np.matrix([
[msg.twist.twist.linear.x],
[msg.twist.twist.angular.z]])

def laserCallback(msg):
global z
#retrieve the laser range data
z = msg.ranges
#this is where I call the ekf algorithm
ekf_localization(mu, cov, u, z, m);

def main():
print("Husky_ekf_localization start!!!")

rospy.init_node('Husky_ekf_localization',anonymous = True)
rospy.Subscriber("/husky_velocity_controller/odom",Odometry,odomCallback)
rospy.Subscriber("scan",LaserScan,laserCallback)
rospy.spin()

if __name__ == '__main__':
main()


My questions: Is this is the correct approach to implement this algorithm?

Since I am using a hokuyo 04lx laser, and each scan from 0 to 180 degrees has 720 range data points. Does step 9 means to extract the 720 data points to get the data that’s corresponding to landmarks and loop through those extracted data points? Any feedback is appreciated, thank you!

edit retag close merge delete