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'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.
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...
I was trying to get this low order recursive function in matlab. i want to calculate the probability of status of a site at next time step, given that I have the initial probability of that being a status.
P= Probability
x= status(0,1)
Dij= probability to pick a site
P(Status of Site(i) being x at next time step)= Summation[P(Status of Site(i) being x at previous time step)*Dij]
and this is what I have done! but my index always exceeds matrix dimensions! I need help with this.
clear all;
clc;
%function [t,i]= CopyingInfluenceModel
%%Define constants
%% generate some random weights vectori.e. the transition matrix=C
% C=[0 (1,2) 0 (1,4) 0 0 0;
% (2,1) 0 (2,3) 0 0 0 0;
% 0 (3,2) 0 (3,4) 0 0 0;
% (1,4) 0 (4,3) 0 (4,5) 0 0;
% 0 0 0 (5,4) 0 (5,6) (5,7);
% 0 0 0 0 (6,5) 0 (6,7);
% 0 0 0 0 (7,5) (7,6) 0];
%copying probabilities=branch weights
onetwo=0.47;
twothree=0.47;
threefour=0.47;
onefour=0.47;
fourfive=0.023;
fivesix=0.47;
fiveseven=0.47;
sixseven=0.47;
selfweight1=0.06;
selfweight2=0.037;
% SourceNodes - a list of Nodes that are forced to be kept in one side of the cut.
% WeightedGraph - symetric matrix of edge weights. Wi,j is the edge
% connecting Nodes i,j use Wi,j=0 or Wi,j == inf to indicate unconnected Nodes
WeightedGraph=[0 onetwo 0 onefour 0 0 0;
onetwo 0 twothree 0 0 0 0;
0 twothree 0 threefour 0 0 0;
onefour 0 threefour 0 fourfive 0 0;
0 0 0 fourfive 0 fivesix fiveseven;
0 0 0 0 fivesix 0 sixseven;
0 0 0 0 fiveseven sixseven 0];
Dij=sparse(WeightedGraph);
% Initializing the variables
t=[];
i=[];
%assigining the initial conditions
t(1)=0;
p(1)= 0.003; %% initial probability of status
%set index no i to 1(initial condition for i=1)
i=1;
%repeating calculating new probabilities
%% If the probability is zero, terminate while loop
while p(i)>=0
%calculate at the next time step for given index no
t(i+1)= t(i);
%calculate the status_probability at given time t=(i+1)
[p(i+1)]=[p(i)]+sum([p(i)]*[Dij(i)]);
[NextStatus(i)]= [p(i+1)]
%index i increases by 1 to calculate next probability
i=i+1;
end
Stack Trace is:
%%??? Index exceeds matrix dimensions.
%%Error in ==> CopyingInfluenceModel at 54
%%[p(i+1)]=[p(i)]+sum([p(i)]*[Dij(i)]);
The problem is Dij not p. Dij has a fixed length so when i exceeds that the program throws an error.
Added:
I can't really see your logic in the code, but I have a strong feeling that you are calculating something wrong. Dij is a 7 x 7 matrix but you treat it as a vector by calling Dij(i). If you are trying to multiply something by a row or column, you need the Dij(i,:) or Dij(:, i) notation.
The logic as you posted it doesn't work, essentially, p(i+i) isn't defined yet. There are a few ways to do it, depending on if you want to keep p or not. I'll post a method that keeps p around, but some work could be done to make the code more efficient.
p=[p;p(i)+sum(p(i)*Dij(i))];
NextStatus(i)= p(i+1)
I have been trying to label the edges of an undirected graph, also, i am using this matgraph tool! I succeeded in making a graph, i just want to assign weights to it... Please help!!
Here is what i tried,
clear all;
close all;
clc;
g=graph;
for k=1:6
add(g,k,k+1)
add(g,1,4)
add(g,5,7)
end
ndraw(g);
x=rand(1,1);
y=rand(1,1)
A =[0 x 0 x 0 0 0;
x 0 x 0 0 0 0;
0 x 0 x 0 0 0;
x 0 x 0 x 0 0;
0 0 0 y 0 x x;
0 0 0 0 x 0 x;
0 0 0 0 x x 0]
If I understood you correctly you can add this code after the code you wrote:
% get line info from the figure
lineH = findobj(gca, 'type', 'line');
xData = cell2mat(get(lineH, 'xdata')); % get x-data
yData = cell2mat(get(lineH, 'ydata')); % get y-data
% if an edge is between (x1,y1)<->(x2,y2), place a label at
% the center of the line, i.e. (x1+x2)/2 (y1+y2)/2 etc
labelposx=mean(xData');
labelposy=mean(yData');
% generate some random weights vector
weights=randi(21,length(labelposx),1);
% plot the weights on top of the figure
text(labelposx,labelposy,mat2cell(weights), 'HorizontalAlignment','center',...
'BackgroundColor',[.7 .9 .7]);