How to solve a System of 8 ODEs in Matlab where for 4 ODEs initial value is at height z=0 and for rest initial value is at z = H - matlab

I have a reactor in which solid is charged from top and gas is charged from the bottom.
I have provided the initial values of them (i.e I know the initial value of solid entities at the top and initial values of gases at the bottom). To solve the heat and mass balance equations I wrote 8 ODEs.
But how can I solve them simultaneously?
Some ODEs initial value is at z=0, and for some other initial value z it is at z = H; where H is the height of the furnace.
function f = odefun( Z,Y )
% FeO + CO -> Fe + CO2 ---(1)
% C + 1/2 O2 -> CO ---(2)
% C + O2 -> CO2 ---(3)
% C + CO2 -> 2CO ---(4)
Fg = Y(1); %volumetic flow rate of gas Nm3s-1 //initialize
Cfeo = Y(2); % initial FeO concentration mol/m3
Cc = Y(3); % initial C concentration mol/m3
yco = Y(4); % mole fraction of CO initially
yco2 = Y(5); % mole fraction of CO2 initially
yo2 = Y(6); % mole fraction of O2 initially
Tg = Y(7); % temp of gas K
Ts = Y(8); % temp of solid K
Fs = 0.044; %Mass flow rate of solid m3/s //initialize and constant
P = 5; %total pressure atm
R = 8.314; %gas constant
M = 104; %molecular mass //constant
keqm1 = 3.33; %equilibrium constant for rx 1
keqm2 = 3 * 10^10; %equilibrium constant for rx 2
keqm3 = 4.75 * 10^20; %equilibrium constant for rx 3
Az = 4; %area of the bed m2 //constant
e = 0.4; %voidage //constant
O = 0.6; %Avg shape factor of solid particle //constant
dp = 0.02; %diameter of particles // constant m
Cpg = 293000; %specific heat capacity of gas (JKg-1K-1) //constant
Cps = 440000; %specific heat capacity of solid (JKg-1K-1) //assume constant
mu = 0.005; %viscosity of gas Kg m-1 s-1 //assume constant
a = 10; %surfce area of DRI per unit volume of bed
Ac = 20; % surfce area of coal per unit volume
hgs = 200; %heat transfer coefficient b/w solid and gas J m-2 s-1 K-1
H1 = -16000; %enthalpy of reaction 1 KJ mol-1
H2 = -110520; %enthalpy of reaction 2 KJ mol-1
H3 = -393509; %enthalpy of reaction 3 KJ mol-1
H4 = -282989; %enthalpy of reaction 4 KJ mol-1
C0 = 28/22.4; % Kg/m3
C1 = 44/22.4;
C2 = 32/22.4;
Dg = (C0 * yco + C1 * yco2 + C2 * yo2); %Density of Gas Kg m-3
Db = (1-e)*(Cfeo*72 + (Y(2) - Cfeo)*56 + Cc*12 )*1000 ; %Density of Bed Kg m-3
Re = (Dg * Fg * e * dp)/(mu*Az) ; %Reynolds number
f1 = (150 * (1-e)^2 * mu)/(e^2 * Dg * dp^2); %friction factor parameter
f2 = (1.75*(1-e))/(e^3 * Dg * dp);
dFgdZ = -(Fg/(P * Az^2))*(f1*Az*Fg + f2 * Fg^2); %volumetric flow rate of gas Nm3 s-1
kp1 = 2.5*exp(-8857.6/Ts); % chemical rxn rate constant s-1
kp2 = 6.52*10^(5) * exp(-22000/Ts) * Ts^(0.5);
kp3 = kp2;
kp4 = 4*10^12 * exp(-40000/Ts);
Vg = yco + yco2; %mass fraction of volatile evolve
kf = 2.0*Re^(-0.336)* Vg; %mass transfer coefficient m/s
Po2 = yo2; % partial pressure atm
Pco = yco;
Pco2 = yco2;
Pco2eqm = 0.2; % Change
Pre2 = (Po2 - (Pco^(2)/keqm2^(2))); % driving force atm
Pre3 = (Po2 - (Pco2^(2)/keqm3^(2)));
Pre4 = (Pco2 - Pco2eqm);
R1 = a*(Fs/Y(1))^(1/3)*kp1*(yco-yco2/keqm1)*P/(R*Ts); %reaction rate constant m-3s-1
R2 = (Ac/((6/(dp*kp2))+(1/kf))) * (9.869*10^(-6)*Pre2)/(M*R*Ts);
R3 = (Ac/((6/(dp*kp3))+(1/kf))) * (9.869*10^(-6)*Pre3)/(M*R*Ts);
R4 = (Ac/((6/(dp*kp4))+(1/kf))) * (9.869*10^(-6)*Pre4)/(M*R*Ts);
dCfeodZ = (R1*Az*(1-e))/Fs; %for FeO Cfeo Kg/s
dCcdZ = ((R2+R3+R4)*Az*(1-e))/Fs; %for C
dycodZ = 22.4*Az*(R2*(yco/2 - 1)+(yco - 2)+R1)/Fg; %for CO
dyco2dZ = 22.4*Az*(yco2*(R2/2 + R4)+R4-R1-R3)/Fg; %for CO2
dyo2dZ = 22.4*Az*(R2/2 + R3 + yo2*(R2/2 + R4))/Fg; %for O2
%Heat transfer equations
S = (6*(1-e)*Az)/(dp*O) ; %surface area of the solid
B = (6*(1-e)*hgs)/(dp*O);
A = 16*R1+12*(R2+R3+R4);
E = H1*R1+ H2*R2+ H3*R3+ H4*R4;
dTgdZ = (22.4/(Fg*Cpg))*(Az*Cpg*Tg*(R2/2 + R4)+ S*(Tg - Ts)*hgs );
dTsdZ = (Az*(1-e)/(Fs*Cps*Db))*(A*Cps*Ts + B*(Tg - Ts) - E);
f = [dFgdZ;dCfeodZ;dCcdZ;dycodZ;dyco2dZ;dyo2dZ;dTgdZ;dTsdZ];
end
These is the function with ODES and the initial values of Fg(z=0), Cfeo(z=H),Cc(z=H) yco(z=0),yco2(z=0),yo2(z=0),Tg(z=0),Ts(z=H) are provided kindly help me out to solve this problem.

