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!