How to optimize a function using fminunc with 3 unknown variables and many dependent variables

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 1

[Raunak Gupta]

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.

Answer 2

[Doan Khai Do]

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]

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.