Plotting graphs from a for loop in MATLAB - matlab

I have this for loop
for mass = 1:.1:4
theta = 60;
Fw = [0 -9.8*mass 0];
Rw = [ 0.2*cosd(theta) 0.2*sind(theta) 0];
Mw = cross(Rw,Fw);
Fjrf = [30 50 0];
Rjrf = [0.4*cosd(theta) 0.4*sind(theta) 0];
Mjrf = cross(Rjrf,Fjrf);
alpha = 5;
Ialpha = [0 0 alpha*(0.02+mass*0.2^2)];
Ms = Ialpha - Mjrf - Mw;
omega = [0 0 pi];
alphav = [0 0 5];
anorm = ([-cosd(theta) -sind(theta) 0]*(norm(omega))^2);
atang = cross(Rjrf,alphav);
a = anorm + atang;
Fmuscle = norm(mass*a - Fjrf - Fw)
mass
plot(mass,Fmuscle)'
hold on
end
Where in the end the output is just various values of Fmuscle, that correspond to values of mass varrying from 1 to 4 at intervals of .1. I am trying to plot all these values on a graph (mass =x and Fmuscle = y) but I cannot figure out how to generate this.
With the code I have above I simply get an empty graph, even though I get all the right variables.
Thanks!

This what you want?
i = 0; %set initial index
clear x y; %clear these variables
for mass = 1:.1:4
i = i + 1; %increment index
theta = 60;
Fw = [0 -9.8*mass 0];
Rw = [ 0.2*cosd(theta) 0.2*sind(theta) 0];
Mw = cross(Rw,Fw);
Fjrf = [30 50 0];
Rjrf = [0.4*cosd(theta) 0.4*sind(theta) 0];
Mjrf = cross(Rjrf,Fjrf);
alpha = 5;
Ialpha = [0 0 alpha*(0.02+mass*0.2^2)];
Ms = Ialpha - Mjrf - Mw;
omega = [0 0 pi];
alphav = [0 0 5];
anorm = ([-cosd(theta) -sind(theta) 0]*(norm(omega))^2);
atang = cross(Rjrf,alphav);
a = anorm + atang;
Fmuscle = norm(mass*a - Fjrf - Fw);
x(i) = mass; % set the appropriate row of x vector
y(i) = Fmuscle; % same for y
end
plot(x, y);

Related

Is there a way to derive implict ODE function from big symbolic expression?

