ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Wed, 01 Aug 2018 17:37:28 -0500EKF localization with unknown correspondencehttps://answers.ros.org/question/299372/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](https://github.com/liulinbo/slam/blob/master/Probabilistic%20Robotics%20_Sebastian%20Thrun%20et%20al..pdf ), to understand SLAM better.
Here is a picture of the algorithm in the book:
![image description](/upfiles/15331609323141694.jpg)
![image description](/upfiles/1533160941554923.jpg)
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](/upfiles/15331603438645169.png),
then, provided a map using gmapping in rviz shown below:
![image description](/upfiles/15331603956514511.png).
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!Wed, 01 Aug 2018 17:37:28 -0500https://answers.ros.org/question/299372/ekf-localization-with-unknown-correspondence/