EKF.asv

%% EKF per localizzazione e tracking (Esempio 3 BS e 1 MS)

clear all;
close all;
clc;

%Parametri
T=1; %campionamento della posizione della MS, 1 sec

%Posizioni delle 3 BS - fisse
rx1=10;
ry1=10;
rx2=120;
ry2=40;
rx3=56;
ry3=22;

gamma=100; %parametro legato allo spettro della forma d'onda trasmessa
c=3*1e8; %velocità di propagazione
Pt=10; %potenza costante trasmessa dalla MS [mW]
R0=10; %distanza di riferimento [m]
SNR0=100; %SNR alla distanza di riferimento



Rbs=[rx1 ry1;rx2 ry2;rx3 ry3]; %posizioni delle BS
N=size(Rbs,1);

init=[5 5 2 2]; %inizializzazione stato MS

%vettore di stato the(n)=[rx(n) ry(n) vx(n) vy(n)]; contiene posizione 
%nel piano e velocità della MS

the_po(:,1)=init'; %inizializzazione della stima MMSE a 
the(:,1)=the_po(:,1);
%posteriori dello stato
P=size(the,1); %cardinalità spazio dei parametri


%inizializzazione della covarianza a posteriori
P_po(:,:,1)=eye(P);

%Equazione dinamica dello stato
siga=1;
a=[0 0 sqrt(siga)*randn sqrt(siga)*randn]'; %vettore delle accelerazioni

A=[]

the(:,2)=A*the(:,1)+a; %aggiornamento dello stato via equazione dinamica

%PREDIZIONE
the_pr(:,2)=A*the_po(:,1); %aggiornamento del valore medio a priori dello stato,
%cioè la predizione all'istante n date (n-1) osservazioni
P_pr(:,:,2)=A*P_po(:,:,1)*A'+Caa; %aggiornamento della covarianza a priori 
%cioè della predizione


%Legame stato-osservazioni
%Legame non lineare!
%le potenze del rumore dipendono dalla distanza BS-MS
sigmab(:,2)=(((c^2)*gamma)/SNR0)*((sqrt((repmat(N,1,the_pr(1,2))-Rbs(:,1)).^2+(repmat(N,1,the_pr(2,2))-Rbs(:,2)).^2))./(R0^2)); %covarianze di rumore
Cbb(:,:,2)=sigmab(:,2)*sigmab(:,2)'; %matrice di covarianza del disturbo, tempo variante
%tempo varianti
b(:,2)=sqrt(sigmab(:,2)).*randn(N,1);
R(:,2)=sqrt((the(1,2)-Rbs(:,1)).^2+(the(2,2)-Rbs(:,2)).^2)+b(:,2); %osservazioni

%AGGIORNAMENTO
%Linearizzazione della funzione non lineare
B(:,:,2)=[the_pr(1,2)/(sqrt((the_pr(1,2)-Rbs(1,1)).^2+(the_pr(2,2)-Rbs(1,2)).^2)) the_pr(2,2)/(sqrt((the_pr(1,2)-Rbs(1,1)).^2+(the_pr(2,2)-Rbs(1,2)).^2)) 0 0;...
    the_pr(1,2)/(sqrt((the_pr(1,2)-Rbs(2,1)).^2+(the_pr(2,2)-Rbs(2,2)).^2)) the_pr(2,2)/(sqrt((the_pr(1,2)-Rbs(2,1)).^2+(the_pr(2,2)-Rbs(2,2)).^2)) 0 0;...
    the_pr(1,2)/(sqrt((the_pr(1,2)-Rbs(3,1)).^2+(the_pr(2,2)-Rbs(3,2)).^2)) the_pr(2,2)/(sqrt((the_pr(1,2)-Rbs(3,1)).^2+(the_pr(2,2)-Rbs(3,2)).^2)) 0 0];

G(:,:,2)=(P_pr(:,:,2)*B(:,:,2)')/(B(:,:,2)*P_pr(:,:,2)*B(:,:,2)'+Cbb); %guadagno di kalman  tempo variante
the_po(:,2)=the_pr(:,2)+G(:,:,2)*(R(:,2)-sqrt((the_pr(1,2)-Rbs(:,1)).^2+(the_pr(2,2)-Rbs(:,2)).^2)); %aggiornamento stima MMSE (a posteriori)
%dello stato

%Equazione di Riccati
P_po(:,:,2)=P_pr(:,:,2)-G(:,:,2)*B(:,:,2)*P_pr(:,:,2); %aggiornamento covarianza della stima