clc
close all
clear all
x1(1)=2;
x2(1)=2;
x3(1)=2;
x4(1)=2;
x = [x1(1); x2(1); x3(1); x4(1)];
xd = [0; 0; 0; 0]; %%reference of tracking but here no tracking
%
Q = 0.4*eye(4);
R = eye(1);
A = [0 1 0 0
35.93 0 0 0
0 0 0 1
-0.98 0 0 0];
B = [0; -0.333; 0; 0.1];
ts = 1;
dt = 0.01;
tf = 50;
i = 0;
for time = ts:dt:tf
i=i+1;
if i ==1
u = [0; 0; 0; 0];
NN_W= [1 1 1 1 1 1 1 1 1 1 ]' *0.1 ;
end
del_theta=[ 2*x1(:,i) 0 0 0 ;
0 2*x2(:,i) 0 0;
0 0 2*x3(:,i) 0;
0 0 0 2*x4(:,i);
x2(:,i) x1(:,i) 0 0;
x3(:,i) 0 x1(:,i) 0;
x4(:,i) 0 0 x1(:,i);
0 x3(:,i) x2(:,i) 0;
0 x4(:,i) 0 x2(:,i);
0 0 x4(:,i) x3(:,i)];
% % u_temp(i,:) = u;
% e = x-xd;
%
% % [K,L,G] = care(A,B,Q);
val_e = x'*Q*x + (-1/4)*NN_W'*del_theta*B*inv(R)*B'*del_theta'*NN_W -(dt^2/(3.6)^2)*NN_W'*del_theta*A*B*B'*A*del_theta'*NN_W + NN_W'*del_theta*A*x;
%
del_e = del_theta*A*x -((dt^2)/(1.8)^2)*NN_W'*del_theta*A*B*B'*A*del_theta'*NN_W - 2*del_theta*B*inv(R)*B'*del_theta'*NN_W;
NN_W=NN_W - ((del_e)*val_e)/(del_e'*del_e+1); %%update
landa=del_theta'*NN_W; %% 4 X 1
% e_temp(n,:) = e;
u=-0.5*inv(R)*B'*landa;
% u = inv(R)*B'*landa*(x-xd);
e=x-xd;
K1_a = dt*(A(1,1)*e(1)+A(1,2)*e(2)+A(1,3)*e(3)+A(1,4)*e(4)+B(1)*u);
K2_a = dt*(A(2,1)*e(1)+A(2,2)*e(2)+A(2,3)*e(3)+A(2,4)*e(4)+B(2)*u);
K3_a = dt*(A(3,1)*e(1)+A(3,2)*e(2)+A(3,3)*e(3)+A(3,4)*e(4)+B(3)*u);
K4_a = dt*(A(4,1)*e(1)+A(4,2)*e(2)+A(4,3)*e(3)+A(4,4)*e(4)+B(4)*u);
K1_b = dt*(A(1,1)*(e(1)+0.5*K1_a)+A(1,2)*(e(2)+0.5*K2_a)+A(1,3)*(e(3)+0.5*K3_a)+A(1,4)*(e(4)+0.5*K4_a)+B(1)*u);
K2_b = dt*(A(2,1)*(e(1)+0.5*K1_a)+A(2,2)*(e(2)+0.5*K2_a)+A(2,3)*(e(3)+0.5*K3_a)+A(2,4)*(e(4)+0.5*K4_a)+B(2)*u);
K3_b = dt*(A(3,1)*(e(1)+0.5*K1_a)+A(3,2)*(e(2)+0.5*K2_a)+A(3,3)*(e(3)+0.5*K3_a)+A(3,4)*(e(4)+0.5*K4_a)+B(3)*u);
K4_b = dt*(A(4,1)*(e(1)+0.5*K1_a)+A(4,2)*(e(2)+0.5*K2_a)+A(4,3)*(e(3)+0.5*K3_a)+A(4,4)*(e(4)+0.5*K4_a)+B(4)*u);
K1_c = dt*(A(1,1)*(e(1)+0.5*K1_b)+A(1,2)*(e(2)+0.5*K2_b)+A(1,3)*(e(3)+0.5*K3_b)+A(1,4)*(e(4)+0.5*K4_b)+B(1)*u);
K2_c = dt*(A(2,1)*(e(1)+0.5*K1_b)+A(2,2)*(e(2)+0.5*K2_b)+A(2,3)*(e(3)+0.5*K3_b)+A(2,4)*(e(4)+0.5*K4_b)+B(2)*u);
K3_c = dt*(A(3,1)*(e(1)+0.5*K1_b)+A(3,2)*(e(2)+0.5*K2_b)+A(3,3)*(e(3)+0.5*K3_b)+A(3,4)*(e(4)+0.5*K4_b)+B(3)*u);
K4_c = dt*(A(4,1)*(e(1)+0.5*K1_b)+A(4,2)*(e(2)+0.5*K2_b)+A(4,3)*(e(3)+0.5*K3_b)+A(4,4)*(e(4)+0.5*K4_b)+B(4)*u);
K1_d = dt*(A(1,1)*(e(1)+K1_c)+A(1,2)*(e(2)+K2_c)+A(1,3)*(e(3)+K3_c)+A(1,4)*(e(4)+K4_c)+B(1)*u);
K2_d = dt*(A(2,1)*(e(1)+K1_c)+A(2,2)*(e(2)+K2_c)+A(2,3)*(e(3)+K3_c)+A(2,4)*(e(4)+K4_c)+B(2)*u);
K3_d = dt*(A(3,1)*(e(1)+K1_c)+A(3,2)*(e(2)+K2_c)+A(3,3)*(e(3)+K3_c)+A(3,4)*(e(4)+K4_c)+B(3)*u);
K4_d = dt*(A(4,1)*(e(1)+K1_c)+A(4,2)*(e(2)+K2_c)+A(4,3)*(e(3)+K3_c)+A(4,4)*(e(4)+K4_c)+B(4)*u);
x1(i+1) = x1(i)+(1/6)*(K1_a+2*K1_b+2*K1_c+K1_d);
x2(i+1) = x2(i)+(1/6)*(K2_a+2*K2_b+2*K2_c+K2_d);
x3(i+1) = x3(i)+(1/6)*(K3_a+2*K3_b+2*K3_c+K3_d);
x4(i+1) = x4(i)+(1/6)*(K4_a+2*K4_b+2*K4_c+K4_d);
x = [x1(i+1); x2(i+1); x3(i+1); x4(i+1)];
x_temp(i,:) = x;
end
tt = ts:dt:tf;
figure(1)
plot(tt,x_temp(:,1),'r'); hold on
plot(tt,x_temp(:,2),'b--'); hold on
plot(tt,x_temp(:,3),'black'); hold on
plot(tt,x_temp(:,4),'g'); hold on