I'm Davide and I have a problem with the derivation of a function that later should be given to ode15i in Matlab.
Basically I've derived a big symbolic expression that describe the motion of a spececraft with a flexible appendice (like a solar panel). My goal is to obtain a function handle that can be integrated using the built-in implicit solver in Matlab (ode15i).
The problem I've encounter is the slowness of the Symbolic computations, especially in the function "daeFunction" (I've run it and lost any hope for a responce after 3/4 hours had passed).
The system of equations, that is derived using the Lagrange's method is an implicit ode.
The complex nature of the system arise from the flexibility modelling of the solar panel.
I am open to any suggestions that would help me in:
running the code properly.
running it as efficiently as possible.
Thx in advance.
Here after I copy the code. Note: Matlab r2021a was used.
close all
clear
clc
syms t
syms r(t) [3 1]
syms angle(t) [3 1]
syms delta(t)
syms beta(t) [3 1]
mu = 3.986e14;
mc = 1600;
mi = 10;
k = 10;
kt = 10;
Ii = [1 0 0 % for the first link it is different thus I should do a functoin or something that writes everything into an array or a vector
0 5 0
0 0 5];
% Dimension of satellite
a = 1;
b = 1.3;
c = 1;
Ic = 1/12*mc* [b^2+c^2 0 0
0 c^2+a^2 0
0 0 a^2+b^2];
ra_c = [0 1 0]';
a = diff(r,t,t);
ddelta = diff(delta,t);
dbeta = diff(beta,t);
dddelta = diff(delta,t,t);
ddbeta = diff(beta,t,t);
R= [cos(angle1).*cos(angle3)-cos(angle2).*sin(angle1).*sin(angle3) sin(angle1).*cos(angle3)+cos(angle2).*cos(angle1).*sin(angle3) sin(angle2).*sin(angle3)
-cos(angle1).*sin(angle3)-cos(angle2).*sin(angle1).*cos(angle3) -sin(angle1).*sin(angle3)+cos(angle2).*cos(angle1).*cos(angle3) sin(angle2).*cos(angle3)
sin(angle2).*sin(angle3) -sin(angle2).*cos(angle1) cos(angle2)];
d_angle1 = diff(angle1,t);
d_angle2 = diff(angle2,t);
d_angle3 = diff(angle3,t);
dd_angle1 = diff(angle1,t,t);
dd_angle2 = diff(angle2,t,t);
dd_angle3 = diff(angle3,t,t);
d_angle = [d_angle1;d_angle2;d_angle3];
dd_angle = [dd_angle1;dd_angle2;dd_angle3];
omega = [d_angle2.*cos(angle1)+d_angle3.*sin(angle2).*sin(angle1);d_angle2.*sin(angle1)-d_angle3.*sin(angle2).*cos(angle1);d_angle1+d_angle3.*cos(angle2)]; % this should describe correctly omega_oc
d_omega = diff(omega,t);
v1 = diff(r1,t);
v2 = diff(r2,t);
v3 = diff(r3,t);
v = [v1; v2; v3];
[J,r_cgi,R_ci]= Jacobian_Rob(4,delta,beta);
% Perform matrix multiplication
for mm = 1:4
vel(:,mm) = J(:,:,mm)*[ddelta;dbeta];
end
vel = formula(vel);
dr_Ccgi = vel(1:3,:);
omega_ci = vel(4:6,:);
assumeAlso(angle(t),'real');
assumeAlso(d_angle(t),'real');
assumeAlso(dd_angle(t),'real');
assumeAlso(r(t),'real');
assumeAlso(a(t),'real');
assumeAlso(v(t),'real');
assumeAlso(beta(t),'real');
assumeAlso(delta(t),'real');
assumeAlso(dbeta(t),'real');
assumeAlso(ddelta(t),'real');
assumeAlso(ddbeta(t),'real');
assumeAlso(dddelta(t),'real');
omega = formula(omega);
Tc = 1/2*v'*mc*v+1/2*omega'*R*Ic*R'*omega;
% kinetic energy of all appendices
for h = 1:4
Ti(h) = 1/2*v'*mi*v+mi*v'*skew(omega)*R*ra_c+mi*v'*skew(omega)*R*r_cgi(:,h)+mi*v'*R*dr_Ccgi(:,h)+1/2*mi*ra_c'*R'*skew(omega)'*skew(omega)*R*ra_c ...
+ mi*ra_c'*R'*skew(omega)'*skew(omega)*R*r_cgi(:,h)+mi*ra_c'*R'*skew(omega)'*R*dr_Ccgi(:,h)+1/2*omega'*R*R_ci(:,:,h)*Ii*(R*R_ci(:,:,h))'*omega ...
+ omega'*R*R_ci(:,:,h)*Ii*R_ci(:,:,h)'*omega_ci(:,h)+1/2*omega_ci(:,h)'*R_ci(:,:,h)*Ii*R_ci(:,:,h)'*omega_ci(:,h)+1/2*mi*r_cgi(:,h)'*R'*skew(omega)'*skew(omega)*R*r_cgi(:,h)+mi*r_cgi(:,h)'*R'*skew(omega)'*R*dr_Ccgi(:,h)...
+ 1/2*mi*dr_Ccgi(:,h)'*dr_Ccgi(:,h);
Ugi(h) = -mu*mi/norm(r,2)+mu*mi*r'/(norm(r,2)^3)*(R*ra_c+R*R_ci(:,:,h)*r_cgi(:,h));
end
Ugc = -mu*mc/norm(r,2);
Ue = 1/2*kt*(delta)^2+sum(1/2*k*(beta).^2);
U = Ugc+sum(Ugi)+Ue;
L = Tc + sum(Ti) - U;
D = 1/2 *100* (ddelta^2+sum(dbeta.^2));
%% Equation of motion derivation
eq = [diff(jacobian(L,v),t)'-jacobian(L,r)';
diff(jacobian(L,d_angle),t)'-jacobian(L,angle)';
diff(jacobian(L,ddelta),t)'-jacobian(L,delta)'+jacobian(D,ddelta)';
diff(jacobian(L,dbeta),t)'-jacobian(L,beta)'+jacobian(D,dbeta)'];
%% Reduction to first order sys
[sys,newVars,R1]=reduceDifferentialOrder(eq,[r(t); angle(t); delta(t); beta(t)]);
DAEs = sys;
DAEvars = newVars;
%% ode15i implicit solver
pDAEs = symvar(DAEs);
pDAEvars = symvar(DAEvars);
extraParams = setdiff(pDAEs,pDAEvars);
f = daeFunction(DAEs,DAEvars,'File','ProvaSum');
y0est = [6778e3 0 0 0.01 0.1 0.3 0 0.12 0 0 0 7400 0 0 0 0 0 0 0 0]';
yp0est = zeros(20,1);
opt = odeset('RelTol', 10.0^(-7),'AbsTol',10.0^(-7),'Stats', 'on');
[y0,yp0] = decic(f,0,y0est,[],yp0est,[],opt);
% Integration
[tSol,ySol] = ode15i(f,[0 0.5],y0,yp0,opt);
%% Funcitons
function [J,p_cgi,R_ci]=Jacobian_Rob(N,delta,beta)
% Function to compute Jacobian see Robotics by Siciliano
% N total number of links
% delta [1x1] beta [N-1x1] variable that describe position of the solar
% panel elements
beta = formula(beta);
L_link = [1 1 1 1]'; % Length of each link elements in [m], later to be derived from file or as function input
for I = 1 : N
A1 = Homog_Matrix(I,delta,beta);
A1 = formula(A1);
R_ci(:,:,I) = A1(1:3,1:3);
if I ~= 1
p_cgi(:,I) = A1(1:3,4) + A1(1:3,1:3)*[1 0 0]'*L_link(I)/2;
else
p_cgi(:,I) = A1(1:3,4) + A1(1:3,1:3)*[0 0 1]'*L_link(I)/2;
end
for j = 1:I
A_j = formula(Homog_Matrix(j,delta,beta));
z_j = A_j(1:3,3);
Jp(:,j) = skew(z_j)*(p_cgi(:,I)-A_j(1:3,4));
Jo(:,j) = z_j;
end
if N-I > 0
Jp(:,I+1:N) = zeros(3,N-I);
Jo(:,I+1:N) = zeros(3,N-I);
end
J(:,:,I)= [Jp;Jo];
end
J = formula(J);
p_cgi = formula(p_cgi);
R_ci = formula(R_ci);
end
function [A_CJ]=Homog_Matrix(J,delta,beta)
% This function is made sopecifically for the solar panel
% define basic rotation matrices
Rx = #(angle) [1 0 0
0 cos(angle) -sin(angle)
0 sin(angle) cos(angle)];
Ry = #(angle) [ cos(angle) 0 sin(angle)
0 1 0
-sin(angle) 0 cos(angle)];
Rz = #(angle) [cos(angle) -sin(angle) 0
sin(angle) cos(angle) 0
0 0 1];
if isa(beta,"sym")
beta = formula(beta);
end
L_link = [1 1 1 1]'; % Length of each link elements in [m], later to be derived from file or as function input
% Rotation matrix how C sees B
R_CB = Rz(-pi/2)*Ry(-pi/2); % Clarify notation: R_CB represent the rotation matrix that describe the frame B how it is seen by C
% it is the same if it was wrtitten R_B2C
% becouse bring a vector written in B to C
% frame --> p_C = R_CB p_B
% same convention used in siciliano how C sees B frame
A_AB = [R_CB zeros(3,1)
zeros(1,3) 1];
A_B1 = [Rz(delta) zeros(3,1)
zeros(1,3) 1];
A_12 = [Ry(-pi/2)*Rx(-pi/2)*Rz(beta(1)) L_link(1)*[0 0 1]'
zeros(1,3) 1];
if J == 1
A_CJ = A_AB*A_B1;
elseif J == 0
A_CJ = A_AB;
else
A_CJ = A_AB*A_B1*A_12;
end
for j = 3:J
A_Jm1J = [Rz(beta(j-1)) L_link(j-1)*[1 0 0]'
zeros(1,3) 1];
A_CJ = A_CJ*A_Jm1J;
end
end
function [S]=skew(r)
S = [ 0 -r(3) r(2); r(3) 0 -r(1); -r(2) r(1) 0];
end
I found your question beautiful. My suggestion is to manipulate the problem numerically. symbolic manipulation in Matlab is good but is much slower than numerical calculation. you can define easily the ode into a system of first-order odes and solve them using numerical integration functions like ode45. Your code is very lengthy and I couldn't manage to follow its details.
All the Best.
Yasien