Related

What type of (probably syntactic) mistake am I making when using a generic function on an array for plotting?

I am trying to plot a geodesic on a 3D surface (tractrix) with Matlab. This worked for me in the past when I didn't need to parametrize the surface (see here). However, the tractrix called for parameterization, chain rule differentiation, and collection of u,v,x,y and f(x,y) values.
After many mistakes I think that I'm getting the right values for x = f1(u,v) and y = f2(u,v) describing a spiral right at the base of the surface:
What I can't understand is why the z value or height of the 3D plot of the curve is consistently zero, when I'm applying the same mathematical formula that allowed me to plot the surface in the first place, ie. f = #(x,y) a.* (y - tanh(y)) .
Here is the code, which runs without any errors on Octave. I'm typing a special note in upper case on the crucial calls. Also note that I have restricted the number of geodesic lines to 1 to decrease the execution time.
a = 0.3;
u = 0:0.1:(2 * pi);
v = 0:0.1:5;
[X,Y] = meshgrid(u,v);
% NOTE THAT THESE FORMULAS RESULT IN A SUCCESSFUL PLOT OF THE SURFACE:
x = a.* cos(X) ./ cosh(Y);
y = a.* sin(X) ./ cosh(Y);
z = a.* (Y - tanh(Y));
h = surf(x,y,z);
zlim([0, 1.2]);
set(h,'edgecolor','none')
colormap summer
hold on
% THESE ARE THE GENERIC FUNCTIONS (f) WHICH DON'T SEEM TO WORK AT THE END:
f = #(x,y) a.* (y - tanh(y));
f1 = #(u,v) a.* cos(u) ./ cosh(v);
f2 = #(u,v) a.* sin(u) ./ cosh(v);
dfdu = #(u,v) ((f(f1(u,v)+eps, f2(u,v)) - f(f1(u,v) - eps, f2(u,v)))/(2 * eps) .*
(f1(u+eps,v)-f1(u-eps,v))/(2*eps) +
(f(f1(u,v), f2(u,v)+eps) - f(f1(u,v), f2(u,v)-eps))/(2 * eps) .*
(f2(u+eps,v)-f2(u-eps,v))/(2*eps));
dfdv = #(u,v) ((f(f1(u,v)+eps, f2(u,v)) - f(f1(u,v) - eps, f2(u,v)))/(2 * eps) .*
(f1(u,v+eps)-f1(u,v-eps))/(2*eps) +
(f(f1(u,v), f2(u,v)+eps) - f(f1(u,v), f2(u,v)-eps))/(2 * eps) .*
(f2(u,v+eps)-f2(u,v-eps))/(2*eps));
% Normal vector to the surface:
N = #(u,v) [- dfdu(u,v), - dfdv(u,v), 1]; % Normal vec to surface # any pt.
% Some colors to draw the lines:
C = {'k','r','g','y','m','c'};
for s = 1:1 % No. of lines to be plotted.
% Starting points:
u0 = [0, u(length(u))];
v0 = [0, v(length(v))];
du0 = 0.001;
dv0 = 0.001;
step_size = 0.00005; % Will determine the progression rate from pt to pt.
eta = step_size / sqrt(du0^2 + dv0^2); % Normalization.
eps = 0.0001; % Epsilon
max_num_iter = 100000; % Number of dots in each line.
% Semi-empty vectors to collect results:
U = [[u0(s), u0(s) + eta*du0], zeros(1,max_num_iter - 2)];
V = [[v0(s), v0(s) + eta*dv0], zeros(1,max_num_iter - 2)];
for i = 2:(max_num_iter - 1) % Creating the geodesic:
ut = U(i);
vt = V(i);
xt = f1(ut,vt);
yt = f2(ut,vt);
ft = f(xt,yt);
utm1 = U(i - 1);
vtm1 = V(i - 1);
xtm1 = f1(utm1,vtm1);
ytm1 = f2(utm1,vtm1);
ftm1 = f(xtm1,ytm1);
usymp = ut + (ut - utm1);
vsymp = vt + (vt - vtm1);
xsymp = f1(usymp,vsymp);
ysymp = f2(usymp,vsymp);
fsymp = ft + (ft - ftm1);
df = fsymp - f(xsymp,ysymp); % Is the surface changing? How much?
n = N(ut,vt); % Normal vector at point t
gamma = df * n(3); % Scalar x change f x z value of N
xtp1 = xsymp - gamma * n(1); % Gamma to modulate incre. x & y.
ytp1 = ysymp - gamma * n(2);
U(i + 1) = usymp - gamma * n(1);;
V(i + 1) = vsymp - gamma * n(2);;
end
% THE PROBLEM! f(f1(U,V),f2(U,V)) below YIELDS ALL ZEROS!!! The expected values are between 0 and 1.2.
P = [f1(U,V); f2(U,V); f(f1(U,V),f2(U,V))]; % Compiling results into a matrix.
units = 35; % Determines speed (smaller, faster)
packet = floor(size(P,2)/units);
P = P(:,1: packet * units);
for k = 1:packet:(packet * units)
hold on
plot3(P(1, k:(k+packet-1)), P(2,(k:(k+packet-1))), P(3,(k:(k+packet-1))),
'.', 'MarkerSize', 5,'color',C{s})
drawnow
end
end
The answer is to Cris Luengo's credit, who noticed that the upper-case assigned to the variable Y, used for the calculation of the height of the curve z, was indeed in the parametrization space u,v as intended, and not in the manifold x,y! I don't use Matlab/Octave other than for occasional simulations, and I was trying every other syntactical permutation I could think of without realizing that f fed directly from v (as intended). I changed now the names of the different variables to make it cleaner.
Here is the revised code:
a = 0.3;
u = 0:0.1:(3 * pi);
v = 0:0.1:5;
[U,V] = meshgrid(u,v);
x = a.* cos(U) ./ cosh(V);
y = a.* sin(U) ./ cosh(V);
z = a.* (V - tanh(V));
h = surf(x,y,z);
zlim([0, 1.2]);
set(h,'edgecolor','none')
colormap summer
hold on
f = #(x,y) a.* (y - tanh(y));
f1 = #(u,v) a.* cos(u) ./ cosh(v);
f2 = #(u,v) a.* sin(u) ./ cosh(v);
dfdu = #(u,v) ((f(f1(u,v)+eps, f2(u,v)) - f(f1(u,v) - eps, f2(u,v)))/(2 * eps) .*
(f1(u+eps,v)-f1(u-eps,v))/(2*eps) +
(f(f1(u,v), f2(u,v)+eps) - f(f1(u,v), f2(u,v)-eps))/(2 * eps) .*
(f2(u+eps,v)-f2(u-eps,v))/(2*eps));
dfdv = #(u,v) ((f(f1(u,v)+eps, f2(u,v)) - f(f1(u,v) - eps, f2(u,v)))/(2 * eps) .*
(f1(u,v+eps)-f1(u,v-eps))/(2*eps) +
(f(f1(u,v), f2(u,v)+eps) - f(f1(u,v), f2(u,v)-eps))/(2 * eps) .*
(f2(u,v+eps)-f2(u,v-eps))/(2*eps));
% Normal vector to the surface:
N = #(u,v) [- dfdu(u,v), - dfdv(u,v), 1]; % Normal vec to surface # any pt.
% Some colors to draw the lines:
C = {'y','r','k','m','w',[0.8 0.8 1]}; % Color scheme
for s = 1:6 % No. of lines to be plotted.
% Starting points:
u0 = [0, -pi/2, 2*pi, 4*pi/3, pi/4, pi];
v0 = [0, 0, 0, 0, 0, 0];
du0 = [0, -0.0001, 0.001, - 0.001, 0.001, -0.01];
dv0 = [0.1, 0.01, 0.001, 0.001, 0.0005, 0.01];
step_size = 0.00005; % Will determine the progression rate from pt to pt.
eta = step_size / sqrt(du0(s)^2 + dv0(s)^2); % Normalization.
eps = 0.0001; % Epsilon
max_num_iter = 180000; % Number of dots in each line.
% Semi-empty vectors to collect results:
Uc = [[u0(s), u0(s) + eta*du0(s)], zeros(1,max_num_iter - 2)];
Vc = [[v0(s), v0(s) + eta*dv0(s)], zeros(1,max_num_iter - 2)];
for i = 2:(max_num_iter - 1) % Creating the geodesic:
ut = Uc(i);
vt = Vc(i);
xt = f1(ut,vt);
yt = f2(ut,vt);
ft = f(xt,yt);
utm1 = Uc(i - 1);
vtm1 = Vc(i - 1);
xtm1 = f1(utm1,vtm1);
ytm1 = f2(utm1,vtm1);
ftm1 = f(xtm1,ytm1);
usymp = ut + (ut - utm1);
vsymp = vt + (vt - vtm1);
xsymp = f1(usymp,vsymp);
ysymp = f2(usymp,vsymp);
fsymp = ft + (ft - ftm1);
df = fsymp - f(xsymp,ysymp); % Is the surface changing? How much?
n = N(ut,vt); % Normal vector at point t
gamma = df * n(3); % Scalar x change f x z value of N
xtp1 = xsymp - gamma * n(1); % Gamma to modulate incre. x & y.
ytp1 = ysymp - gamma * n(2);
Uc(i + 1) = usymp - gamma * n(1);;
Vc(i + 1) = vsymp - gamma * n(2);;
end
x = f1(Uc,Vc);
y = f2(Uc,Vc);
P = [x; y; f(Uc,Vc)]; % Compiling results into a matrix.
units = 35; % Determines speed (smaller, faster)
packet = floor(size(P,2)/units);
P = P(:,1: packet * units);
for k = 1:packet:(packet * units)
hold on
plot3(P(1, k:(k+packet-1)), P(2,(k:(k+packet-1))), P(3,(k:(k+packet-1))),
'.', 'MarkerSize', 5,'color',C{s})
drawnow
end
end

