# simulation of an attitude optimal Backstepping controller based quaternion for a UAV in MATLAB

I'am trying to implement this open access research article Attitude Optimal Backstepping Controller Based Quaternion for a UAV

I did not find the same result in this paper Can anyone tell me where my error is in this code

Case 1. Let the initial condition of the quaternion error be $\mathrm{Q}_{e0}= (0.8224, 0.2226, 0.4397, 0.3604)^{T}$, which corresponds to an angle $\phi(0) = 34.67^{\circ}$; the matrices $P_{1}$ and $P_{2}$ are computed through Riccati equation and $\gamma_{2}$ is chosen to achieve good performances. For that case $q_{0}(0) >0$ and will be stabilized to 1. Figures 1, 2, and 3 show the attitude responses and the control inputs, which gives a convergence in short time. These results are nearby the normal reflect when analyzing the initial location of $\phi$ which lies in the first quadrant.

function xdot = test(x)
R1=0.1*eye(3,3);
v=[0.0098,0.0098,0.0176];
J=diag(v);
R2=0.03*eye(3,3);
gamma= 150;
Q1=eye(3,3);
Q2=eye(3,3);
W_d=[0.56;0.61;0.42];
%--------------------------------------------------------------------------
R_T=[ x(1)^2+x(2)^2-x(3)^2-x(4)^2, 2*(x(2)*x(3)+x(1)*x(4)), 2*(x(2)*x(4)-x(1)*x(3))
2*(x(2)*x(3)-x(1)*x(4)), x(1)^2-x(2)^2+x(3)^2-x(4)^2, 2*(x(3)*x(4)+x(1)*x(2))
2*(x(2)*x(4)+x(1)*x(3)), 2*(x(3)*x(4)-x(1)*x(2)), x(1)^2-x(2)^2-x(3)^2+x(4)^2];
W_star=R_T*W_d;
q0=x(1);
qv=[x(2);x(3);x(4)];
W_aux=[x(5);x(6);x(7)];

B1=0.5*[0,x(4),-x(3);-x(4),0,x(2);x(3),-x(2),0]+q0*eye(3,3);
B2=inv(J);
A2=(-inv(J)*[0,x(7),-x(6);-x(7),0,x(5);x(6),-x(5),0]*J-inv(J)*[0,W_star(3),-W_star(2);-W_star(3),0,W_star(1);W_star(2),-W_star(1),0]*J);
G2=inv(J);
B22=inv(J);
z1=[x(8);x(9);x(10)];
z2=[x(11);x(12);x(13)];
p1=[x(14) x(15) x(16);x(17) x(18) x(19);x(20) x(21) x(22)];
p2=[x(23) x(24) x(25);x(26) x(27) x(28);x(29) x(30) x(31)];
W2=1/(2*gamma^2)*B22'*p2*z2;
Ksi2=-inv(R2)*B2'*p2*z2;

v1=-inv(R1)*inv(B1)*inv(R1)*p1*qv*sign(q0);
q0dot=-0.5*qv'*W_aux;
qvdot=B1*W_aux;
B1dot=0.5*[0,-qvdot(3),qvdot(2);qvdot(3),0,-qvdot(1);-qvdot(2),qvdot(1),0]+q0dot*eye(3,3);
p1dot=p1*B1*inv(R1)*B1'*p1-Q1;
v1dot=inv((B1^2*R1^2))*(B1*p1*qvdot + B1*p1dot*qv - p1*B1dot*qv)*sign(q0);
%v1dot=-inv(R1)*invB1dot*inv(R1)*p1*x1+(-inv(R1)*inv(B1)*inv(R1))*(p1dot*x1+p1*x1dot);
v2=inv(R2)*B2'*p2*z2+inv(B2)*v1dot*sign(q0)-inv(B2)*A2*v1*sign(q0)+inv(B2)*inv(p2)*B1'*p1*z1-inv(B2)*(1/2*gamma^2)*B22*(B22')*p2*z2;
W_auxdot=A2*W_aux+B2*v2+G2*W2;
z1dot=B1*z2-B1*inv(R1)*B1'*p1*z1;
z2dot=A2*z2+B2*Ksi2-inv(p2)*B1'*p1*z1;
p1dot=p1dot(:);
T2=1/(2*gamma^2)*G2*(G2')-B2*inv(R2)*B2';
p2dot=-A2'*p2-p2*A2-p2*T2*p2-Q2;
p2dot=p2dot(:);
xdot=[q0dot;qvdot;W_auxdot;z1dot;z2dot;p1dot;p2dot];
xdot=xdot(:);
end

execute function

% test for function deri
clc;clear ALL;close all;
[t,x]=ode45(@(t,x)test(x),[0 1],[0.8224 0.2226 0.4397 0.3604 0 0 0 1 0.5 3 2 1 1.2 0.1 0.2 0.3 0.4 0.5 0.1 0.2 0.3 0.1 0.01 0.02 0.3 0.4 0.2 0.03 0.02 0.001 0.4])
• Do you really expect anyone here to spend hours of their own time reading the paper and working through your very long post? Try contacting the authors of the paper - but you will need to give them more information than "my code doesn't work!" – alephzero Aug 6 '18 at 15:33
• @alephzero - OP just copied the paper in the post. Since the paper is already linked, I removed it. – Chuck Aug 6 '18 at 15:37