Struggling with while loop (computer graphics)

Working on a little project to do with computer graphics. So far I have (I think) everything in order as the code below patches my world to the screen fine.
However, I have a final hurdle to jump: I would like the code to patch for different values of theta. In the code, it is set at 2*pi/4 but I would like to iterate and patch for every angle between 0:pi/4:2*pi. However, when I try to put the code in a for or while loop it doesn't seem to do what I expect, that is, to patch with one angle, then patch with another etc.
Really stuck I have tried a lot of stuff and now I'm just without any ideas. Would really appreciate any help or suggestions.
function world()
% Defining House Vertices
house_verts = [-5, 0, -5;
5, 0, -5;
5, 10, -5;
0,15,-5;
-5,10,-5;
-5,0,5;
5,0,5;
5,10,5;
0,15,5;
-5,10,5];
% Sorting out the homeogenous co-ordinates
ones = [1,1,1,1,1,1,1,1,1,1];
ones=transpose(ones);
house_verts = [house_verts, ones];
house_verts = transpose(house_verts);
% House faces
house_faces = [1,2,3,4,5;
2,7,8,3,3;
6,7,8,9,10;
1,6,10,5,5;
3,4,9,8,8;
4,5,10,9,9;
1,2,7,6,6];
world_pos = [];
% creating a street
street_vector = [1,0,1]; % the direction of the street
orthog_street_vector = [-1,0,1];
for i = 1:15
% current_pos1 and 2 will be the positions of the two houses
% opposite each other on the street
current_pos1 = 30*i*street_vector + 50*orthog_street_vector;
current_pos2 = 30*i*street_vector - 50*orthog_street_vector;
world_pos = [world_pos;current_pos1;current_pos2];
end
% initialising world vertices and faces
world_verts = [];
world_faces = [];
% Populating the street
for i =1:size(world_pos,1)
T = transmatrix(world_pos(i,:)); % a translation matrix
s = [1,1/2 + rand(),1];
S=scalmatrix(s); % a matrix for a random scaling of the height (y-coordinate)
Ry = rotymatrix(rand()*2*pi); % a matrix for a random rotation about the y-axis
A = T*Ry*S; % the compound transformation matrix to take the house into the world
obj_faces = size(world_verts,2) + house_faces; %increments the basic house faces to match the current object
obj_verts = A*house_verts;
world_verts = [world_verts, obj_verts]; % adds the vertices to the world
world_faces = [world_faces; obj_faces]; % adds the faces to the world
end
% initialising aligned vertices
align_verts = [];
% Aligning the vertices to the particular camera at angle theta
for elm = world_verts
x = 350 + 350*cos(2*pi/4);
z = 350 + 350*sin(2*pi/4);
y = 80;
u = [x,y,z];
v = [250,0,250];
d = v - u;
phiy = atan2(d(1),d(3));
phix = -atan2(d(2),sqrt(d(1)^2+d(3)^2));
T = transmatrix([-u(1),-u(2),-u(3)]);
Ry = rotymatrix(phiy);
Rx = rotxmatrix(phix);
A = Rx*Ry*T;
align_verts = [align_verts, A*elm];
end
% initialising projected vertices
proj_verts=[];
% Performing the projection
for elm = align_verts
proj = projmatrix(10);
projverts = proj*elm;
projverts = ((10/projverts(3))*projverts);
proj_verts = [proj_verts,projverts];
end
% Displaying the world
for i = 1:size(world_faces,1)
for j = 1:size(world_faces,2)
x(j) =proj_verts(1,world_faces(i,j));
z(j) = proj_verts(2,world_faces(i,j));
end
patch(x,z,'w')
end
end
function T = transmatrix(p)
T = [1 0 0 p(1) ; 0 1 0 p(2) ; 0 0 1 p(3) ; 0 0 0 1];
end
function S = scalmatrix(s)
S = [s(1) 0 0 0 ; 0 s(2) 0 0 ; 0 0 s(3) 0 ; 0 0 0 1];
end
function Ry = rotymatrix(theta)
Ry = [cos(theta), 0, -sin(theta),0;
0,1,0,0;
sin(theta),0,cos(theta),0;
0,0,0,1];
end
function Rx = rotxmatrix(phi)
Rx = [1, 0, 0, 0;
0, cos(phi), -sin(phi), 0;
0, sin(phi), cos(phi), 0;
0, 0, 0, 1];
end
function P = projmatrix(f)
P = [1,0,0,0
0,1,0,0
0,0,1,0
0,0,1/f,0];
end
Updated code: managed to get the loop to work but now there is some bug i don't understand when it does a full rotation again any help would be great.
function world()
% Defining House Vertices
house_verts = [-5, 0, -5;
5, 0, -5;
5, 10, -5;
0,15,-5;
-5,10,-5;
-5,0,5;
5,0,5;
5,10,5;
0,15,5;
-5,10,5];
% Sorting out the homeogenous co-ordinates
ones = [1,1,1,1,1,1,1,1,1,1];
ones=transpose(ones);
house_verts = [house_verts, ones];
house_verts = transpose(house_verts);
% House faces
house_faces = [1,2,3,4,5;
2,7,8,3,3;
6,7,8,9,10;
1,6,10,5,5;
3,4,9,8,8;
4,5,10,9,9;
1,2,7,6,6];
world_pos = [];
% creating a street
street_vector = [1,0,1]; % the direction of the street
orthog_street_vector = [-1,0,1];
for i = 1:15
% current_pos1 and 2 will be the positions of the two houses
% opposite each other on the street
current_pos1 = 30*i*street_vector + 50*orthog_street_vector;
current_pos2 = 30*i*street_vector - 50*orthog_street_vector;
world_pos = [world_pos;current_pos1;current_pos2];
end
% initialising world vertices and faces
world_verts = [];
world_faces = [];
% Populating the street
for i =1:size(world_pos,1)
T = transmatrix(world_pos(i,:)); % a translation matrix
s = [1,1/2 + rand(),1];
S=scalmatrix(s); % a matrix for a random scaling of the height (y-coordinate)
Ry = rotymatrix(rand()*2*pi); % a matrix for a random rotation about the y-axis
A = T*Ry*S; % the compound transformation matrix to take the house into the world
obj_faces = size(world_verts,2) + house_faces; %increments the basic house faces to match the current object
obj_verts = A*house_verts;
world_verts = [world_verts, obj_verts]; % adds the vertices to the world
world_faces = [world_faces; obj_faces]; % adds the faces to the world
end
% initialising aligned vertices
align_verts = [];
% initialising projected vertices
proj_verts=[];
% Aligning the vertices to the particular camera at angle theta
theta = 0;
t = 0;
while t < 100
proj_verts=[];
align_verts = [];
for elm = world_verts
x = 300 + 300*cos(theta);
z = 300 + 300*sin(theta);
y = 80;
u = [x,y,z];
v = [200,0,200];
d = v - u;
phiy = atan2(d(1),d(3));
phix = -atan2(d(2),sqrt(d(1)^2+d(3)^2));
T = transmatrix([-u(1),-u(2),-u(3)]);
Ry = rotymatrix(phiy);
Rx = rotxmatrix(phix);
A = Rx*Ry*T;
align_verts = [align_verts, A*elm];
end
% Performing the projection
for elm = align_verts
proj = projmatrix(6);
projverts = proj*elm;
projverts = ((6/projverts(3))*projverts);
proj_verts = [proj_verts,projverts];
end
% Displaying the world
for i = 1:size(world_faces,1)
for j = 1:size(world_faces,2)
x(j) = proj_verts(1,world_faces(i,j));
z(j) = proj_verts(2,world_faces(i,j));
end
patch(x,z,'w')
pbaspect([1,1,1]); % adjusts the aspect ratio of the figure
end
title('Projected Space', 'fontsize', 16, 'interpreter', 'latex')
xlabel('$x$', 'fontsize', 16, 'interpreter', 'latex')
ylabel('$z$', 'fontsize', 16, 'interpreter', 'latex')
zlabel('$y$', 'fontsize', 16, 'interpreter', 'latex')
axis([-5,5,-5,2,0,5]) % sets the axes limits
view(0,89)
pause(0.0000001)
theta = theta + 0.01;
clf
end
end
function T = transmatrix(p)
T = [1 0 0 p(1) ; 0 1 0 p(2) ; 0 0 1 p(3) ; 0 0 0 1];
end
function S = scalmatrix(s)
S = [s(1) 0 0 0 ; 0 s(2) 0 0 ; 0 0 s(3) 0 ; 0 0 0 1];
end
function Ry = rotymatrix(theta)
Ry = [cos(theta), 0, -sin(theta),0;
0,1,0,0;
sin(theta),0,cos(theta),0;
0,0,0,1];
end
function Rx = rotxmatrix(phi)
Rx = [1, 0, 0, 0;
0, cos(phi), -sin(phi), 0;
0, sin(phi), cos(phi), 0;
0, 0, 0, 1];
end
function P = projmatrix(f)
P = [1,0,0,0
0,1,0,0
0,0,1,0
0,0,1/f,0];
end