Error state kalman filter estimates wrong values

I've implemented this error state kalman filter, which has IMU data as input (220Hz) and it corrects the prediction with UWB measurements (50 Hz).
I want to estimate the pose of a quadrotor.
The code is:
function [x_hat,bound_p] = ESKF(d_meas,p_am,clockUWB,dt,u)
% u = IMU inputs
% d_meas = UWB measure
% p_am = anchor coordinate. There are 4 anchors that send the measurement one at a time cyclically
g = [0 0 9.81]';
am = u(1:3); % from acc
wm = u(4:6); % from gyro
persistent P Q R I dx_hat x_n p q v ba bw Fw
if isempty(P)
sig_an = 0.05; % [m/s^2]
sig_wn = 0.01; % [rad/s]
sig_wbn = 0.0001; % [rad/s*sqrt(s)]
sig_abn = 0.0001; % [m/(s^2*sqrt(s)]
sig_uwb = 0.0214; % [m]
Q_an = sig_an*sig_an*eye(3); % [m^2/s^2]
Q_wn = sig_wn*sig_wn*eye(3); % [rad^2]
Q_abn = sig_abn*sig_abn*eye(3); % [m^2/s^4]
Q_wbn = sig_wbn*sig_wbn*eye(3); % [rad^2/s^2]
clockUWB = 0;
dx_hat = zeros(15,1); % error state
x_n = [5 4 5 1 0 0 1 0 0 0 0 0 0 0 0 0]'; % initial state
P = eye(15,15);
Fw = [zeros(3,12);eye(12,12)];
Q = blkdiag(Q_an,Q_wn,Q_wbn,Q_abn);
R = sig_uwb*sig_uwb;
I = eye(length(dx_hat));
p = x_n(1:3);
v = x_n(4:6);
q = x_n(7:10);
bw = x_n(11:13);
ba = x_n(14:16);
end
%% NOMINAL STATE
if ((wm - bw) == [0 0 0]')
qw = [1 0 0 0]';
else
nw = norm(wm - bw);
qw = [cos(nw*dt/2); ((wm - bw)/nw)*sin(nw*dt/2)];
end
R_ui = quat2rotm(q');
% PREDICTION
p = p + v*dt + 1/2*(R_ui*(am - ba) + g)*dt*dt;
v = v + (R_ui*(am - ba) + g)*dt;
q = quatmultiply(q',qw')';
q = q/norm(q);
% bw = bw;
% ba = ba;
%% ERROR STATE
% delta_p = delta_p + delta_v*dt;
% delta_v = delta_v + (-R_ui*skew(am - ba)*delta_th - R_ui*delta_ba)*dt + an);
% delta_th = R_ui'*delta_th - delta_bw*dt + wn;
% delta_bw = delta_bw + wbn*ones(3,1);
% delta_ba = delta_ba + abn*ones(3,1);
F = [ eye(3), eye(3)*dt, zeros(3,3), zeros(3,3), zeros(3,3);
zeros(3,3), eye(3), -R_ui*skew(am-ba)*dt, zeros(3,3), -R_ui*dt;
zeros(3,3), zeros(3,3), R_ui', -eye(3)*dt, zeros(3,3);
zeros(3,3), zeros(3,3), zeros(3,3), eye(3), zeros(3,3);
zeros(3,3), zeros(3,3), zeros(3,3), zeros(3,3), eye(3)];
%PREDCTION
dx_hat = F*dx_hat;
P = F * P * F' + Fw * Q * Fw';
%% UPDATE
% Only when measures arrive
if (clockUWB == 1)
h = p - p_am;
d_am = norm(h);
H = [ (1/(2*d_am))*2*h'*eye(3), zeros(1,3), zeros(1,3), zeros(1,3), zeros(1,3)];
K = P * H' * inv(H * P * H' + R);
dx_hat = K * (d_meas - d_am);
delta_x = [dx_hat(1:6);[1,1/2*dx_hat(7:9)']';dx_hat(10:15)];
P = (I - K * H) * P * (I - K * H)' + K * R * K';
%% ERROR INJECTION
p = p + delta_x(1:3);
v = v + delta_x(4:6);
q = quatmultiply(q',delta_x(7:10)')';
q = q/norm(q);
bw = bw + delta_x(11:13);
ba = ba + delta_x(14:16);
sig_p = 3*[sqrt(P(1,1));sqrt(P(2,2));sqrt(P(2,2))];
bound_p = [p(1) + sig_p(1),p(1) - sig_p(1),p(2) + sig_p(2),p(2) - sig_p(2),p(3) + sig_p(3),p(3) - sig_p(3)];
x_hat = [p;v;q;bw;ba];
yaw_err = abs(dx_hat(9));
yaw_err_old = yaw_err;
%% RESET ERROR
G = blkdiag(eye(6),eye(3) - skew(1/2*dx_hat(7:9)),eye(6));
delta_x = zeros(16,1);
dx_hat = zeros(15,1);
P = G * P * G';
else
x_hat = [p;v;q;bw;ba];
delta_x = zeros(16,1);
sig_p = 3*[sqrt(P(1,1));sqrt(P(2,2));sqrt(P(2,2))];
bound_p = [p(1) + sig_p(1),p(1) - sig_p(1),p(2) + sig_p(2),p(2) - sig_p(2),p(3) + sig_p(3),p(3) - sig_p(3)];
end
end
It estimates position perfectly, and also velocity is nice.
The problem is that it estimates roll, pitch and yaw (which I derive from the quaternion thanks to quat2eul function) very badly and some bias are totally wrong.
Can someone tell me where the code is wrong?
Thanks
This is the main simulink model
In the green block there is the function of the filter.
In order to simulate a trajectory and UWB measurements in the UWB block there is this script:
function [xt,yt,zt,d_meas,p_am] = fcn(t,clock)
sig_uwb = 0.0214;
dn = normrnd(0,sig_uwb);
persistent k i
if isempty(k)
k = 0;
clock = 0;
i = 0;
end
x = 3*cos(1/3*t) + 3;
y = 3*sin(1/3*t) + 3;
z = 5;
a1 = [-4.12,-3.67,2.72]; % anchors coordinates
a2 = [2.45,-2.70,0.063];
a3 = [-2.43,3.07,0.075];
a4 = [3.65,2.42,2.62];
d1 = norm(a1 - [x,y,z]);
d2 = norm(a2 - [x,y,z]);
d3 = norm(a3 - [x,y,z]);
d4 = norm(a4 - [x,y,z]);
A = [a1,d1;a2,d2;a3,d3;a4,d4];
if (clock == 1)
k = k + 1;
i = mod(k,4) + 1;
d_meas = A(i,4) + dn;
p_am = A(i,(1:3))';
else
d_meas = 0;
p_am = zeros(3,1);
end
xt = x;
yt = y;
zt = z;
So the drone simulates a circular trajectory with radius equals to 3.
The IMU block contains just 2 vector:
am = [0 -1/3 -9.81] and wm = [0 0 1/3].
The bias should be constant and 0. I obtain instead values like 3 or 2 and non constant.
Roll and pitch aren't 0 as they should.
The text I am using to implement the ESKF are Quaternion kinematics for the error-state KF from pag. 52.

Matlab 3d projectile secant method with vector

How can I use the secant method in order to calculate the angle b(between the positive x-axis and negative y-axis) I have to throw a ball for it to land on the x-axis, with these conditions:
the ball is thrown from origo, at a height z=1.4 m, with the and angle a (for the horizontal plane) 30◦, with a speed of 25 m / s. The air resistance coefficient is c = 0.070 and the wind force is 7 m / s at the ground which increases with the height according to: a(z) = 7 + 0.35z.
The motion of the ball is described with the following:
x''=−qx', y''=−q(y'−a(z)), z''=−9.81−qz', q=c*sqrt(x'^2+(y'−a(z))^2+z'^2)
I did a variable substitution (u) and then used RK4 to calculate the ball's motion, however I can't figure out how to use the secant method to find the angle b. The problem is that when I plot with these start and guessing values is that the ball doesn't land on the x-axis:
clear all, close all, clc
a = pi/3; %start angle
c = 0.07; % air resistance coeff.
v0 = 25; %start velocity, 25 m/s
t = 0; %start time
h = 0.1; % 0.1 second step
b = 0
x0 = 0; xPrim = v0*sin(a)*cosd(b);
y0 = 0; yPrim = v0*sin(a)*sind(b);
z0 = 1.4; zPrim = v0*cos(a);
u = [x0 xPrim y0 yPrim z0 zPrim]';
uVek = u';
% Secant method
b0 = 270; % Start guess nr 1
b1 = 360; % Start guess nr 2
f0 = funk (b0);
db = 1;
while abs(db) > 0.5e-8
f1 = funk (b1);
db = f1 * (b1 - b0) / (f1 - f0);
b0 = b1; % Updates b0
f0 = f1; % Updates f0
b1 = b1 - db % new b
end
while u(5) >= 0 && u(3)<=0
f1 = FRK4(t,u);
f2 = FRK4(t+h/2,u+(h/2)*f1);
f3 = FRK4(t+h/2,u+(h/2)*f2);
f4 = FRK4(t+h,u+h*f3);
f = (f1 + 2*f2 + 2*f3 + f4)/6;
u = u + h*f;
uVek = [uVek; u'];
t = t+h;
end
x = uVek(:,1)'; y = uVek(:,3)'; z = uVek(:,5)';
plot3(x,y,z)
xlabel('X');
ylabel('Y');
zlabel('Z');
grid on
where FRK4 is a function:
function uPrim = FRK4(t,u)
c = 0.07;
az = 7+0.35*(u(5));
q = c*sqrt(u(2)^2 +(u(4)- az)^2 + u(6)^2);
uPrim = [u(2) -q*u(2) u(4) -q*(u(4) - az) u(6) -9.81-q*u(6)]';
end
where funk is a function:
function f = funk(b)
a = pi/3
v0 = 25
x0 = 0; xPrim = v0*sin(a)*cosd(b);
y0 = 0; yPrim = v0*sin(a)*sind(b);
z0 = 1.4; zPrim = v0*cos(a);
f = [x0 xPrim y0 yPrim z0 zPrim]';
end

Active contours with open ends

Does anyone know how to make an open active contour? I'm familiar with closed contours and I have several Matlab programs describing them. I've tried to change the matrix P but that was not enough: My Matlab code is as follows:
% Read in the test image
GrayLevelClump=imread('cortex.png');
cortex_grad=imread('cortex_grad.png');
rect=[120 32 340 340];
img=imcrop(GrayLevelClump,rect);
grad_mag=imcrop(cortex_grad(:,:,3),rect);
% Draw the initial snake as a line
a=300;b=21;c=21;d=307;
snake = brlinexya(a,b,c,d); % startpoints,endpoints
x0=snake(:,1);
y0=snake(:,2);
N=length(x0);
x0=x0(1:N-1)';
y0=y0(1:N-1)';
% parameters for the active contour
alpha = 5;
beta = 10;
gamma = 1;
kappa=1;
iterations = 50;
% Make a force field
[u,v] = GVF(-grad_mag, 0.2, 80);
%disp(' Normalizing the GVF external force ...');
mag = sqrt(u.*u+v.*v);
px = u./(mag+(1e-10)); py = v./(mag+(1e-10));
% Make the coefficient matrix P
x=x0;
y=y0;
N = length(x0);
a = gamma*(2*alpha+6*beta)+1;
b = gamma*(-alpha-4*beta);
c = gamma*beta;
P = diag(repmat(a,1,N));
P = P + diag(repmat(b,1,N-1), 1) + diag( b, -N+1);
P = P + diag(repmat(b,1,N-1),-1) + diag( b, N-1);
P = P + diag(repmat(c,1,N-2), 2) + diag([c,c],-N+2);
P = P + diag(repmat(c,1,N-2),-2) + diag([c,c], N-2);
P = inv(P);
d = gamma * (-alpha);
e = gamma * (2*alpha);
% Do the modifications to the matrix in order to handle OAC
P(1:2,:) = 0;
P(1,1) = -gamma;
P(2,1) = d;
P(2,2) = e;
P(2,3) = d;
P(N-1:N,:) = 0;
P(N-1,N-2) = d;
P(N-1,N-1) = e;
P(N-1,N) = d;
P(N,N) = -gamma;
x=x';
y=y';
figure,imshow(grad_mag,[]);
hold on, plot([x;x(1)],[y;y(1)],'r');
plot(x([1 end]),y(([1 end])), 'b.','MarkerSize', 20);
for ii = 1:50
% Calculate external force
vfx = interp2(px,x,y,'*linear');
vfy = interp2(py,x,y,'*linear');
tf=isnan(vfx);
ind=find(tf==1);
if ~isempty(ind)
vfx(ind)=0;
end
tf=isnan(vfy);
ind=find(tf==1);
if ~isempty(ind)
vfy(ind)=0;
end
% Move control points
x = P*(x+gamma*vfx);
y = P*(y+gamma*vfy);
%x = P * (gamma* x + gamma*vfx);
%y = P * (gamma* y + gamma*vfy);
if mod(ii,5)==0
plot([x;x(1)],[y;y(1)],'b')
end
end
plot([x;x(1)],[y;y(1)],'r')
I've modified the coefficient matrix P in order to handle cases with open ended active contours, but that is clearly not enough.
My image:

Implementing a Kalman Filter in MATLAB using 'ss'

I am trying to implement a Kalman Filter for estimating the state 'x' (displacement and velocity) of an oscillator. The code is below and should be simple to follow.
clear; clc; close all;
% io = csvread('sim.csv');
% u = io(:, 1);
% y = io(:, 2);
% clear io;
% Estimation of state of a single degree-of-freedom oscillator using
% the Kalman filter
% x[n + 1] = A x[n] + B u[n] + w[n]
% y[n] = C x[n] + D u[n] + v[n]
% Here, x[n] is 2 x 1, u[n] is 1 x 1
% A is 2 x 2, B is 2 x 1, C is 1 x 2, D is 1 x 1
%% Code begins here
N = 1000;
u = randn(N, 1); % Synthetic input
y = randn(N, 1); % Synthetic output
%% Definitions
dt = 0.005; % Time step in seconds
T = 1.50; % Oscillator period
zeta = 0.05; % Damping ratio
sv0 = max(abs(u)) * dt;
sd0 = sv0 * dt;
Q = [sd0 ^ 2 0.0; 0.0 sv0 ^ 2]; % Prediction error covariance matrix
smeas = 0.001 * max(abs(u));
R = smeas ^ 2; % Measurement error (co)variance scalar
wn = 2. * pi / Ts;
c = 2.0 * zeta * wn;
k = wn ^ 2;
A = [0. 1.; -k -c];
Ad = expm(A * dt);
Bd = A \ (Ad - eye(2));
Bd = Bd(:, 2);
C = [-k -c];
D = -1.0;
%% State-space model and Kalman filter
sys = ss(Ad, Bd, C, D, dt, 'inputname', 'u', 'outputname', 'y');
[kest,L,P] = kalman(sys, Q, R, []);
Here is my problem. I get the error: 'In the "kalman(SYS,QN,RN,NN,...)" command, QN must be a real square matrix with at most 1 rows.'.
I thought that QN = Q = const and should be 2 x 2, but it is asking for a scalar. Perhaps I don't understand the difference between Q and QN in MATLAB's 'kalman' help description. Any insights?
Thanks.
MATLAB is assuming the process noise is only one stochastic variable and not two like Q is representing.
So you have to add the G and H matrices to your sys like so:
G = eye(2);
H = [0,0];
sys = ss(Ad, [Bd, G], C, [D, H], dt, 'inputname', 'u', 'outputname', 'y');
Just as a reminder, using MATLAB's syntax:
x*=Ax+Bu+Gw
y=Cx+Du+Hw+v