clear all; close all; clc; %% ------Define the performance parameters and simulation parameters------ % ***** dimensions about the system ***** dim_x=2; dim_y=1; dim_f=1; N=4; Theta = 1 * [1 1 0 1; 0 1 1 0; 1 0 1 0; 0 1 0 1]; dim_omega=1; dim_nu=1; N_S=2; % number of sensor nodes % ***** paremers about performance and simulation ***** N_times=40; % time horizon a=1/4; b=1.5; %%%%% the initial values and P(.) P_ini=blkdiag(16,9,1); % P_ini_f=blkdiag(1); % P_ini=blkdiag(P_ini_x,P_ini_f); %%%%% the initial values of \bar{x}(k) %%%%% x_bar_initial=[4;3;1]; x_bar_k=x_bar_initial; %%%%% the initial values of \hat{x}(k) %%%%% x_hat_initial=zeros(3,1); x1_hat_k=x_hat_initial; x2_hat_k=x_hat_initial; x3_hat_k=x_hat_initial; x4_hat_k=x_hat_initial; x_hat_k=[x1_hat_k;x2_hat_k;x3_hat_k;x4_hat_k]; varepsilon=0.1;%quantization level bar_varepsilon=varepsilon^2*dim_y/4; k_index=1; P{0+k_index}=P_ini; for k=0:1:N_times %% %%%%solve the optimization problem tic; %%%%% obtain the system parameters %%%%%% % A_k=[0.5+0.3*cos(0.6*k) 0.02+0.01*cos(0.2*k) 0; % 0.03 0.6+0.02*sin(0.2*k) 1; % 0 0 1]; % A_k=[0.3+0.05*cos(0.6*k) 0.02+0.01*cos(0.2*k); 0.03 0.4+0.02*sin(0.2*k)]; B_k=[0; 1]; C_k=[0.3+0.05*sin(0.3*k); 0.1]; D1_k=[0.2+0.1*cos(k) -0.30]; E1_k=[-0.02]; D2_k=[0.3 0.20+0.1*cos(k)]; E2_k=[0.03]; D3_k=[0.2 0.2+0.05*sin(k)]; E3_k=[0.01+0.02*cos(k)]; D4_k=[0.15+0.1*sin(k) 0.15]; E4_k=[-0.03]; % D_k=D3_k; % E_k=E3_k; % F_k=1; if k>0 & k<=9 F_k=1; elseif k==10 F_k=0.5; elseif k>11 & k<=19 F_k=1; elseif k==20 F_k=-2; else F_k=1; end %%%%%%%%% construct the big matrix for the augment system %%%%%% A_bar_k=[A_k B_k; zeros(dim_f,dim_x) F_k]; C_bar_k=[C_k; zeros(dim_f,dim_f)]; D1_bar_k=[D1_k zeros(dim_f,dim_f)]; D2_bar_k=[D2_k zeros(dim_f,dim_f)]; D3_bar_k=[D3_k zeros(dim_f,dim_f)]; D4_bar_k=[D4_k zeros(dim_f,dim_f)]; S_k=0.09; T_k=0.04; %%%%%%%%%%%%%%%%%%%Sensor Network Parameter%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% nf=dim_x+dim_f; r=nf; p=dim_y; IN = eye(N); InfN = eye(nf*N); IpN = eye(p*N); % Theta1 = blkdiag(Theta(1,1), Theta(1,2), Theta(1,3)); % Theta2 = blkdiag(Theta(2,1), Theta(2,2), Theta(2,3)); % Theta3 = blkdiag(Theta(3,1), Theta(3,2), Theta(3,3)); D_mathcal_k = blkdiag(D1_bar_k,D2_bar_k,D3_bar_k,D4_bar_k); E_mathcal_k = blkdiag(E1_k,E2_k,E3_k,E4_k); Upsilon_nf1 = blkdiag(eye(nf),zeros(nf,nf),zeros(nf,nf),zeros(nf,nf)); Upsilon_nf2 = blkdiag(zeros(nf,nf),eye(nf),zeros(nf,nf),zeros(nf,nf)); Upsilon_nf3 = blkdiag(zeros(nf,nf),zeros(nf,nf),eye(nf),zeros(nf,nf)); Upsilon_nf4 = blkdiag(zeros(nf,nf),zeros(nf,nf),zeros(nf,nf),eye(nf)); Upsilon_r1 = blkdiag(eye(r),zeros(r,r),zeros(r,r),zeros(r,r)); Upsilon_r2 = blkdiag(zeros(r,r),eye(r),zeros(r,r),zeros(r,r)); Upsilon_r3 = blkdiag(zeros(r,r),zeros(r,r),eye(r),zeros(r,r)); Upsilon_r4 = blkdiag(zeros(r,r),zeros(r,r),zeros(r,r),eye(r)); Upsilon_p1 = blkdiag(eye(p),zeros(p,p),zeros(p,p),zeros(p,p)); Upsilon_p2 = blkdiag(zeros(p,p),eye(p),zeros(p,p),zeros(p,p)); Upsilon_p3 = blkdiag(zeros(p,p),zeros(p,p),eye(p),zeros(p,p)); Upsilon_p4 = blkdiag(zeros(p,p),zeros(p,p),zeros(p,p),eye(p)); H_nf1 = kron(ones(N,1)',eye(nf))*Upsilon_nf1; H_nf2 = kron(ones(N,1)',eye(nf))*Upsilon_nf2; H_nf3 = kron(ones(N,1)',eye(nf))*Upsilon_nf3; H_nf4 = kron(ones(N,1)',eye(nf))*Upsilon_nf4; H_r1 = kron(ones(N,1)',eye(r))*Upsilon_r1; H_r2 = kron(ones(N,1)',eye(r))*Upsilon_r2; H_r3 = kron(ones(N,1)',eye(r))*Upsilon_r3; H_r4 = kron(ones(N,1)',eye(r))*Upsilon_r4; H_p1 = kron(ones(N,1)',eye(p))*Upsilon_p1; H_p2 = kron(ones(N,1)',eye(p))*Upsilon_p2; H_p3 = kron(ones(N,1)',eye(p))*Upsilon_p3; H_p4 = kron(ones(N,1)',eye(p))*Upsilon_p4; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% R_k=chol(P{k+k_index})'; %%%%%%%%% scalars variables %%%%%%%%% K_var=sdpvar((dim_x+dim_f)*N,dim_y*N,'full'); eps_11=sdpvar(1,1,'symmetric'); eps_12=sdpvar(1,1,'symmetric'); eps_13=sdpvar(1,1,'symmetric'); eps_14=sdpvar(1,1,'symmetric'); eps_21=sdpvar(1,1,'symmetric'); eps_22=sdpvar(1,1,'symmetric'); eps_23=sdpvar(1,1,'symmetric'); eps_24=sdpvar(1,1,'symmetric'); eps_3=sdpvar(1,1,'symmetric'); eps_4=sdpvar(1,1,'symmetric'); kappa=sdpvar(1,1,'symmetric'); P_var=sdpvar(dim_x+dim_f,dim_x+dim_f,'symmetric'); % P_var_f=sdpvar(dim_f,dim_f,'symmetric'); % P_var=blkdiag(P_var_x,P_var_f); eps_bar=1-(eps_11+eps_12+eps_13+eps_14)-(eps_21+eps_22+eps_23+eps_24)-eps_3-eps_4; Lambda_k=blkdiag(eps_bar,(eps_11*H_r1'*H_r1+eps_12*H_r2'*H_r2+eps_13*H_r3'*H_r3+eps_14*H_r4'*H_r4),inv(bar_varepsilon)*(eps_21*H_p1'*H_p1+eps_22*H_p2'*H_p2+eps_23*H_p3'*H_p3+eps_24*H_p4'*H_p4),eps_3*inv(S_k)*eye(dim_y),eps_4*inv(T_k)*eye(dim_y)); Xi_1k = kron(IN,A_bar_k*R_k)-K_var*D_mathcal_k*kron(IN,R_k); Xi_2k = -K_var*E_mathcal_k*kron(ones(N,1),eye(1)); Xi_k=[zeros(nf*N,1) Xi_1k -K_var kron(ones(N,1),C_bar_k) Xi_2k]; % Xi_k=[zeros(dim_x+dim_f,1) (A_bar_k-K_var*D_bar_k)*R_k -K_var C_bar_k -K_var*E_k]; MA=[0.1;0.2]; NA=[0.2 0.1]; MF=0.05; NF=0.02; M_bar=blkdiag(MA,MF); N_bar=blkdiag(NA,NF); M_mathcal=kron(IN,M_bar); N_mathcal=kron(IN,N_bar); R_mathcal=kron(IN,R_k); N_mathcal_bar=N_mathcal*[x_hat_k R_mathcal zeros(nf*N,N) zeros(nf*N,1) zeros(nf*N,1)]; LMI1=[-Lambda_k Xi_k'*H_nf1' zeros(19,8) N_mathcal_bar'; H_nf1*Xi_k -P_var kappa*H_nf1*M_mathcal zeros(3,8); zeros(8,19) kappa*M_mathcal'*H_nf1' -kappa*eye(8,8) zeros(8,8); N_mathcal_bar zeros(8,3) zeros(8,8) -kappa*eye(8,8)]; LMI2=[-Lambda_k Xi_k'*H_nf2' zeros(19,8) N_mathcal_bar'; H_nf2*Xi_k -P_var kappa*H_nf2*M_mathcal zeros(3,8); zeros(8,19) kappa*M_mathcal'*H_nf2' -kappa*eye(8,8) zeros(8,8); N_mathcal_bar zeros(8,3) zeros(8,8) -kappa*eye(8,8)]; LMI3=[-Lambda_k Xi_k'*H_nf3' zeros(19,8) N_mathcal_bar'; H_nf3*Xi_k -P_var kappa*H_nf3*M_mathcal zeros(3,8); zeros(8,19) kappa*M_mathcal'*H_nf3' -kappa*eye(8,8) zeros(8,8); N_mathcal_bar zeros(8,3) zeros(8,8) -kappa*eye(8,8)]; LMI4=[-Lambda_k Xi_k'*H_nf4' zeros(19,8) N_mathcal_bar'; H_nf4*Xi_k -P_var kappa*H_nf4*M_mathcal zeros(3,8); zeros(8,19) kappa*M_mathcal'*H_nf4' -kappa*eye(8,8) zeros(8,8); N_mathcal_bar zeros(8,3) zeros(8,8) -kappa*eye(8,8)]; cons = [LMI1<=0,LMI2<=0,LMI3<=0,LMI4<=0, P_var>0, eps_11>0, eps_12>0, eps_13>0, eps_14>0, eps_21>0, eps_22>0, eps_23>0, eps_24>0,eps_3>0, eps_4>0,kappa>0]; solvesdp(cons, trace(P_var)); % solvesdp(cons); K_value=double(K_var); K_array{k+k_index}=K_value; P{k+1+k_index}=double(P_var); time_require(k+k_index)=toc; %% %%%%%%%%%------------ update the system states and estimator states x_value(:,k_index+k)=x_bar_k; x_bar_k=(A_bar_k+M_bar*blkdiag(sin(k),cos(k))*N_bar)*x_bar_k+C_bar_k*0.3*cos(k); y1_bar_k=varepsilon*round(D1_bar_k*x_bar_k/varepsilon)+E1_k*0.2*sin(k); y2_bar_k=varepsilon*round(D2_bar_k*x_bar_k/varepsilon)+E2_k*0.2*sin(k); y3_bar_k=varepsilon*round(D3_bar_k*x_bar_k/varepsilon)+E3_k*0.2*sin(k); y4_bar_k=varepsilon*round(D4_bar_k*x_bar_k/varepsilon)+E4_k*0.2*sin(k); r1 = y1_bar_k - D1_bar_k*x1_hat_k; r2 = y2_bar_k - D2_bar_k*x2_hat_k; r3 = y3_bar_k - D3_bar_k*x3_hat_k; r4 = y4_bar_k - D4_bar_k*x4_hat_k; x1_hat_value(:,k_index+k)=x1_hat_k; x2_hat_value(:,k_index+k)=x2_hat_k; x3_hat_value(:,k_index+k)=x3_hat_k; x4_hat_value(:,k_index+k)=x4_hat_k; x_hat_k=[x1_hat_k;x2_hat_k;x3_hat_k;x4_hat_k]; x1_hat_k=A_bar_k*x1_hat_k+Theta(1,1)*[K_value(1,1);K_value(2,1);K_value(3,1)]*r1+Theta(1,2)*[K_value(1,2);K_value(2,2);K_value(3,2)]*r2+Theta(1,3)*[K_value(1,3);K_value(2,3);K_value(3,3)]*r3+Theta(1,4)*[K_value(1,4);K_value(2,4);K_value(3,4)]*r4; x2_hat_k=A_bar_k*x2_hat_k+Theta(2,1)*[K_value(4,1);K_value(5,1);K_value(6,1)]*r1+Theta(2,2)*[K_value(4,2);K_value(5,2);K_value(6,2)]*r2+Theta(2,3)*[K_value(4,3);K_value(5,3);K_value(6,3)]*r3+Theta(2,4)*[K_value(4,4);K_value(5,4);K_value(6,4)]*r4; x3_hat_k=A_bar_k*x3_hat_k+Theta(3,1)*[K_value(7,1);K_value(8,1);K_value(9,1)]*r1+Theta(3,2)*[K_value(7,2);K_value(8,2);K_value(9,2)]*r2+Theta(3,3)*[K_value(7,3);K_value(8,3);K_value(9,3)]*r3+Theta(3,4)*[K_value(7,4);K_value(8,4);K_value(9,4)]*r4; x4_hat_k=A_bar_k*x4_hat_k+Theta(4,1)*[K_value(10,1);K_value(11,1);K_value(12,1)]*r1+Theta(4,2)*[K_value(10,2);K_value(11,2);K_value(12,2)]*r2+Theta(4,3)*[K_value(10,3);K_value(11,3);K_value(12,3)]*r3+Theta(4,4)*[K_value(10,4);K_value(11,4);K_value(12,4)]*r4; k end it=1:N_times; % figure(1); set(gca, 'Fontname', 'Times newman'); plot(it,x_value(1,1:N_times),'-r','linewidth',2); hold on; plot(it,x1_hat_value(1,1:N_times),it,x2_hat_value(1,1:N_times),it,x3_hat_value(1,1:N_times),it,a*x4_hat_value(3,1:N_times)); xlabel('Time (k)','FontSize',12); ylabel('State trajectories of x^{(1)}(k) and its estimates','FontSize',12); h=legend('$x^{(1)}(k)$',' $\hat{\bar{x}}^{(1)}_1(k)$',' $\hat{\bar{x}}^{(1)}_2(k)$',' $\hat{\bar{x}}^{(1)}_3(k)$',' $\hat{\bar{x}}^{(1)}_4(k)$'); set(h,'Interpreter','latex'); hold on; figure(2); set(gca, 'Fontname', 'Times newman'); plot(it,x_value(2,1:N_times),'-r','linewidth',2); hold on; plot(it,x1_hat_value(2,1:N_times),it,x2_hat_value(2,1:N_times),it,x3_hat_value(2,1:N_times),it,b*x4_hat_value(3,1:N_times)); xlabel('Time (k)','FontSize',12); ylabel('State trajectories of x^{(2)}(k) and its estimates','FontSize',12); h=legend('$x^{(2)}(k)$',' $\hat{\bar{x}}^{(2)}_1(k)$',' $\hat{\bar{x}}^{(2)}_2(k)$',' $\hat{\bar{x}}^{(2)}_3(k)$',' $\hat{\bar{x}}^{(2)}_4(k)$'); set(h,'Interpreter','latex'); hold on; figure(3); set(gca, 'Fontname', 'Times newman'); plot(it,x_value(3,1:N_times),'-r','linewidth',2); hold on; plot(it,x1_hat_value(3,1:N_times),it,x2_hat_value(3,1:N_times),it,x3_hat_value(3,1:N_times),it,x4_hat_value(3,1:N_times)); xlabel('Time (k)','FontSize',12); ylabel('State trajectories of f(k) and its estimates','FontSize',12); h=legend('$f(k)$',' $\hat{\bar{x}}^{(3)}_1(k)$',' $\hat{\bar{x}}^{(3)}_2(k)$',' $\hat{\bar{x}}^{(3)}_3(k)$',' $\hat{\bar{x}}^{(3)}_4(k)$'); set(h,'Interpreter','latex'); hold on; % figure(3); % set(gca, 'Fontname', 'Times newman'); % for i=1:N_times % error_wtod(i)=(x_value(1:2,i)-x_hat_value(1:2,i))'*(x_value(1:2,i)-x_hat_value(1:2,i)); % end % plot(it,error_wtod,'linewidth',2); % h=xlabel('Time ($k$)','FontSize',14); % set(h,'Interpreter','latex'); % h=ylabel('Trajectories of the estimation error','FontSize',14); % set(h,'Interpreter','latex'); % hold on;