Rotating a gaussian function - matlab

Good day,
I have code that stretches and rotates a gaussian 2d pdf as such:
mu = [0 0];
Sigma = [1 0; 0 1];
Scale = [3 0; 0 1];
Theta = 10;
Rotate = [cosd(Theta) -sind(Theta); sind(Theta) cosd(Theta)];
Sigma = (Sigma*Scale)*Rotate
x1 = -100:1:100; x2 = -100:1:100;
[X1,X2] = meshgrid(x1,x2);
F = mvnpdf([X1(:) X2(:)],mu,Sigma);
F = reshape(F,length(x2),length(x1));
imshow(F*255)
Unfortunately, when I change Theta to a value other than 0, it says
SIGMA must be a square, symmetric, positive definite matrix. Can I know what's going on
If you consult the article on Wikipedia about the general elliptical version of the Gaussian 2D PDF, it doesn't look like you're rotating it properly. In general, the equation is:
Source: Wikipedia
where:
Usually, A = 1 and we'll adopt that here. The angle theta will rotate the PDF counter-clockwise, and so we can use this raw form of the equation over mvnpdf. Using your definitions and constants, it would become this:
x1 = -100:1:100; x2 = -100:1:100;
[X1,X2] = meshgrid(X1, X2);
sigma1 = 1;
sigma2 = 1;
scale1 = 3;
scale2 = 1;
sigma1 = scale1*sigma1;
sigma2 = scale2*sigma2;
Theta = 10;
a = ((cosd(Theta)^2) / (2*sigma1^2)) + ((sind(Theta)^2) / (2*sigma2^2));
b = -((sind(2*Theta)) / (4*sigma1^2)) + ((sind(2*Theta)) / (4*sigma2^2));
c = ((sind(Theta)^2) / (2*sigma1^2)) + ((cosd(Theta)^2) / (2*sigma2^2));
mu = [0 0];
A = 1;
F = A*exp(-(a*(X1 - mu(1)).^2 + 2*b*(X1 - mu(1)).*(X2 - mu(2)) + c*(X2 - mu(2)).^2));
imshow(F*255);
I see that you are trying to rotate the variances in 2D... You have to hit your sigma matrix with your transform on both sides of it since the "sigma" matrix is essentially a tensor.
mu = [0 0];
Sigma = [1 0; 0 1];
Scale = [10 0;0 1];
Theta = pi/3;
m = makehgtform('zrotate',Theta);
m = m(1:2,1:2);
Sigma = m*(Sigma*Scale)*m.';
x1 = -100:1:100;
x2 = -100:1:100;
[X1,X2] = meshgrid(x1,x2);
F = mvnpdf([X1(:) X2(:)],mu,Sigma);
F = reshape(F,length(x2),length(x1));
imshow(F*255)

