EKF localization with unknown correspondence

asked 2018-08-01 17:37:28 -0600

frodyteen gravatar image

updated 2018-08-03 16:35:02 -0600

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: image description image description

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:

1,

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

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 flag offensive close merge delete