EKF localization with unknown correspondence

edit

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!