Matlab define several step functions

I would like to define a Matlab function like the one shown in the figure below, but repeating regularly along the t axis.
So far I tried two different codes:
function Borne = borne(p)
pxt = x;
Borne = zeros(size(pxt));
i0 = (pxt <= 0.1);
i1 = (pxt > 0.1 & pxt < 0.3);
i2 = (pxt > 0.3 & pxt < 0.5);
i3 = (pxt > 0.5 & pxt < 0.7);
i4 = (pxt > 0.7 & pxt < 0.9);
i5 = (pxt > 0.9 & pxt < 1.1);
Borne(i0) = 3;
Borne(i1) = -1;
Borne(i2) = 3;
Borne(i3) = -1;
Borne(i4) = 3;
Borne(i5) = -1;
This one works, but I might be obliged to go to time=100 perhaps.
function Borne = borne(p)
x=0:0.2:100;
y=ones(1,length(x));
for i=1:length(x)
if mod(i,2) == 1;
y(i)=3;
else
y(i)=-1;
end
end
Borne=stairs(x,y);
This one doesn't work at all, it gives me a constant function at 147 circa. Also, at the end of the for loop both x and y have length=1, and I don't know why.
Is there a better way to define my function, maybe? If not, how can I improve my codes?
Thank you very much!
You can do it one-shot with the remainder (rem) function and logical indexing:
%// Data
period = 1;
up_start = .1;
up_stop = .4;
up_value = 3;
down_value = -1;
x = linspace(0,10,200); %// x axis
%// Generate function
Borne = zeros(size(x)); %// initiallize
aux = rem(x,period);
ind = (aux>=up_start) & (aux<up_stop); %// index of "up" values
Borne(ind) = up_value;
Borne(~ind) = down_value;
If you want steps at 2, 4, 6 etc. you could use cumsum:
t = (1:14).*0.1;
x(2:4:12) = -4;
x(4:4:14) = 4;
x(1) = 3;
y = cumsum(x);
[t; y] =
1 2 3 4 5 6 7 8 9 10 11 12 13 14
0 1 1 0 0 1 1 0 0 1 1 0 0 1
The way this works is, you first create a vector that is +1 and -1 where you want the step to be. cumsum will take the cumulative sum of this vector, thus altering between 1 and 0.
If you want, you can plot this using stairs.
Update
With your values, this will be:
n = 8; % Don't know the length of t
t = (0:n).*0.1;
x = zeros(1, length(t));
x(2:4:length(t)) = -4;
x(4:4:length(t)) = 4;
x(1) = 3;
y = cumsum(x);
[t; y] =
0.00000 0.10000 0.20000 0.30000 0.40000 0.50000 0.60000 0.70000
3.00000 -1.00000 -1.00000 3.00000 3.00000 -1.00000 -1.00000 3.00000
No complicated code is needed for something like this. You can use the square function, which is part of the Signal Processing toolbox that comes with most distributions of Matlab:
miny = -1; % Minimum amplitude
maxy = 3; % Maximum amplitude
period = 0.4; % Period in Hz, 1/frequency
duty = 0.5; % Duty cycle, percentage of time spent at maxy
offset = 0.1; % Phase offset in sec.
t = 0:0.01:3;
y = 0.5*(maxy-miny)*square(2*pi*(t-offset)/period,duty*100)-miny;
figure;
plot(t,y)
axis([t(1) t(end) miny-0.1*(maxy-miny) maxy+0.1*(maxy-miny)])

