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
Hi I am trying to make program for a Gauss seidel power flow solution using matlab and i am facing an error in line 55.
"Error in ==> gauss at 55
[ybus,ych]=ybus;"
How to solve this error? I already tried by adding the function "ybus" but still this error is occurring
% Function ybus
% THIS IS THE PROGRAM FOR CREATING Ybus MATRIX.
function [yb,ych]=ybus
% The line impedances are
zz=[0 0.02+0.1i 0 0 0.05+0.25i
0.02+0.1i 0 0.04+0.2i 0 0.05+0.25i
0 0.04+0.2i 0 0.05+0.25i 0.08+0.4i
0 0 0.05+0.25i 0 0.1+0.5i
0.05+0.25i 0.05+0.25i 0.08+0.4i 0.1+0.5i 0];
% The line chargings are
ych=j*[0 0.03 0 0 0.02
0.03 0 0.025 0 0.020
0 0.025 0 0.02 0.01
0 0 0.02 0 0.075
0.02 0.02 0.01 0.075 0];
% The Ybus matrix is formed here
for m=1:5
for n=1:5
if zz(m,n) == 0
yb(m,n)=0;
else
yb(m,n)=-1/zz(m,n);
end
end
end
for m=1:5
ysum=0;
csum=0;
for n=1:5
ysum=ysum+yb(m,n);
csum=csum+ych(m,n);
end
yb(m,m)=csum-ysum;
end
% Program loadflow_gs
% THIS IS A GAUSS-SEIDEL POWER FLOW PROGRAM
clear all
d2r=pi/180;w=100*pi;
% The Y_bus matrix is
[ybus,ych]=ybus;
g=real(ybus);b=imag(ybus);
% The given parameters and initial conditions are
p=[0;-0.96;-0.35;-0.16;0.24];
q=[0;-0.62;-0.14;-0.08;-0.35];
mv=[1.05;1;1;1;1.02];
th=[0;0;0;0;0];
v=[mv(1);mv(2);mv(3);mv(4);mv(5)];
acc=input('Enter the acceleration constant: ');
del=1;indx=0;
% The Gauss-Seidel iterations starts here
while del>1e-6
% P-Q buses
for i=2:4
tmp1=(p(i)-j*q(i))/conj(v(i));
tmp2=0;
for k=1:5
if (i==k)
tmp2=tmp2+0;
else
tmp2=tmp2+ybus(i,k)*v(k);
end
end
vt=(tmp1-tmp2)/ybus(i,i);
v(i)=v(i)+acc*(vt-v(i));
end
% P-V bus
q5=0;
for i=1:5
q5=q5+ybus(5,i)*v(i);
end
q5=-imag(conj(v(5))*q5);
tmp1=(p(5)-j*q5)/conj(v(5));
tmp2=0;
for k=1:4
tmp2=tmp2+ybus(5,k)*v(k);
end
vt=(tmp1-tmp2)/ybus(5,5);
v(5)=abs(v(5))*vt/abs(vt);
% Calculate P and Q
for i=1:5
sm=0;
for k=1:5
sm=sm+ybus(i,k)*v(k);
end
s(i)=conj(v(i))*sm;
end
% The mismatch
delp=p-real(s)';
delq=q+imag(s)';
delpq=[delp(2:5);delq(2:4)];
del=max(abs(delpq));
indx=indx+1;
if indx==1
pause
end
end
'GS LOAD FLOW CONVERGES IN ITERATIONS',indx,pause
'FINAL VOLTAGE MAGNITUDES ARE',abs(v)',pause
'FINAL ANGLES IN DEGREE ARE',angle(v)'/d2r,pause
'THE REAL POWERS IN EACH BUS IN MW ARE',(real(s)+[0 0 0 0 0.24])*100,pause
'THE REACTIVE POWERS IN EACH BUS IN MVar ARE',(-imag(s)+[0 0 0 0 0.11])*10
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.