Suppose I have a Matlab array PE of size Ex1. The elements of PE are between 0 and 1 and they sum up to one.
Take another array PEY of size ExY. The elements of PEY are one or zero. Moreover, for each row, there exists at least a one. Y=3.
For example
clear
E=4;
Y=3;
PE=[1/2; 1/6; 1/6; 1/6];
PEY=[1 0 0; 0 1 1; 1 1 1; 0 1 1];
Now, consider the simplex with vertices (1,0,0), (0,1,0), and (0,0,1)
patch([0 0 1],[0 1 0],[1 0 0],[0.8 0.8 0.8]);
axis equal
axis([0 1 0 1 0 1])
view(120,30)
I want to draw a convex subset A of such simplex. A is constructed as follows.
STEP 1: We construct an Ex1 cell array PEY_expanded such that, for each e-th row of PEY that has more than a 1, we write down all admissible 1xY vectors containing just a 1 and stack them in PEY_expanded{e}.
PEY_expanded=cell(E,1);
for e=1:E
if isequal(PEY(e,:),[1 1 1])
PEY_expanded{e}=[1 0 0; 0 1 0; 0 0 1];
elseif isequal(PEY(e,:),[1 1 0])
PEY_expanded{e}=[1 0 0; 0 1 0];
elseif isequal(PEY(e,:),[1 0 1])
PEY_expanded{e}=[1 0 0; 0 0 1];
elseif isequal(PEY(e,:),[0 1 1])
PEY_expanded{e}=[0 1 0; 0 0 1];
else
PEY_expanded{e}=PEY(e,:);
end
end
STEP 2: Take Cartesian product PEY_expanded{1} x PEY_expanded{2} x ... PEY_expanded{E} and get PEY_cartesian.
Note: the code below is specific for E=4 and not general
size_PEY_expanded=zeros(E,1);
for e=1:E
size_PEY_expanded(e)=size(PEY_expanded{e},1);
end
[a,b,c,d]=ndgrid(1: size_PEY_expanded(1),1: size_PEY_expanded(2),...
1: size_PEY_expanded(3), 1: size_PEY_expanded(4));
PEY_Cartesian= [PEY_expanded{1}(a,:),PEY_expanded{2}(b,:),...
PEY_expanded{3}(c,:), PEY_expanded{4}(d,:)];
PEY_Cartesian_rearranged=cell(prod(size_PEY_expanded),1);
for i=1:prod(size_PEY_expanded)
PEY_Cartesian_rearranged{i}=[PEY_Cartesian(i,1:3); PEY_Cartesian(i,4:6);...
PEY_Cartesian(i,7:9); PEY_Cartesian(i,10:end)];
end
PEY_Cartesian=PEY_Cartesian_rearranged;
STEP 3: For each possible cell of PEY_Cartesian, for y=1,...,Y, weight PEY_Cartesian{i}(e,y) by PE(e) and then sum across e.
PY=zeros(prod(size_PEY_expanded),Y);
for i=1:prod(size_PEY_expanded)
for y=1:Y
temp=0;
for e=1:E
temp=temp+PE(e)*PEY_Cartesian{i}(e,y);
end
PY(i,y)=temp;
end
end
STEP 4: Draw the region A that is the convex hull of the rows of PY (black region in the picture)
%Need https://fr.mathworks.com/matlabcentral/fileexchange/37004-suite-of-functions-to-perform-uniform-sampling-of-a-sphere
close all
patch([0 0 1],[0 1 0],[1 0 0],[0.8 0.8 0.8]);
axis equal
axis([0 1 0 1 0 1])
view(120,30)
hold on
T = delaunayTriangulation(PY);
K = convexHull(T);
patch('Faces',K,'Vertices',T.Points,'FaceColor','k','edgecolor','k');
hold on
QUESTION:
The algorithm above is unfeasible for large E. In my actual case I have E=216, for example. In particular, step 2 is unfeasible.
Could you suggest an easier way to proceed? Given that A is a convex region, maybe there is some shortcut I'm unable to see.
I am trying to use the Extended Kalman filter to locate my mobile device in the spice using the TDoA method.
The problem I get that is my cone doesn't converge to the ground truth position.
Sometimes the matrix P, Xh, and X have complex values is that OK?
Have I to modify something or to add stop conditions.
a part of my code is like
%% inititialisation
% Covarience matrix of process noise
Q=[ 0.01 0 0;0 0.01 0;0 0 0.01];
% Covarience matrix of measurment noise
R=[ 0.001 0.0005 0.0005 ; 0.0005 0.001 0.0005 ; 0.0005 0.0005 0.001];
% System Dynamics
A=[1 0 0 ;0 1 0 ;0 0 1 ];
%Assumed initial conditions
Xh(:,1)=[0 0 0]';
B1=[2 2 3; 1.5 2 3; 2.5 2 3; 2 1.5 3; 2 2.5 3]; %the position of 5 beacons installes in the ceiling
%inital value of covarience of estimation error
P(:,:,1)=[0 0 0 ; 0 0 0 ; 0 0 0 ];
Xb=[2 2 2];
X(:,1)=Xb;%
Z(:,1)=Dd20(:,1);%Dd20 is a matrix of TDOA 3*n ofsize
for n=1:2000
% PROCESS AND OBSERVATION PROCESS WITH GAUSSINA NOISE
X(:,n+1)=A*X(:,n)+[sqrt(((Q(1,1))*randn(1)));sqrt((Q(2,2))*randn(1));sqrt((Q(3,3))*randn(1))]; % State process % w generating process noise
Z(:,n+1)=Z(:,n)+[sqrt(R(1,1))*randn(1);sqrt(R(1,1))*randn(1);sqrt(R(1,1))*randn(1)];%mesurment matrix
hsn(:,n+1)=[(sqrt(((X(1,n+1)-B1(5,1))^2+(X(2,n+1)-B1(5,2))^2+(X(3,n+1)-B1(5,3))^2))-(sqrt(((X(1,n+1)-B1(1,1))^2+(X(2,n+1)-B1(2,1))^2+(X(3,n+1)-B1(1,3))^2))));
(sqrt(((X(1,n+1)-B1(2,1))^2+(X(2,n+1)-B1(2,2))^2+(X(3,n+1)-B1(2,3))^2))-(sqrt(((X(1,n+1)-B1(1,1))^2+(X(2,n+1)-B1(2,1))^2+(X(3,n+1)-B1(1,3))^2))));
(sqrt(((X(1,n+1)-B1(4,1))^2+(X(2,n+1)-B1(4,2))^2+(X(3,n+1)-B1(4,3))^2))-(sqrt(((X(1,n+1)-B1(1,1))^2+(X(2,n+1)-B1(2,1))^2+(X(3,n+1)-B1(1,3))^2))))] +[sqrt(((R(1,1))*randn(1)));sqrt(((R(1,1))*randn(1)));sqrt(((R(1,1))*randn(1)))];
% prediction of next state
Xh(:,n+1)=A*Xh(:,n);% ESTIMATE
P(:,:,n+1)=A*P(:,:,n)*A'+Q;% PRIORY ERROR COVARIENCE
% CORRECTION EQUTIONS
H(:,:,n+1)=[(Xh(1,n+1)-B1(5,1))/((sqrt(((Xh(1,n+1)-B1(5,1)^2)+(Xh(2,n+1)-B1(5,2)^2)+(Xh(3,n+1)-B1(5,3)^2))))-(Xh(1,n+1)-B1(5,1))/(sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2))))),...
(Xh(2,n+1)-B1(5,2))/((sqrt(((Xh(1,n+1)-B1(5,1)^2)+(Xh(2,n+1)-B1(5,2)^2)+(Xh(3,n+1)-B1(5,3)^2))))-(Xh(2,n+1)-B1(5,2))/(sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2))))),
(Xh(3,n+1)-B1(5,3))/((sqrt(((Xh(1,n+1)-B1(5,1)^2)+(Xh(2,n+1)-B1(5,2)^2)+(Xh(3,n+1)-B1(5,3)^2))))-(Xh(3,n+1)-B1(5,3))/(sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2)))));
(Xh(1,n+1)-B1(2,1))/(sqrt(((Xh(1,n+1)-B1(2,1)^2)+(Xh(2,n+1)-B1(2,2)^2)+(Xh(3,n+1)-B1(2,3)^2))))-(Xh(1,n+1)-B1(1,1))/((sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2))))),
(Xh(2,n+1)-B1(2,2))/(sqrt(((Xh(1,n+1)-B1(2,1)^2)+(Xh(2,n+1)-B1(2,2)^2)+(Xh(3,n+1)-B1(2,3)^2))))-(Xh(2,n+1)-B1(1,2))/((sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2))))),
(Xh(3,n+1)-B1(2,3))/(sqrt(((Xh(1,n+1)-B1(2,1)^2)+(Xh(2,n+1)-B1(2,2)^2)+(Xh(3,n+1)-B1(2,3)^2))))-(Xh(3,n+1)-B1(1,3))/((sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2)))));
(Xh(1,n+1)-B1(4,1))/(sqrt(((Xh(1,n+1)-B1(4,1)^2)+(Xh(2,n+1)-B1(4,2)^2)+(Xh(3,n+1)-B1(4,3)^2))))-(Xh(1,n+1)-B1(1,1))/((sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2))))),
(Xh(2,n+1)-B1(4,2))/(sqrt(((Xh(1,n+1)-B1(4,1)^2)+(Xh(2,n+1)-B1(4,2)^2)+(Xh(3,n+1)-B1(4,3)^2))))-(Xh(2,n+1)-B1(1,2))/((sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2))))),
(Xh(3,n+1)-B1(4,3))/(sqrt(((Xh(1,n+1)-B1(4,1)^2)+(Xh(2,n+1)-B1(4,2)^2)+(Xh(3,n+1)-B1(4,3)^2))))-(Xh(3,n+1)-B1(1,3))/((sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2)))));
]; % Jacobian matrix
%THIS SUBROTINE COMPUTES KALMAN GAIN
K(:,:,n+1)=P(:,:,n+1)*H(:,:,n+1)'*(R+H(:,:,n+1)*P(:,:,n+1)*H(:,:,n+1)')^(-1);
Inov=Z(:,n+1)-hsn(:,n);% INNOVATION
Xh(:,n+1)=Xh(:,n+1)+ K(:,:,n+1)*Inov; %computes final estimate
P(:,:,n+1)=(eye(3)-K(:,:,n+1)*H(:,:,n+1))*P(:,:,n+1);% %computes covarience of estimation error
end
I convert my .m file above as a function like below , my input is nothing and my output is q. But I have a problem. When I put my created function block to the simulink and connect to the display screen , matlab gives me some errors like;
*Try and catch are not supported for code generation.
Function 'tb_optparse.m' (#80.5667.6083), line 157, column 25:
"try"
Launch diagnostic report.*
Function call failed.
Function 'MATLAB Function' (#94.848.897), line 37, column 3:
"mstraj(path, [15 15 15], [], [1 0 1], 0.02 , 0.2)"
Launch diagnostic report.
Errors occurred during parsing of MATLAB function 'MATLAB Function'(#93)
How can I fix these errors? Thanks
function output = fcn()
%mdl_puma560 %to create puma robot
for type=1:3 % main for loop. It turns 3 times. At first, it sets the path
% to x-y plane and draw the robot, at second for y-z plane
% and then for x-z plane
if type==1
% The path of robot for x-y plane
path=[0 0 1;0 0 0;0 2 0 ;0.5 1 0 ;1 2 0;1 0 0;1.5 0 1;1.5 0 0;
1.5 2 0;2.2 2 0;2.5 1.6 0;2.5 0.4 0;2.2 0 0;1.5 0 0;0 0 1];
elseif type==2
% Same thing as first part
path=[-0.5 0 0;0 0 0;0 0 1;0 -0.5 0.5;0 -1 1;0 -1 0;-0.5 -1.2 0;0 -1.2 0;
0 -1.2 1;0 -1.7 1;0 -2 0.7;0 -2 0.3;0 -1.7 0;0 -1.2 0];
elseif type==3
% Same thing as first and second part
path=[0 -0.5 0;0 0 0;0 0 1;0.5 0 0.5;1 0 1;1 0 0;1.3 -0.5 0;1.3 0 0;
1.3 0 1;1.7 0 1;2 0 0.7;2 0 0.3;1.7 0 0;1.3 0 0];
end
% I created a trajectory
p=mstraj(path, [15 15 15], [], [1 0 1], 0.02 , 0.2);
% [15 15 15] means the maximum speed in x,y,z directions.
% [1 0 1] means the initial coordinates
% 0.02 means acceleration time
% 0.2 means smoothness of robot
numrows(p)*0.2; % 200 ms sample interval
Tp=transl(0.1*p); % Scale factor of robot
Tp=homtrans( transl(0.4,0,0),Tp); % Origin of the letter
q=p560.ikine6s(Tp) ; % The inverse kinematic
% for i=1:length(q)
% %q matrix has 280 rows and 6 columns. So this for loop turns 280 times
% % At every turns , it plots one part of movement. q(1,:), q(2,:), ...
%
% p560.plot(q(i,:))
%
% end
end
output=q;
Well as the error message says, it looks like your function mstraj is calling try/catch which isn't supported for code generation (MATLAB functions in Simulink are first converted to C code when you run the model).
Have a look at Call MATLAB Functions in the documentation for ways to work around this using coder.extrinsic. Extrinsic functions return data of type mxArray so you will need to convert it to whatever the data type of p (see Converting mxArrays to Known Types in the documentation page above).
In you case, it would probably look something like:
function output = fcn()
coder.extrinsic('mstraj');
% etc...
p = 0; % Define p as a scalar of type double (change to required data type if not appropriate)
p=mstraj(path, [15 15 15], [], [1 0 1], 0.02 , 0.2);
% etc...