Kalman Filter in Matlab

I have a video and I have to locate the position of a ball using the Kalman equations. Suppose the initial position of the ball in the first frame (xi,yi) is known.
Please guide me what would be the current position (currx,curry); as in the code below in subsequent frames.
What I have so far:
R = [0.2845 0.0045;0.0045 0.0455];
Hi = [1 0 0 0; 0 1 0 0];
Q = 0.01*eye(4);
Pe = 100*eye(4);
F = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
x = zeros(569,4);
kfinite = false;
for k = 1:nframes
% Kalman initialization
if ~kfinite
Au = [xi yi 0 0].';
currx = xi;
curry = yi;
% Prediction 1st equation
else
u = [x1 y1 0 0].';
Au = F*x(k-1,:).' + u;
currx = xi;
curry = yi;
end
vx = currx-Au(1);
vy = curry-Au(2);
xactual = [currx curry vx vy].';
xactualnext = F*xactual;
ydesirednext = Hi*xactualnext;
% Prediction 2nd equation
pp = F*Pe*F.' + Q;
% correction 3rd equation
K = pp*Hi.' / (Hi*pp*Hi.' + R);
% correction 4th equation
Anu = Au + K*(ydesirednext - Hi*Au);
% correction 5th equation
Pe = (eye(4) - K*Hi)*pp;
x1 = Anu(1);
x2 = Anu(2);
kfinite = true;
end