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 at 15:33
  • 1
    @alephzero - OP just copied the paper in the post. Since the paper is already linked, I removed it. – Chuck Aug 6 at 15:37

That is quite the wall of text. You don't need to rewrite an entire academic paper to ask a question (please don't). You failed to show what it is you thought you were supposed to get or what you actually got.

That said, I think your quaternion to rotation matrix equation is not correct. It looks like maybe it only works for unit quaternions. The quaternion errors you're passing in are not.

Your Answer

By clicking "Post Your Answer", you acknowledge that you have read our updated terms of service, privacy policy and cookie policy, and that your continued use of the website is subject to these policies.

Not the answer you're looking for? Browse other questions tagged or ask your own question.