trying to use fmincon to constrain results to a particular surface, results get stuck at a corner

We are trying to creat a system that can select objects a person is looking at in a room based on user head position and orientation. We decided to include a Duble Kalman filter, this should output a smootend version of the user's head position (q) and the point on the wall where the user's gaze falls (x).

The kaman filter doesn't know where the walls are, one of my supervisors found a paper that suggests using the fmincon function to constrain x to the walls of the room. For some reason the fmincon function keeps sending x to the corners of the room, green objects in the figure below, the red represents q.

% see what wall user is looking at and select constraints accoringly

surfaceNo_(globalIndx_) = estimateSurface2(zk_(1:2,globalIndx_),q_upd_(:,globalIndx_));

[Af,b,Aeq,beq,lb,ub] = getConstraints(surfaceNo_(globalIndx_));

W=Px_upd_(:,:,globalIndx_)^-1;

x_upd_constrained_(:,globalIndx_) = fun_con(x_upd_(:,globalIndx_),W,Af,b,Aeq,beq,lb,ub);

% fprintf('surface: %c\n--------------------\n',surfaceNo)

gamma=(W^-1)*Aeq'*((Aeq*(W^-1)*Aeq')^-1);

Px_upd_constrained_(:,:,globalIndx_) = (eye(3)-gamma*Aeq)*Px_upd_(:,:,globalIndx_);

function x_udp_constrained=fun_con(x_udp,W,A,b,Aeq,beq,lb,ub)

% x_udp_constrained = fmincon(@fun,x_udp,A,b,Aeq,beq,lb,ub);

x_udp_constrained = fmincon(@fun,x_udp,A,b,Aeq,beq);

f0=fun(x_udp);

f=fun(x_udp_constrained);

function f=fun(x)

f = (x-x_udp)'*W*(x-x_udp);

end

end

Answer 1

[Varun]

Hi Ahmed,

It appears that you are using “fmincon” function in your implementation to solve a constrained optimization problem where the objective is to minimize the weighted squared distance (x - x_upd)'*W*(x - x_upd). But you are facing an issue with “fmincon” sending “x” to the corners of the room.

To workaround this, you can try experimenting with different optimization options for “fmincon”. You can set these options using the “optimoptions” function in MATLAB. For example, try different algorithms like “sqp” or “interior-point”.

Please refer to the following code snippet:

options = optimoptions('fmincon', 'Display', 'iter', ...
                       'Algorithm', 'sqp', ...
                       'PlotFcn', {@optimplotx, @optimplotfval, @optimplotconstrviolation});
x_upd_constrained = fmincon(@fun, x_upd, A, b, Aeq, beq, lb, ub, [], options);

You can also try to improve the initial guess to “fmincon” for e.g., setting the initial guess that is more centrally located within the room rather than near the corners.

Please refer to the following documentations to learn more:

1. “fmincon”: https://www.mathworks.com/help/optim/ug/fmincon.html
2. “Optimization Options”: https://www.mathworks.com/help/optim/ug/optimization-options-reference.html

Hope it helps.

[Salah Ad-Din Ahmed]

Thanks I apritiate your answer, but we have aalready solved the problem. It was due to angle subtraction in the residual. ex lets say your residual = (measument - prrediction) = 179 degrees - (-179 degrees) = 358 degrees,

this is the obtuse ange, in reality we need the acute angle for the difference between the 2 angles is actually 2 degrees, however when subtracting the 2 we get a much larger number causing the kalman filter to overcompensate for the state resulting in the state going to random locations, then we add constraints and we end up with the situations you saw.

to solve the we used residual = wrapToPi(measument - prrediction) since the result of the wrapToPi() funtion is in the range -180 to 180, 358 is wraped to 2 degrees.