I have the following function to be solved by ODE45 solver of Matlab:
function f = odefun(t, y)
global mu ft
% y = [a, h, k, p, q, L, lla, lh, lk, lp, lq, lL];
a = y(1);
h = y(2);
k = y(3);
p = y(4);
q = y(5);
L = y(6);
lla = y(7);
lh = y(8);
lk = y(9);
lp = y(10);
lq = y(11);
lL = y(12);
n = sqrt(mu./a.^3); % mean motion
G = sqrt(1-h.^2-k.^2);
K = 1+p.^2+q.^2;
sL = sin(L);
cL = cos(L);
r = (a.*G.^2)./(1+h.*sL+k.*cL);
% components of B Matrix
B11 = 2.*(n.^-1).*(G.^-1).*(k.*sL-h.*cL);
B12 = 2.*(n.^-1).*a.*(r.^-1).*G;
B13 = 0;
B21 = -(n.^-1).*(a.^-1).*G.*cL;
B22 = (n.^-1).*(a^-2).*r.*(G.^-1).*(h+sL)+(n.^-1).*(a.^-1).*G.*sL;
B23 = -(n.^-1).*(a.^-2).*r.*(G.^-1).*k.*(p.*cL-q.*sL);
B31 = (n.^-1).*(a.^-1).*G.*sL;
B32 = (n.^-1).*(a.^-2).*r.*(G.^-1).*(k+cL)+(n.^-1).*(a.^-1).*G.*cL;
B33 = (n.^-1).*(a.^-2).*r.*(G.^-1).*h.*(p.*cL-q.*sL);
B41 = 0;
B42 = 0;
B43 = 0.5.*(n.^-1).*(a.^-2).*r.*(G.^-1).*K.*sL;
B51 = 0;
B52 = 0;
B53 = 0.5.*(n.^-1).*(a.^-2).*r.*(G.^-1).*K.*cL;
B61 = 0;
B62 = 0;
B63 = n.*(a.^2).*(r.^-2).*G+(n.^-1).*(a.^-2).*r.*(G.^-1).*(q.*sL-p.*cL);
B = [B11 B12 B13;
B21 B22 B23;
B31 B32 B33;
B41 B42 B43;
B51 B52 B53;
B61 B62 B63];
% costate vector
lambdaVec = [lla lh lk lp lq lL];
% control law vector
uvec = lambdaVec * B;
unorm = norm(uvec);
u = uvec./unorm;
u = u';
z_ = (B*u).*ft;
C = [0; 0; 0; 0; 0; n.*(a.^2)*(r.^-2).*G];
zdot = z_+C;
which should solve a system of 12 ODEs with 12 initial conditions, however when i run the code i get the following error:
Error using vertcat
Dimensions of arrays being concatenated are not consistent.
Error in odefun (line 49)
B = [B11 B12 B13;
which refers to matrix B. what possibly goes wrong here?
B11, B12, ... are the components of the matrix which are defined based on function variables e.g. "y" vector.
Related
I am trying to solve this problem. But I keep getting an error.
This is my First Code.
% Program 3.3
function [L, U, P] = lufact(A)
[N, N] = size(A);
X = zeros(N, 1);
Y = zeros(N, 1);
C = zeros(1, N);
R = 1:N;
for p = 1: N-1
[max1, j] = max(abs(A(p:N, p)));
C = A(p,:);
A(p,:) = A(j + p - 1,:);
A(j + p -1, :) = C;
d = R(p);
R(p) = R(j + p -1);
R(j + p - 1) = d;
if A(p,p) == 0
'A is Singular. No unique Solution'
break
end
for k = p + 1:N
mult = A(k,p)/A(p,p);
A(k,p) = mult;
A(k,p + 1:N) = A(k, p + 1:N) - mult *A(p, p + 1:N);
end
I=(1:N)'*ones(1,N,1); J=I';
L = (I>J).*A + eye(N);
U = (J>=I).*A;
P = zeros(N);
for k=1:N
P(k,R(k))=1;
end
end
X(N) = Y(N)/A(N,N);
for k = N-1: -1: 1
X(k) = (Y(k) - A(k, k+1:N)*X(k+1:N))/A(k,k);
end
And This is my 2nd Code which I'm using to solve this problem.
function B = Ques3(A)
% Computes the inverse of a matrix A
[L,U,P] = lufact(A);
N = max(size(A));
I = eye(N);
B = zeros(N);
for j = 1:N
Y = forsub(L,P*I(:,j));
B(:,j) = backsub(U,Y);
end
But I keep getting an error in MATLAB,
>> Ques3(A)
Unrecognized function or variable 'forsub'.
Error in Ques3 (line 12)
Y = forsub(L,P*I(:,j));
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.
I'm having trouble on an easy exercise about an artificial neural network with 2 features, a hidden layer of 5 neurons and two possible outputs (0 or 1).
My X matrix is a 51x2 matrix, and y is a 51x1 vector.
I know I'm not supposed to do the while E>1 but I wanted to see if eventually my error would be lower than 1
I'd like to know what I am doing wrong. My error doesn't seem to lower (around 1.5 no matter how much iterations I'm doing). Do you see in the code where I am doing a mistake? I'm supposed to use gradient descent.
function [E, v,w] = costFunction(X, y,alpha1,alpha2)
[m n] = size(X);
E = 1;
v = 2*rand(5,3)-1;
w = 2*rand(2,6)-1;
grad_v=zeros(size(v));
grad_w=zeros(size(w));
K = 2;
E = 2;
while E> 1
a1 = [ones(m,1) X];
z2 = a1 * v';
a2 = sigmoid(z2);
a2 = [ones(size(a2,1),1),a2];
z3 = a2 * w';
h = sigmoid(z3);
cost = sum((-y.*log(h)) - ((1-y).*log(1-h)),2);
E = (1/m)*sum(cost);
Delta1=0;
Delta2=0;
for t = 1:m
a1 = [1;X(t,:)'];
z2 = v * a1;
a2 = sigmoid(z2);
a2 = [1;a2];
z3 = w * a2;
a3 = sigmoid(z3);
d3 = a3 - y(t,:)';
d2 = (w(:,2:end)'*d3).*sigmoidGradient(z2);
Delta2 += (d3*a2');
Delta1 += (d2*a1');
end
grad_v = (1/m) * Delta1;
grad_w = (1/m) * Delta2;
v -= alpha1 * grad_v;
w -= alpha2 * grad_w;
end
end
%% Gram—Schmidt as Triangular Orthogonalization
clear
M = [1,1,1; 1,1,0; 1,1,9]
[m,n] = size(M);
Q = M;
Rinv = eye(n);
for i = 1:n
Ri = eye(n);
Ri(i,i) = 1 / norm(Q(:,i));
for j = i+1:n
Ri(i,j) = -Q(:,i)'*Q(:,j) / norm(Q(:,i));
end
Q = Q*Ri;
Rinv = Rinv*Ri;
end
Q
R = inv(Rinv)
The question: Q*R gives M butQ is not orthagonal.
This should return the correct result:
clear();
M = [1,1,1; 1,1,0; 1,1,9];
[m,n] = size(M);
Q = zeros(m,n);
R = zeros(n,n);
for j = 1:n
v = M(:,j);
for i = 1:j-1
R(i,j) = Q(:,i).' * M(:,j);
v = v - (R(i,j) * Q(:,i));
end
R(j,j) = norm(v);
Q(:,j) = v / R(j,j);
end
Alternatively, you could give a try to the following implementation I found on Matlab File Exchange (it contains many know variants of the algorithm): https://it.mathworks.com/matlabcentral/fileexchange/55881-gram-schmidt-orthogonalization
My following code generates a graph with a looping variable for the x-axis. Specifically, eta_22 varies from 0 to 1, with loop iteration size of 0.01.
The code below the line is the source function file.
My question is: How can I generate a graph with eta_1 varying from 0 to 1, with loop iteration size of 0.01 as well? (I want a plot of AA on the y-axis, and eta_1, eta_2 varying from 0 to 1.)
My attempts: I have tried to create nested "for" loops, but the plot itself is looping. I have tried to put the plot line outside of the "for" loops as well, but that did not work.
Thanks for any help.
global Lambda mu mu_A mu_T beta tau eta_1 eta_2 lambda_T rho_1 rho_2 gamma
alpha = 100;
TIME = 365;
eta_22 = zeros(1,alpha);
AA = zeros(1,alpha);
for m = 1:1:alpha
eta_2 = m./alpha;
eta_22(m) = m./alpha;
Lambda = 531062;
mu = (1/70)/365;
mu_A = 0.25/365;
mu_T = 0.2/365;
beta = 0.187/365;
tau = 4/365;
lambda_T = 0.1;
rho_1 = 1/60;
rho_2 = (rho_1)./(270.*rho_1-1);
gamma = 1e-3;
eta_1 = 0;
S0 = 191564208;
T0 = 131533276;
H0 = 2405659;
C0 = 1805024;
C10 = 1000000;
C20 = 1000000;
CT10 = 500000;
CT20 = 500000;
y0 = [S0, T0, H0, C0, C10, C20, CT10, CT20];
[t,y] = ode45('SimplifiedEqns',[0:1:TIME],y0);
S = y(:,1);
T = y(:,2);
H = y(:,3);
C = y(:,4);
C1 = y(:,5);
C2 = y(:,6);
CT1 = y(:,7);
CT2 = y(:,8);
N = S + T + H + C + C1 + C2 + CT1 + CT2;
HIVinf1=[0:1:TIME];
HIVinf2=[beta.*(S+T).*(C1+C2)./N];
HIVinf=trapz(HIVinf1,HIVinf2);
AA(m) = HIVinf;
end
plot(100.*eta_22,AA./1000)
_____________________________________________________________________________________________________
function ydot = SimplifiedEqns(t,y)
global Lambda mu mu_A mu_T beta tau eta_1 eta_2 lambda_T rho_1 rho_2 gamma
S = y(1);
T = y(2);
H = y(3);
C = y(4);
C1 = y(5);
C2 = y(6);
CT1 = y(7);
CT2 = y(8);
N = S + T + H + C + C1 + C2 + CT1 + CT2;
ydot = zeros(8,1);
ydot(1)=Lambda-mu.*S-beta.*(H+C+C1+C2).*(S./N)-tau.*(T+C).*(S./N);
ydot(2)=tau.*(T+C).*(S./N)-beta.*(H+C+C1+C2).*(T./N)-(mu+mu_T).*T;
ydot(3)=beta.*(H+C+C1+C2).*(S./N)-tau.*(T+C).*(H./N)-(mu+mu_A).*H;
ydot(4)=beta.*(H+C+C1+C2).*(T./N)+tau.*(T+C).*(H./N)-(mu+mu_A+mu_T+lambda_T).*C;
ydot(5)=lambda_T.*C-(mu+mu_A+rho_1+eta_1).*C1;
ydot(6)=rho_1.*C1-(mu+mu_A+rho_2+eta_2).*C2;
ydot(7)=eta_1.*C1-(mu+rho_1+gamma).*CT1;
ydot(8)=eta_2.*C2-(mu+rho_2+gamma.*(rho_1)./(rho_1+rho_2)).*CT2+(rho_1).*CT1;
end
The simplest way:
eta_1=0:1/alpha:1;
eta_2=0:1/alpha:1;
lsize=length(eta_1) % I assume eta_1 and eta_2 are of the same size
for i=1:lsize
%Update your AA(i) here
end
plot(eta_1,AA,eta_2,AA)