clc;clear all;close all;

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% lire les donn�es :
load data_sec.mat
load vrai_data.mat
load data_var.mat
S=50; % la langueur de l'axe spatial
N=100; % la langueur de l'axe spectral
Np=60; % nombre de points d'observation
R=0.01; %variance de bruit
X(:,1) = [2;35;3;23];
X1(:,1) = [2.5;55;2;17];
% Spectral power density of the white
noise.

dt = 0.4; % Number of samples and stepsize.

ra =4.63;
rc = 62.4;
rw =1.3;
rmu= 0.09;
Qc1 =blkdiag(ra, rc, rw,rmu); % Spectral power density of the
white noise.

F =zeros(4); % Dynamic state transition matrix in

continous-time domain.??

L=eye(4); % Noise effect matrix in continous-time

domain.??

[A,Q] = lti_disc(F,L,Qc1,dt); % Discretize the plant equation.

[A1,Q1] = lti_disc(F,L,Qc1,dt);
h_func = @ekf_exp_h; % la fnction non lin�aire
dh_dx_func = @ekf_exp_dh_dx; %la d�riv�e de la fonction non lin�aire
dh_dx_func1 = @ekf_exp_dh_dx1;

[y_sim X_sim] = simdata_kalman(X,S,N,A,h_func);

y_sim=y_sim;
K = 2;
I = 1e4;
for ji=1
[est1,tps1,x] = MAP_Enligne(y_sim,K,I,ji);
X1=x;
end
n=1:N;
nn = repmat(n(:)',S,1);
Y = y_sim + repmat((0:S-1)',1,N) + 1;
datax.lbd = 1:N;
datax.tau = 1:S;
datax.y = y_sim;
K = size(est1.c,2);
px1 = est1;
px1.b = ones(1,K);
px1.e = S*ones(1,K);
showsample(showsample,'Best estimation for Gibbs',100,datax,px1)

%% %%%%%%%%%%%%%%%%%%% kalman multi-SENSORS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Point = floor(X_sim(2,:));
bG=7; bD=6;
% Initial guesses for the state mean and covariance.
M_k = X_sim(:,1);
P_k =zeros(4);

figure,

tEMP= tic;
for k=1:S

intV= Point(k)-bG:Point(k)+bD;
intrV = linspace(intV(1),intV(end),Np);
Obs = linspace(Y(1,intV(1)),Y(1,intV(end)),Np);
for ii=1:3
[M_k,P_k] = ekf_predict1(M_k,P_k,A1,Q1);
for jj=1:length(Obs)
[M,P] = ekf_update1(M_k,P_k,Obs(jj),dh_dx_func,R*eye(1),h_func,
[],intrV(jj));
MM(:,jj) = M;
PP(:,:,jj) = P;
end
end
for lk=1:size(MM,1)
M_k(lk) = (mean(MM(lk,:)));%*Obs'))/(mean(Obs));
P_k(lk,lk) = mean(PP(lk,lk,:))./(size(MM,2));
end
MOY(:,k)=M_k;
Y_m(k,:) = feval(h_func, M_k,intV(:));

hold on
plot(nn',Y','k');
plot(intV,Y_m(k,:)+k,'LineWidth',2);
hold off
title('la trajectoire estim�e');
axis([0 N 0 S+5]);

end

%%%figure
scsz = get(0,'ScreenSize');
fig.h = figure(...
'Numbertitle','off',...
'Position',[scsz(3)*.2 scsz(4)*.2 scsz(3)*.6 scsz(4)*.6],...
'Units','normalized');

fig.mu = axes('Position',[.04 .09 .40 .4]);

fig.c = axes('Position',[.52 .59 .4 .4]);
fig.a = axes('Position',[.52 .09 .4 .4]);
fig.w = axes('Position',[.04 .59 .40 .4]);

set(fig.h,'CurrentAxes',fig.c);
hold on
plot(X_sim(2,:),1:S,'.-k','LineWidth',2)
plot(MOY(2,:),1:S,'.-b','LineWidth',2)
axis([1 N 0 S]);
hold off
xlabel('Centre');

set(fig.h,'CurrentAxes',fig.a);
hold on
plot(X_sim(1,:),1:S,'.-k','LineWidth',2)
plot(MOY(1,:),1:S,'.-b','LineWidth',2)
hold off
xlabel('Amplitude');
axis([0 5 0 S]);

set(fig.h,'CurrentAxes',fig.mu);
hold on
plot(X_sim(4,:),1:S,'.-k','LineWidth',2)
plot(MOY(4,:),1:S,'.-b','LineWidth',2)
xlabel('fr�quence');
hold off
axis([0 25 0 S]);

set(fig.h,'CurrentAxes',fig.w);
hold on
plot(X_sim(3,:),1:S,'.-k','LineWidth',2)
plot(MOY(3,:),1:S,'.-b','LineWidth',2)
xlabel('Variance');
hold off
axis([0 4 0 S]);
drawnow;
tEMP1= toc
EMM_YA = sum((X_sim(1,:)-MOY(1,:)).^2)/length(MOY(1,:));
EMM_YC = sum((X_sim(2,:)-MOY(2,:)).^2)/length(MOY(1,:));
EMM_YV = sum((X_sim(3,:)-MOY(3,:)).^2)/length(MOY(1,:));
EMM_YMU = sum((X_sim(4,:)-MOY(4,:)).^2)/length(MOY(1,:));
disp('Mean square errors of all estimates:');

fprintf('EKF1_MSEA = %.4f\n',EMM_YA);
fprintf('EKF1_MSEC = %.4f\n',EMM_YC);
fprintf('EKF1_MSEV = %.4f\n',EMM_YV);
fprintf('EKF1_MSEMU = %.4f\n',EMM_YMU);