How to entegrate existing .m file into the simulink .mdl file - matlab

I'm using robotic toolbox by Peter Corke in Matlab . I have .m file for puma560 robot (it is for robot trajectory. The robot follows given path). When I try to use for ex. "sl_ctorque" simulink file which is in robotic toolbox(it is about computed torque method) , I couldn't entegrate my .m file into the simulink file. My .m file is given below. So if anyone know how to do this idea, I'd appreciate it. Thanks!
clear;clc;
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

You need to write your m file as a function and then use the MATLAB Function Block.
The MATLAB Function block allows you to add MATLABĀ® functions to SimulinkĀ® models for deployment to desktop and embedded processors. This capability is useful for coding algorithms that are better stated in the textual language of the MATLAB software than in the graphical language of the Simulink product.
Then you can open the block as paste your function:
to see an example check out this page.

Related

How to draw weighted convex hull in Matlab

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.

extanded kalman filter for tdoa 3D positioning

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

How to shift x-axis label to right in matlab

I am trying to draw a box plot using matlab. I want to shift the position of the x-axis labels to the right. See the following code which seems not to work correctly.
positions = [2.5 3.5 4.5 5.5 6.5 7.5];
colors = [0 0 1; 0 0 1;0 0 1; 1 0 0;1 0 0;1 0 0];
x = boxplot([KA_query_length Plan_query_length Comp_query_length FR_query_length KI_query_length QA_query_length], 'Position', positions, 'Colors',colors,'whisker',0);
set(gca, 'FontSize', 20,'FontWeight','bold');
Given below is what the graph looks like at the moment.

Entegrating existing .mfile to the .mdl simulink

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...

Debugging 24 symmetric operators

I have written the following code to calculate the disorientation between two points in a large dataset using 24 crystal symmetry operators. The code seems to work fine though the end result is not right. I have a huge dataset containing the euler angles for each points. I find the misorientation between 1st point and 2nd point and then between 2 and 3rd and so on.
I want the end result to contain the corresponding misorientaion angles for each two data points. Following is the full code for your better understanding of my requirement.
LA=[phi1 phi phi2];
function gi=get_gi(pvec)
%pvec is a 1x3 vector of [phi1 phi phi2]
g_11=((cosd(pvec(1)).*cosd(pvec(3)))-(sind(pvec(1)).*sind(pvec(3)).*cosd(pvec(2))));
g_12=((sind(pvec(1)).*cosd(pvec(3)))+(cosd(pvec(1)).*sind(pvec(3)).*cosd(pvec(2))));
g_13= (sind(pvec(3)).*sind(pvec(2)));
g_21 =((-cosd(pvec(1)).*sind(pvec(3)))-(sind(pvec(1)).*cos(pvec(3)).*cos(pvec(2))));
g_22 = ((-sin(pvec(1)).*sind(pvec(3)))+(cosd(pvec(1)).*cosd(pvec(3)).*cosd(pvec(2))));
g_23 = (cosd(pvec(3)).*sind(pvec(2)));
g_31 = (sind(pvec(1)).* sind(pvec(2)));
g_32 = -cosd(pvec(1)).* sind(pvec(2));
g_33 = cosd(pvec(2));
gi =[g_11 g_12 g_13;g_21 g_22 g_23;g_31 g_32 g_33];
f = [1 1 1 -1 1 -1 -1 -1 1 1 -1 -1];
l= [1 1 1];
for i=1:3:10
l1= [f(i) 0 0;0 f(i+1) 0;0 0 f(i+2)];
l2= [0 f(i) 0;0 0 f(i+1);f(i+2) 0 0];
l3= [0 0 f(i);f(i+1) 0 0;0 f(i+2) 0];
l4= -[0 0 f(i);0 f(i+1) 0;f(i+2) 0 0];
l5= -[0 f(i) 0;f(i+1) 0 0;0 0 f(i+2)];
l6= -[f(i) 0 0;0 0 f(i+1);0 f(i+2) 0];
l=[l;l1;l2;l3;l4;l5;l6];
end
k=1;
t=1;
for m=1:(length(a)-1)
for i=2:3:71
for j=2:3:71
g_r= (get_gi(LA(m,:)*l(i:i+2,1:3))*(inv(get_gi(LA(m+1,:)))*inv(l(j:j+2,1:3))));
tr(k)= g_r(1,1) + g_r(2,2) +g_r(3,3);
angle(k) = real(acosd((tr(k)-1)/2));
k=k+1;
end
angle(angle==0)=360;
del_theta=min(angle)
del(t)=del_theta;
t=t+1;
end