Asked by Doan Khai Do
on 14 Sep 2019

Hi everyone,

I want to find a robot location in an occupancy grid with known pose, I tried to estimate the robot location (with 3 unknown variables of x,y and theta) by using fminunc but my code didn’t work, really appreciate if anyone can help.

x0=[12,8,pi/3]; %initial guess of robot location robotPose = [x0]; intsectionPts= rayIntersection(map,robotPose,angles,maxrange,0.9) % find intersection points of laser beam and occupied cell hold on for i=1:6 x(i)=intsectionPts(i,1); y(i)=intsectionPts(i,2); end fun = (sqrt((x(1)-x0(1))^2+(y(1)-x0(2))^2)-4.6669)^2+(sqrt((x(2)-x0(1))^2+(y(2)-x0(2))^2)-0.6075)^2+(sqrt((x(3)-x0(1))^2+(y(3)-x0(2))^2)-0.6734)^2+(sqrt((x(4)-x0(1))^2+(y(4)-x0(2))^2)-1.3216)^2+(sqrt((x(5)-x0(1))^2+(y(5)-x0(2))^2)-4.708)^2+(sqrt((x(6)-x0(1))^2+(y(6)-x0(2))^2)-0.9899)^2 [x,fval] = fminunc(fun,x0)

Answer by Raunak Gupta
on 18 Sep 2019

Accepted Answer

Hi,

From the code I can see that variable x and y have six components (is a vector of length six) and are assigned with some value from intersectionPts which is calculated with rayIntersection method. Since these values are assigned beforehand the optimization function fun that is mentioned has a set value from beginning. Also, the third parameter theta that is mentioned is not present in the optimization problem. That is why there is nothing to optimize and solution that is calculated is the one which is assigned by intersectionPts.

I suggest verifying the optimization function and if variable x and y does have six components and intersectionPts is calculated purposefully then I think those can be used as initialization points for the problem.

You may refer to fminunc options for changing the parameters that govern how the optimization is done.

Sign in to comment.

Answer by Doan Khai Do
on 18 Sep 2019

Hi Raunak,

Could you advise which option should I use ?

How could matlab understand to generate new x, and theta if my fun is not satisfied ? As theta is just used to generate 6 sub-data (which is distance from a robot that could generate 6 laser beam) that used for fun function.

Thanks for help.

Raunak Gupta
on 19 Sep 2019

Hi,

Since fminunc is an unconstrained optimization it will start from the intial point that is given. It will give a point closer to starting point such that the points minimizes the optimization function in its neighborhood. If no such point exist that minimizes the objective function then you may try changing the 'Algorithm' property in options parameter. Even if it doesn’t find any new point other than the starting point then the starting point itself is the point that locally minimizes the objective function.

Sign in to comment.

Opportunities for recent engineering grads.

Apply Today
## 0 Comments

Sign in to comment.